Inertial Measurement Units II!
Gordon Wetzstein!
Stanford University!
!
EE 267 Virtual Reality!
Lecture 10!
stanford.edu/class/ee267/!
!
Polynesian Migration!
wikipedia!
Lecture Overview!
• short review of coordinate systems, tracking in flatland, and
accelerometer-only tracking !
• rotations: Euler angles, axis & angle, gimbal lock!
• rotations with quaternions!
• 6-DOF IMU sensor fusion with quaternions!
• primary goal: track
orientation of head or
device!
• inertial sensors required
pitch, yaw, and roll to be
determined!
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! view matrix! model matrix!
rotation! translation!
M view = R ⋅T ( −eye )
oculus.com!
Euler angles !
θy
rotation! translation!
θx M view = R ⋅T ( −eye )
θz
( )
R = Rz ( −θ z ) ⋅ Rx ( −θ x ) ⋅ Ry −θ y
roll! pitch! yaw!
oculus.com!
Euler angles !
θy
2 important
coordinate systems: !
body/sensor world/inertial
frame! frame!
θx M view = R ⋅T ( −eye )
θz
( )
R = Rz ( −θ z ) ⋅ Rx ( −θ x ) ⋅ Ry −θ y
roll! pitch! yaw!
oculus.com!
Gyro Integration aka Dead Reckoning!
• from gyro measurements to orientation – use Taylor expansion!
have: angle at! have: !
last time step! time step!
∂
θ ( t + Δt ) ≈ θ ( t ) + θ ( t ) Δt + ε , ε ∼ O ( Δt 2 )
! ∂t
want: angle at!
current time step!
= ω approximation error!!
have: gyro measurement!
(angular velocity)!
Orientation Tracking in Flatland!
• problem: track 1 angle in 2D space!
• sensors: 1 gyro, 2-axis
accelerometer!
VRduino
• sensor fusion with complementary IMU
filter, i.e. linear interpolation:!
( ) (
θ (t ) = α θ (t−1) + ω! Δt + (1− α ) atan2 a! x , a! y )
• no drift, no noise!!
Tilt from Accelerometer!
• assuming acceleration points up (i.e. no external forces), we can
compute the tilt (i.e. pitch and roll) from a 3-axis accelerometer !
⎛ 0 ⎞ ⎛ 0 ⎞
a!
â =
a! ⎜ ⎟
( )
= R ⎜ 1 ⎟ = Rz ( −θ z ) ⋅ Rx ( −θ x ) ⋅ Ry −θ y ⎜ 1 ⎟
⎜ ⎟
⎝ 0 ⎠ ⎝ 0 ⎠
⎛ − cos ( −θ ) sin ( −θ )
⎜
x z
⎞
⎟ ( ( )
θ x = −atan2 âz ,sign ây ⋅ âx2 + ây2 )
= ⎜ cos ( −θ x ) cos ( −θ z ) ⎟
⎜ ⎟ θz = −atan2 ( − â , â ) both in rad
sin ( −θ x )
x y
⎜⎝ ⎟⎠
Euler Angles and Gimbal Lock!
• so far we have represented head rotations with Euler angles: 3
rotation angles around the axis applied in a specific sequence!
• problematic when interpolating between rotations in keyframes
(in computer animation) or integration à singularities!
Gimbal Lock!
The Guerrilla CG Project, The Euler (gimbal lock) Explained – see: https://www.youtube.com/watch?v=zc8b2Jo7mno!
Rotations with Axis-Angle Representation
and Quaternions!
Rotations with Axis and Angle Representation!
• solution to gimbal lock: use axis and angle representation for
rotation!!
• simultaneous rotation around a
normalized vector v by angle θ
!
• no “order” of rotation, all at once around
that vector!
Quaternions!
• think about quaternions as an extension of complex numbers
to having 3 (different) imaginary numbers or fundamental
quaternion units i,j,k!
q = qw + iqx + jqy + kqz
ij = − ji = k
i≠ j≠k ki = −ik = j
!
i 2 = j 2 = k 2 = ijk = −1 jk = −kj = i
Quaternions!
• think about quaternions as an extension of complex numbers
to having 3 (different) imaginary numbers or fundamental
quaternion units i,j,k!
q = qw + iqx + jqy + kqz
• quaternion algebra is well-defined and will give us a powerful
tool to work with rotations in axis-angle representation in
practice!
Quaternions!
• axis-angle to quaternion (need normalized axis v)!
⎛θ ⎞ ⎛θ ⎞ ⎛θ ⎞ ⎛θ ⎞
q (θ ,v ) = cos ⎜ ⎟ + i vx sin ⎜ ⎟ + j vy sin ⎜ ⎟ + k vz sin ⎜ ⎟
⎝ 2⎠ ⎝ 2⎠ ⎝ 2⎠ ⎝ 2⎠
!"
# # $ !#"# $ !#"# $ !#"# $
qw qx qy qz
Quaternions!
• axis-angle to quaternion (need normalized axis v)!
⎛θ ⎞ ⎛θ ⎞ ⎛θ ⎞ ⎛θ ⎞
q (θ ,v ) = cos ⎜ ⎟ + i vx sin ⎜ ⎟ + j vy sin ⎜ ⎟ + k vz sin ⎜ ⎟
⎝ 2⎠ ⎝ 2⎠ ⎝ 2⎠ ⎝ 2⎠
!"
# # $ !#"# $ !#"# $ !#"# $
qw qx qy qz
• valid rotation quaternions have unit length!
q = qw2 + qx2 + qy2 + qz2 = 1
Two Types of Quaternions!
• vector quaternions represent 3D points or vectors u=(ux,uy,uz)
can have arbitrary length!
qu = 0 + i u x + j u y + k uz
!
• valid rotation quaternions have unit length!
q = qw2 + qx2 + qy2 + qz2 = 1
Quaternion Algebra!
• quaternion addition:!
!
!
(
q + p = ( qw + pw ) + i ( qx + px ) + j qy + py + k ( qz + pz ) )
• quaternion multiplication:!
(
qp = qw + iqx + jqy + kqz )( p w + ipx + jpy + kpz )
= (q p − q p − q p − q p ) +
w w x x y y z z
i (q p + q p + q p − q p ) +
w x x w y z z y
j (q p − q p + q p + q p ) +
w y x z y w z x
k (q p + q p − q p + q p ) +
w z x y y x z w
Quaternion Algebra!
• quaternion conjugate:! q* = qw − iqx − jqy − kqz
!
q*
−1
• quaternion inverse:! q = 2
q
• rotation of vector quaternion qu by q :! q'u = qqu q −1
• inverse rotation:! qu = q −1q'u q
• successive rotations by q1 then q2 : ! q'u = q2 q1 qu q1−1 q2−1
Quaternion Algebra!
• detailed derivations and reference of general quaternion
algebra and rotations with quaternions in course notes!
• please read course notes for more details!!
Quaternion-based!
6-DOF Orientation Tracking!
Quaternion-based Orientation Tracking!
1. 3-axis gyro integration!
2. computing the tilt correction quaternion!
3. applying a complementary filter!
Gyro Integration with Quaternions!
(0)
• start with initial quaternion: ! q = 1+ i0 + j0 + k0
• convert 3-axis gyro measurements ω! = (ω! x , ω! y , ω! z ) to
instantaneous rotation quaternion as!
⎛ ω! ⎞
qΔ = q ⎜ Δt ω! ,
ω! ⎟⎠
avoid division by 0!!
⎝
angle! rotation
!
! axis!
• integrate as! qω(t+Δt ) = q(t )qΔ
Gyro Integration with Quaternions!
(t+Δt )
• integrated gyro rotation quaternion qω represents rotation
from body to world frame, i.e.!
( world ) (t+Δt ) (body ) (t+Δt )−1
qu = qω qu qω
(t )
• last estimate q is either from gyro-only (for dead reckoning)
or from last complementary filter!
• integrate as! qω(t+Δt ) = q(t )qΔ
Tilt Correction with Quaternions!
• assume accelerometer measures gravity vector in body
(
(sensor) coordinates! a! = a! , a! , a!
x y z )
• transform vector quaternion of a! into current estimation of
world space as!
( world ) (t+Δt ) ( body ) (t+Δt )−1
qa = qω qa qω
(body )
qa = 0 + ia! x + ja! y + ka! z
Tilt Correction with Quaternions!
• assume accelerometer measures gravity vector in body
(
(sensor) coordinates! a! = a! , a! , a!
x y z )
• transform vector quaternion of a! into current estimation of
world space as!
( world ) (t+Δt ) ( body ) (t+Δt )−1
qa = qω qa qω
• if gyro quaternion is correct, then accelerometer world vector
points up, i.e.! ( world )
qa = 0 + i0 + j 9.81+ k0
Tilt Correction with Quaternions!
• gyro quaternion likely includes drift!
• accelerometer measurements are noisy and also include
forces other than gravity, so it’s unlikely that accelerometer
world vector actually points up!
• if gyro quaternion is correct, then accelerometer world vector
points up, i.e.! ( world )
qa = 0 + i0 + j 9.81+ k0
!
Tilt Correction with Quaternions!
( world )
solution: compute tilt correction quaternion that would rotate qa
into up direction!
( world )
how? get normalized vector part of vector quaternion! qa
⎛ q( world ) qa( world ) qa( world ) ⎞
v = ⎜ ( world
ax
⎟
y
, , z
⎜⎝ qa ) qa( world ) qa( world ) ⎟⎠
!
Tilt Correction with Quaternions!
(
world )
solution: compute tilt correction quaternion that would rotate qa
into up direction!
⎛ n ⎞
qt = q ⎜ φ , ⎟
⎝ n ⎠
⎛ vx ⎞ ⎛ ⎞
0
⎜ ⎟ ⎜
⎜ vy ⎟ i ⎜ 1
⎟ = cos (φ ) ⇒
⎟
( )
φ = cos −1 vy
⎜ v ⎟ ⎝ 0 ⎠
⎝ z ⎠
⎛ vx ⎞ ⎛ 0 ⎞ ⎛ −vz ⎞
⎜ ⎟ ⎟ =⎜ 0 ⎟
n=⎜ vy ⎟ × ⎜ 1
⎜ ⎟ ⎜ ⎟
⎜ ⎟
vz ⎠ ⎝ 0 ⎠ ⎜⎝ vx ⎟⎠
⎝
Complementary Filter with Quaternions!
• complementary filter: rotate into gyro world space first, then
rotate “a bit” into the direction of the tilt correction quaternion!
⎛ n ⎞ (t+Δt )
qc (t+Δt )
= q ⎜ (1− α )φ , ⎟ qω 0 ≤α ≤1
⎝ n ⎠
−1
( world )
• rotation of any vector quaternion is then ! qu = qc(t+Δt )qu( body )qc(t+Δt )
Integration into Graphics Pipeline!
(t+Δt )
• compute qc via quaternion complementary filter first !
• stream from microcontroller to PC!
• convert to 4x4 rotation matrix (see course notes)! qc(t+Δt ) ⇒ Rc
• set view matrix to M view = Rc−1 to rotate the world in front of the
virtual camera !
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 )
Must read: course notes on IMUs!!
