Quaternions & IMU Sensor Fusion with Complementary Filtering!

Transcription

1 !! Quaternions & IMU Sensor Fusion with Complementary Filtering! Gordon Wetzstein! Stanford University! EE 267 Virtual Reality! Lecture 10! stanford.edu/class/ee267/! April 27, 2016!

2 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!!

3 wikipedia! Polynesian Migration!

4 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!

5 primary goal: track head orientation! inertial sensors required to determine pitch, yaw, and roll! oculus.com!

6 from lecture 2:! vertex in clip space! vertex! v clip = M proj! M view! M model!v oculus.com!

7 from lecture 2:! vertex in clip space! vertex! v clip = M proj! M view! M model!v projection matrix! view matrix! model matrix! oculus.com!

8 from lecture 2:! vertex in clip space! vertex! v clip = M proj! M view! M model!v projection matrix! view matrix! model matrix! rotation! translation! M view = R!T T "eye oculus.com!

9 from lecture 2:! vertex in clip space! vertex! v clip = M proj! M view! M model!v projection matrix! view matrix! model matrix! oculus.com! roll! rotation! translation! M view = R!T T "eye R = R z!" z ) R x!" x ) R y!" y pitch! yaw!

10 2 important coordinate systems:! oculus.com! body/sensor frame! inertial frame! M view = R!T "eye R = R z!" z ) R x!" x ) R y!" y roll! pitch! yaw!

11 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

12 ! 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!

13 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 1!a!a = R 1 & & = R z " 0 ' z )) R x ' x )) R y ' y! " & & normalize gravity vector rotated into sensor coordinates!

14 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 1!a!a = R 1 " 0 =!sin!" z ) 0 cos!" z ) 0 cos!" z sin!" z & & = R z & ' cos!" x 0 sin!" x ' z )) R x ' x )) R y ' y! " & &!sin!" x ) cos!" x ) & ' cos!" y ) 0 sin!" y ) cos!" y )!sin!" y & ' & '

15 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! â = 1!a!a =!sin!" z )cos!" x cos!" z )cos!" x sin!" x & '

16 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! â = 1!a!a =!sin!" z )cos!" x cos!" z )cos!" x sin!" x & ' â x roll! =!sin!" z â y cos!" z =! tan!" z )! z = "atan2 "â x,â y in rad![", ]

17 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! â = 1!a!a =!sin!" z )cos!" x cos!" z )cos!" x sin!" x & ' â z pitch! â 2 x + â = sin!" x 2 y cos 2!" x ) sin 2!" z = sin!" x cos!" x + cos 2!" z ) = tan!" x ) = 1

18 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! â = 1!a!a =!sin!" z )cos!" x cos!" z )cos!" x sin!" x & ' â z pitch! â 2 x + â = sin!" x 2 y cos 2!" x ) sin 2!" z! x = "atan2 â z, â x 2 + â y 2 + cos 2!" z ) = 1 in rad! " 2, & 2 ' )

19 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! â z pitch! â 2 x + â = sin!" x 2 y cos 2!" x ) sin 2!" z! x = "atan2 â z,sign â y ) â 2 2 x + â y + cos 2!" z ) = 1 in rad! ", [ ]

20 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!

21 Tilt Correction with Complementary Filtering! idea for sensor fusion: low-pass filter accelerometer and highpass filter gyro! linear interpolation between gyro & accelerometer!! pitch:!! x = "! x + x t) + 1 " ) rad2deg atan2 â z,sign â y )& â 2 2 x + â y yaw:!! y =! y + " y t roll:!! z = "! z + z t) + 1 " ) rad2deg atan2 â x,â y 0! "! 1

22 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 â z,sign â y )& â 2 2 x + â y yaw:!! y =! y + " y t roll:!! z = "! z + z t) + 1 " ) rad2deg atan2 â x,â y 0! "! 1

23 Example: Pitch! gyro dead reckoning)! accelerometer! complementary filter!

24 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!

25 Gimbal Lock! The Guerrilla CG Project, The Euler gimbal lock) Explained see youtube.com!

26 Rotations with Axis and Angle Representation! solution to gimbal lock: use a different representation for head rotation! axis and angle:! y! v x! z!

27 Quaternions!

28 ! Quaternions! what s a quaternion?! extension of complex numbers 3 imaginary numbers! q = q 0 + q 1 i + q 2 j + q 3 k i 2 = j 2 = k 2 = ijk =!1 i! j! k

29 Quaternions! what s a quaternion?! singularity-free description of rotation! = cos! 2, v sin! x 2, v sin! y 2, v sin! z 2 q!,v " & '

30 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! = cos! 2, v sin! x 2, v sin! y 2, v sin! z 2 q!,v " q = q q q q 3 2 = 1 & '

31 Quaternion from Axis & Angle! angle in radians! q!,v unit-length axis! " = cos! 2, v sin! x 2, v sin! y 2, v sin! z 2 & ' scalar = angle! vector = axis!

32 Quaternion to Axis & Angle! convert quaternion to rotation, then use glm::rotate, v x, v y, v z )!!! = 2 "acos q 0 in radians! in degrees! s = 1! q 0 2 v x = q 1 s, v y = q 2 s, v z = q 3 s

33 ! Quaternion Algebra! unit quaternion:! q = q q q q 2 2 = 1 vector quaternion represents 3D point or vector), need not be normalized:! q v = 0,v x,v y,v z addition:! q + p = q 0 + p 0 ) + q 1 + p 1 )i + q 2 + p 2 ) j + q 3 + p 3 )k

34 ! Quaternion Algebra! conjugate:! q * = q 0! q 1 i! q 2 j! q 3 k inverse:! q!1 = q* q 2

35 Quaternion Algebra! multiplication:! qp = q 0 + q 1 i + q 2 j + q 3 k) p 0 + p 1 i + p 2 j + p 3 k) + = q 0 p 0 q 1 p 1 q 2 p 2 q 3 p 3 q 0 p 1 + q 1 p 0 + q 2 p 3 q 3 p 2 )i j q 0 p 2 q 1 p 3 + q 2 p 0 + q 3 p 1 q 0 p 3 + q 1 p 2 q 2 p 1 + q 3 p 0 )k

36 Quaternion Algebra! all quaternions representing rotations are unit quaternions as opposed to vector quaternions)! rotation of vector quaternion by q :! inverse rotation:! q p q' p = qq p q!1 q' p = q!1 q p q successive rotations: q 2 q 1 rotate first by q 1 and then by! q 2

37 Quaternion-based! Orientation Tracking!

38 Quaternion-based Orientation Tracking! orientation representation: q t) 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)!

39 Quaternion-based Orientation Tracking! how to get rotation quaternion from gyro measurements?! measurements:! magnitude:! l =!!!! =!! x,!! y,!! z need this in radians / second!! q! = q!tl, 1 " l! & ' = cos!tl & " 2 ',! x l sin!tl 2 & " ',! y l sin!tl 2 & " ',! z l angle = rate of rotation radians/sec)! sin!tl & & 2 ' '

40 Quaternion-based Orientation Tracking! make sure not to divide by 0 if l=0!! measurements:! magnitude:! l =!!!! =!! x,!! y,!! z need this in radians / second!! q! = q!tl, 1 " l! & ' = cos!tl & " 2 ',! x l sin!tl 2 & " ',! y l sin!tl 2 & " ',! z l angle = rate of rotation radians/sec)! sin!tl & & 2 ' '

41 Quaternion-based Orientation Tracking! how to integrate rotation quaternion with gyro measurements?! q t+!t) = q t) q! q! = q!tl, 1 " l! & ' = cos!tl & " 2 ',! x l sin!tl 2 & " ',! y l sin!tl 2 & " ',! z l angle = rate of rotation radians/sec)! sin!tl & & 2 ' '

42 Quaternion-based Orientation Tracking! how to integrate rotation quaternion with gyro measurements?! q t+!t) = q t) q! forward process) model simple!! assumption: angular velocity is constant from t to t+1!

43 Tilt Correction with Quaternions! assume accelerometer measures gravity vector in body sensor) coordinates! transform measurements into inertial coordinates! t) q a inertial q ) a = q t) body q ) a q t)!1, q a = 0,!a x,!a y,!a ) z ) inertial),q a are vector quaternions! q a body

44 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/s 2! with gyro drift: doesn t point up! 6DOF sensor fusion to stabilize drift w.r.t. tilt pitch and roll)!

45 ! Tilt Correction with Quaternions! 1.! compute accelerometer in inertial coords! 2.! get normalized vector part of vector quaternion! inertial q ) a = q t) body q ) a q t)!1 inertial) q a v =! " inertial) q a1 inertial) q a, q a 2 inertial) q a inertial, q a 3 inertial) q a inertial & 3.! compute angle between v and UP 0,1,0) with dot product! v i 0,1,0 ) = v 0,1,0 ) cos!! = acos v y = acos " inertial) q a2 inertial) q a & '

46 Tilt Correction with Quaternions! 4.! compute normalized rotation axis between v and UP with cross product! v tilt = v! 0,1,0 = " v z v x 2 + v z 2,0, v x v x 2 + v z 2 & ' 5.! tilt correction quaternion is! q tilt!,v tilt ) i.e.! 0,0,1,0 ) = q tilt!,v tilt )q t) body q ) a q t)"1 q "1 tilt!,v tilt )

47 Complementary Filter! same idea as for Euler angles: use gyro measurements and correct tilt! t+!t q tilt _ corrected = q 1" ),v tilt )q gyro t+!t ) 0! "! 1 = q tilt _ corrected t+!t t ) where q gyro q! is the updated rotation quaternion from only the gyro measurements see slide 42)!

48 Complementary Filter! same idea as for Euler angles: use gyro measurements and correct tilt a bit into tilt of accelerometer! t+!t) q tilt _ corrected = q 1" ),v tilt )q gyro t+!t) 0! "! 1 just remember: all rotation quaternions need to be normalized, vector quaternions i.e. q 0 =0) need not be!

49 Head and Neck Model! y! x! z y l h l n IMU!!z pitch around base of neck!! l n x roll around base of neck!!

50 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,!l n,!l h )" R "T 0,l n,l h )"T!eye)

51 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 it s best to use them as general guidelines but follow our notation in class!