Quaternions & IMU Sensor Fusion with

Complementary Filtering!
Gordon Wetzstein!
Stanford University!
!

EE 267 Virtual Reality!

Lecture 10!
stanford.edu/class/ee267/!
!

April 27, 2016 !

 Updates!
! project proposals due: May 6, 2016 (next Friday)!
! 1 page!
! at least 3 scientific references (no websites)!
! brief intro & motivation!
! milestones and timeline!
! clearly list expected outcomes of project!

! project proposal pitch: May 9 (the Mon after)!

! 1 slide per team presented in 1 minute!!
 Polynesian Migration!

wikipedia!
 Lecture Overview!

! short review of coordinate sytems!

! gyro-based head orientation tracking!
! tilt correction with complementary filtering!
! rotations: Euler angles, axis & angle, gimbal lock!
! rotations with quaternions!
! 9 DOF IMU sensor fusion with complementary filtering!
 ! primary goal: track head
orientation!

! inertial sensors required to

determine pitch, yaw, and
roll!

oculus.com!
 from lecture 2:!
vertex in clip space! vertex!

vclip = M proj ! M view ! M model ! v

oculus.com!
 from lecture 2:!
vertex in clip space! vertex!

vclip = M proj ! M view ! M model ! v

projection matrix
matrix! view matrix!
matrix model matrix
matrix!

oculus.com!
 from lecture 2:!
vertex in clip space! vertex!

vclip = M proj ! M view ! M model ! v

projection matrix
matrix! view matrix!
matrix model matrix
matrix!

rotation! translation!

T ( "eye )
M view = R !T

oculus.com!
 from lecture 2:!
vertex in clip space! vertex!

vclip = M proj ! M view ! M model ! v

projection matrix
matrix! view matrix!
matrix model matrix
matrix!

rotation! translation!

T ( "eye )
M view = R !T

R = Rz ( !" z ) # Rx ( !" x ) # Ry !" y ( )

roll! pitch! yaw!
oculus.com!
 2 important
coordinate systems: !
body/sensor inertial frame!
frame!

M view = R !T ( "eye )

( )
R = Rz ( !" z ) # Rx ( !" x ) # Ry !" y
roll! pitch! yaw!
oculus.com!
 Remember: Dead Reckoning with Gyro!

! integrate angular velocity from gyro measurements:!

! ( t + "t ) # ! ( t ) + ! ( t ) "t + O ( "t 2 )

! for each axis separately:! ! x = ! x + " x #t

! y = ! y + " y #t
! z = ! z + " z #t
 Remember: Dead Reckoning with Gyro!

! big challenge with gyro: drift over time!

! why drift? mostly integration error!

!
! possible solution (?): try using accelerometer data instead of
gyro!
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!
normalize gravity vector in
inertial coordinates!

! 0 $ ! 0 $
1
a! # &
( )
a! = R # 1 & = Rz ( '( z ) ) Rx ( '( x ) ) Ry '( y # 1 &
# &
" 0 % " 0 %

normalize gravity vector rotated into

sensor coordinates!
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!

! 0 $ ! 0 $
1
a! # &
( )
a! = R # 1 & = Rz ( '( z ) ) Rx ( '( x ) ) Ry '( y # 1 &
# &
" 0 % " 0 %
# cos ( !" ) ! sin ( !" ) 0 & # 1
%
z z
(%
0 0
(%
( )
& # cos !" y 0 ( )
sin !" y &
(# 0 &
= % sin ( !" ) cos ( !" ) 0 ( % 0 cos ( !" x ) ! sin ( !" x ) (% 0 1 0 (% 1 (
% ( %$ 0 ('
( ) ( )
z z
% (%
1 ' $ 0 sin ( !" x ) cos ( !" x )
( % ! sin !" 0 cos !" y ('
$ 0 0 '$ y
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!

# ! sin ( !" ) cos ( !" ) &

% (
z x
1
a = a! = % cos ( !" z ) cos ( !" x ) (
a! % (
%$ sin ( !" x ) ('
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!

roll!
# ! sin ( !" ) cos ( !" ) &
ax ! sin ( !" z )
% ( = ! tan ( !" z )
z x
1
a! = % cos ( !" z ) cos ( !" x )
=
a =
a!
( ay cos ( !" z )
% (
%$ sin ( !" x ) ('

( )
! z = "atan2 " ax , ay in rad !["# , # ]
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!

pitch!
# ! sin ( !" ) cos ( !" ) &
sin ( !" x )
% (
z x az
1 =
a = a! = % cos ( !" z ) cos ( !" x ) ( ax2 + ay2 (
cos 2 ( !" x ) sin 2 ( !" z ) + cos 2 ( !" z ) )
a! % (
%$ sin ( !" x ) (' =1
sin ( !" x )
= = tan ( !" x )
cos ( !" x )
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!

pitch!
# ! sin ( !" ) cos ( !" ) &
sin ( !" x )
% (
z x az
1 =
a = a! = % cos ( !" z ) cos ( !" x ) ( ax2 + ay2 (
cos 2 ( !" x ) sin 2 ( !" z ) + cos 2 ( !" z ) )
a! % (
%$ sin ( !" x ) (' =1

( $
% ) '
! x = "atan2 az , ax2 + ay2 in rad ! & " 2 , 2 )
(
# #
 Pitch and Roll from Accelerometer!

! use only accelerometer data can estimate pitch & roll, not yaw!
! assume no external forces (only gravity) acc is pointing UP!!

pitch!

az sin ( !" x )
=
ax2 + ay2 (
cos 2 ( !" x ) sin 2 ( !" z ) + cos 2 ( !" z ) )
=1

( ( ) )
! x = "atan2 az ,sign ay # ax2 + ay2 in rad ![ "# , # ]
 Tilt Correction!

! idea: stabilize gyro drift using noisy accelerometer = 6 DOF

sensor fusion !

! remember: !
! gyro is reliable in the short run, but drifts over time, mainly
due to accumulation of integration error!
! accelerometer is noisy and affected by motion, thus
reliable in the long run but not over short periods of time!
! with accelerometer, can only measure tilt not heading!
 Tilt Correction with Complementary Filtering!

! idea for sensor fusion: low-pass filter accelerometer and high-

pass filter gyro!

linear interpolation between gyro & accelerometer!!

pitch:! ( ( ( )
! x = " (! x + # x $t ) + (1% " ) rad2deg %atan2 az ,sign ay & ax2 + ay2 ))
yaw:! ! y = ! y + " y #t
roll:! ( (
! z = " (! z + # z $t ) + (1% " ) rad2deg %atan2 % ax , ay )) 0 !" !1
 Tilt Correction with Complementary Filtering!

! low-pass filter accelerometer and high-pass filter gyro!

! very popular among hobbyists super easy to implement!!
! filter used in Oculus DK 1, see LaValle et al. ICRA 2014!

pitch:! ( ( ( )
! x = " (! x + # x $t ) + (1% " ) rad2deg %atan2 az ,sign ay & ax2 + ay2 ))
yaw:! ! y = ! y + " y #t
roll:! ( (
! z = " (! z + # z $t ) + (1% " ) rad2deg %atan2 % ax , ay )) 0 !" !1
 Example: Pitch!

gyro (dead reckoning)!

accelerometer!

complementary filter!
 Euler Angles and Gimbal Lock!

! so far we have represented head rotations with 3 angles!

! problematic when interpolating between keyframes (in

computer animation) or integration ! singularities!
 Gimbal Lock!

The Guerrilla CG Project, The Euler (gimbal lock) Explained see youtube.com!
Rotations with Axis and Angle Representation!

! solution to gimbal lock: use a different representation for head

rotation!

y!
! axis and angle: !

x!

z!
Quaternions!!
 Quaternions!

! whats a quaternion?!
! extension of complex numbers 3 imaginary numbers!

q = q0 + q1i + q2 j + q3k

!
i 2 = j 2 = k 2 = ijk = !1
i! j!k
 Quaternions!

! whats a quaternion?!
! singularity-free description of rotation!

" ! ! ! !%
q (! ,v ) = $ cos , vx sin , vy sin , vz sin '
# 2 2 2 2&
 Quaternions!

! 4 values for 3 degrees of freedom (DOF)? !

! all quaternions representing a rotation are normalized! ! 3DOF!
! may have to normalize in practice due to roundoff!

" ! ! ! !%
q (! ,v ) = $ cos , vx sin , vy sin , vz sin '
# 2 2 2 2&

q = q02 + q12 + q22 + q32 = 1

 Quaternion from Axis & Angle!

angle in unit-length
radians! axis!

" ! ! ! !%
q (! ,v ) = $ cos , vx sin , vy sin , vz sin '
# 2 2 2 2&

scalar = vector = axis!

angle!
 Quaternion to Axis & Angle!

::rotate(! , vx, vy, vz)!

! convert quaternion to rotation, then use glm::rotate(

! = 2 " acos ( q0 ) in radians! in degrees!

s = 1! q02

q1 q2 q3
v x = , v y = , vz =
s s s
 Quaternion Algebra!

! unit quaternion:! q = q02 + q12 + q22 + q22 = 1

! vector quaternion (represents 3D point or vector), need not be

!
normalized:!
(
qv = 0,vx ,vy ,vz )
! addition:! q + p = ( q0 + p0 ) + ( q1 + p1 ) i + ( q2 + p2 ) j + ( q3 + p3 ) k
 Quaternion Algebra!

! conjugate:! q* = q0 ! q1i ! q2 j ! q3k

!

!1q*
! inverse:! q = 2
q
 Quaternion Algebra!
multiplication:! qp = ( q0 + q1i + q2 j + q3 k ) ( p0 + p1i + p2 j + p3 k )

= ( q0 p0 q1 p1 q2 p2 q3 p3 ) +
( q0 p1 + q1 p0 + q2 p3 q3 p2 ) i
( q0 p2 q1 p3 + q2 p0 + q3 p1 ) j
( q0 p3 + q1 p2 q2 p1 + q3 p0 ) k
 Quaternion Algebra!

! all quaternions representing rotations are unit quaternions (as

opposed to vector quaternions)!

! rotation of vector quaternion q p by q :! q' p = qq p q !1

! inverse rotation:! q' p = q !1q p q

! successive rotations: q2 q1 rotate first by q1 and then by! q2

 Quaternion-based!
Orientation Tracking!
 Quaternion-based Orientation Tracking!
(t )
! orientation representation: q is rotation from body/sensor
frame to inertial frame at time t!

! start with initial quaternion: ! q( 0 ) = (1,0,0,0 )

! integration how does the gyro data fit in here? !

! 6 DOF sensor fusion gyro + accelerometer (with quaternions)!

 Quaternion-based Orientation Tracking!

! how to get rotation quaternion from gyro measurements?!

measurements:! != !
! (
! x ,!
! y ,!
!z ) magnitude:! !
l= !
need this in radians / second!!

# 1 & # # !tl & "! x # !tl & "! y # !tl & "! z # !tl & &
q! = q % !tl, "! ( = % cos %
$ 2 (' l %$ ( %$ ( %$ (
, sin , sin , sin
$ l ' $ 2 ' l 2 ' l 2 ' ('

angle = rate of rotation (radians/sec)!

 Quaternion-based Orientation Tracking!

! make sure not to divide by 0 if l=0!!

measurements:! != !
! (
! x ,!
! y ,!
!z ) magnitude:! !
l= !
need this in radians / second!!

# 1 & # # !tl & "! x # !tl & "! y # !tl & "! z # !tl & &
q! = q % !tl, "! ( = % cos %
$ 2 (' l %$ ( %$ ( %$ (
, sin , sin , sin
$ l ' $ 2 ' l 2 ' l 2 ' ('

angle = rate of rotation (radians/sec)!

 Quaternion-based Orientation Tracking!

! how to integrate rotation quaternion with gyro measurements? !

(t+!t ) (t )
q = q q!

# 1 & # # !tl & "! x # !tl & "! y # !tl & "! z # !tl & &
q! = q % !tl, "! ( = % cos %
$ 2 (' l %$ ( %$ ( %$ (
, sin , sin , sin
$ l ' $ 2 ' l 2 ' l 2 ' ('

angle = rate of rotation (radians/sec)!

 Quaternion-based Orientation Tracking!

! how to integrate rotation quaternion with gyro measurements? !

(t+!t ) (t )
q = q q!

forward (process) model simple! !

! assumption: angular velocity is constant from t to t+1 !

 Tilt Correction with Quaternions!

! assume accelerometer measures gravity vector in body

(sensor) coordinates!

! transform measurements into inertial coordinates! qa(t )

qa ( inertial)
= q qa(t ) ( body ) (t )!1
q (
, qa = 0, a! x , a! y , a! z )
! qa( body ) ,qa( inertial) are vector quaternions !
 Tilt Correction with Quaternions!

! assume accelerometer measures only gravity vector in body

(sensor) coordinates!
! without gyro drift: in inertial coordinates, accelerometer vector
should point UP (0,1,0) with 9.81 m/s2!
! with gyro drift: doesnt point up!

! 6DOF sensor fusion to stabilize drift w.r.t. tilt (pitch and roll)!
 Tilt Correction with Quaternions!
!1
( inertial)
1.! compute accelerometer in inertial coords! qa = q(t )qa( body )q(t )
( inertial)
2.! get normalized vector part of vector quaternion! qa

! q( inertial) q( inertial) q( inertial) $

v = # ( inertial
a1 a2 a3
! )
, , &
#" qa qa( inertial) qa( inertial) &%

3.! compute angle between v and UP (0,1,0) with dot product!

" q( inertial) %
v i ( 0,1,0 ) = v ( 0,1,0 ) cos ! ( )
! = acos vy = acos $ ( inertial
a2

$# qa ) '
'&
 Tilt Correction with Quaternions!
4.! compute normalized rotation axis between v and UP with
cross product!
# vz vx &
vtilt = v ! ( 0,1,0 ) = % " 2 2 ,0, 2 2 (
$ v x + vz v x + vz '

5.! tilt correction quaternion is! qtilt (! ,vtilt )

i.e.! ( 0,0,1,0 ) = qtilt (!,vtilt )q (t ) ( body ) (t )"1

qa q "1
qtilt (! ,vtilt )
 Complementary Filter!

! same idea as for Euler angles: use gyro measurements and

correct tilt!

(t+!t )
qtilt _ corrected = q (( ) tilt ) gyro 0 ! " ! 1
1" # $ ,v q (t+!t )

(t+!t )
where qgyro = qtilt
(t )
_ corrected q!
is the updated rotation
quaternion from only the gyro measurements (see slide 42)!
 Complementary Filter!

! same idea as for Euler angles: use gyro measurements and

correct tilt a bit into tilt of accelerometer!

(t+!t )
qtilt _ corrected = q (( ) tilt ) gyro 0 ! " ! 1
1" # $ ,v q (t+!t )

! just remember: all rotation quaternions need to be normalized,

vector quaternions (i.e. q0=0) need not be !
 Head and Neck Model!

y !x !z
y
lh
ln IMU!

ln
!z x
pitch around base of neck!! roll around base of neck!!
 Head and Neck Model!

! why? there is not always positional tracking! this gives some

motion parallax!
! can extend to torso, and using other kinematic constraints!

! integrate into pipeline as!

M view = T ( 0,!ln ,!lh ) " R "T ( 0,ln ,lh ) "T ( !eye )

 Additional Information!

! S. LaValle, A. Yershova, M. Katsev, M. Antonov Head Tracking for the Oculus

Rift, Proc. ICRA 2014!

! E. Kraft A Quaternion-based Unscented Kalman Filter for Orientation Tracking,

IEEE Proc. Information Fusion, 2003!
! N. Trawny, I. Roumeliotis Indirect Kalman Filter for 3D Attitude Estimation: A
Tutorial for Quaternion Algebra, Technical Report 2005!

Note: I found that none of these papers alone are free of errors or follow the
coordinate system we use, so its best to use them as general guidelines but follow
our notation in class!!