Sensor Fusion, 2014 Lecture 9: 1

Lecture 9: Modeling and motion models

Whiteboard:
I Principles and some examples.
Slides:
I Sampling formulas.
I Noise models.
I Standard motion models.
I Position as integrated velocity, acceleration,... in nD.
I Orientation as integrated angular speed in 2D and 3D.
I Odometry
I (Extended) Kalman filter applications
Sensor Fusion, 2014 Lecture 9: 2

Lecture 8: Summary
Simultaneous Localization And Mapping (SLAM)
I Joint estimation of trajectory x1:k and map parameters θ in
sensor model yk = h(xk ; θ) + ek .
I Algorithms:
I NLS SLAM: iterate between filtering and mapping
I EKF-SLAM: EKF (information form) on augmented state
vector zk = (xkT , θT )T .
I FastSLAM: MPF on augmented state vector zk = (xkT , θT )T .

http://youtu.be/VQlaCHl3Yc4 http://youtu.be/hA_NZeuoy9Y
Sensor Fusion, 2014 Lecture 9: 3

Modeling

First problem: physics give continuous time model, filters require

(linear or nonlinear) discrete time model:
Classification Nonlinear Linear
Continuous time ẋ = a(x, u) + v ẋ = Ax + Bu + v
y = c(x, u) + e y = Cx + Du + e
Discrete time xk+1 = f (x, u) + v xk+1 = Fx + Gu + v
y = h(x, u) + e y = Hx + Ju + e
But first, sampling overview.
Sensor Fusion, 2014 Lecture 9: 4

Sampling formulas
LTI state space model

ẋ = Ax + Bu, xk+1 = Fx + Gu
y = Cx + Du, yk = Hx + Ju.

u is either input or process noise (then J denotes cross-correlated

noise!).
Zero order hold sampling assuming the input is piecewise constant:
Z T
AT
e x(t) +
x(t + T ) = |{z} e Aτ B u(t + T − τ )d τ
F |0 {z }
G
Z T
= e AT x(t) + e Aτ d τ Bu(t).
0

First order hold sampling assuming the input is piecewise linear is

another option.
Sensor Fusion, 2014 Lecture 9: 5

Sampling formulas

Bilinear transformation assumes band-limited input

2 z −1
x(t) ≈ sx(t) = Ax + Bu
T z +1
gives

M = (Inx − T /2A)−1 ,
F = M(Inx + T /2A),
G = T /2MB,
H = CM,
J = D + HG .
Sensor Fusion, 2014 Lecture 9: 6

Sampling of nonlinear models

There are two options:

I Discretized linearization (general):
1. Linearization:

A = ax0 (x, u), B = au0 (x, u), C = cx0 (x, u), D = cu0 (x, u),
RT
2. Sampling: F = e AT , G = 0
e At dtB, H = C and J = D.
I Linearized discretization (best, when possible!):
1. Nonlinear sampling:
Z t+T
x(t + T ) = f (x(t), u(t)) = x(t) + a(x(τ ), u(τ ))d τ,
t

2. Discretization:F = fx0 (xk , uk ) and G = fu0 (xk , uk ).

Sensor Fusion, 2014 Lecture 9: 7

Sampling of state noise

a vt is continuous white noise with variance Q.
Z T
0 0 T
Q̄a = e f τ Qe (f ) τ dτ
0

b vt = vk is a stochastic variable which is constant in

each sample interval and has variance Q/T .
T T
1
Z Z
0τ 0 )T τ
Q̄b = ef dτ Q e (f dτ
T 0 0

c vt is a sequence of Dirac impulses active immediately

after a sample
P is taken. Loosely speaking, we assume
ẋ = f (x) + k vk δkT −t where vk is discrete white
noise with variance TQ.
0T 0 )T T
Q̄c =Te f Qe (f
Sensor Fusion, 2014 Lecture 9: 8

d vt is white noise such that its total influence during

one sample interval is TQ.

Q̄d =TQ

e vt is a discrete white noise sequence with variance

TQ. That is, we assume that the noise enters
immediately after a sample time, so
x(t + T ) = f (x(t) + v (t)).

Q̄e =Tg 0 (x)Qg 0 (x)T

Recommendation: the simple solution in d works fine, but

remember the scaling with T !
Sensor Fusion, 2014 Lecture 9: 9

Models
Continuous time (physical) and discrete time counterparts

ẋ(t) = a(x(t), u(t), w (t); θ),

x(t + T ) = f (x(t), u(t), w (t); θ, T ).

I Kinematic models do not attempt to model forces. ’Black-box’

multi-purpose.
I Translation kinematics describes position, often based on
F = ma
I Rotational kinematics describes orientation.
I Rigid body kinematics combines translational and rotational
kinematics.
I Constrained kinematics. Coordinated turns (circular path
motion).
I Application specific force models.
I Gray-box models, where θ has to be calibrated (estimated,
identified) from data.
Sensor Fusion, 2014 Lecture 9: 10

Translational motion with n integrators

Translational kinematics models in nD, where p(t) denotes the
position X (1D), X , Y (2D) and X , Y , Z (3D), respectively.
Another interpretation is p(t) = ψ(t) or p(t) = (φ(t), θ(t), ψ(t))T .
The signal w (t) is process noise for a pure kinematic model and a
motion input signal in position, velocity and acceleration,
respectively, for the case of using sensed motion as an input rather
than as a measurement.

State Continuous time ẋ = Discrete time x(t + T ) =

x =p w x + Tw !
T2
       
p 0 n In 0n In TIn I
x= x+ w x+ 2 n w
v 0n 0n In 0n In TIn
 3 
       2  T
p 0 n In 0 n 0n In TIn T2 In In
 T62 
x = v  0n 0n In  x + 0n  w  0n In TIn  x +  In  w
2
a 0n 0n 0n In 0n 0n In TIn
Sensor Fusion, 2014 Lecture 9: 11

Different sampled models of a double integrator

State x(t) = (p(t), v (t))

   
0n In 0n
Continuous time A= B= C = (In , 0n ) D = 0n
0 0n I
 n   Tn2 
In TIn I
2 n
ZOH F = G = H = (In , 0n ) J = 0n
0n In TI n
   2 
In TIn T In T2
FOH F = G = H = (In , 0n ) J= I
6 n
0n In TI
   T2 n 
In TIn I T2
G = T4 n T


BIL F = H = In , I
2 n
J= I
2 n
0n In I
2 n
Sensor Fusion, 2014 Lecture 9: 12

Navigation models

I Navigation models have access to inertial information.

I 2D orientation (course, or yaw rate) much easier than 3D
orientation
Sensor Fusion, 2014 Lecture 9: 13

Rotational kinematics in 2D

The course, or yaw, in 2D can be modeled as integrated white noise

ψ̇(t) = w (t),

or any higher order of integration. Compare to the tables for

translational kinematics with p(t) = ψ and n = 1.
Sensor Fusion, 2014 Lecture 9: 14

Coordinated turns in 2D body coordinates

Basic motion equations
vx
ψ̇ = = vx R −1 ,
R
v2
ay = x = vx2 R −1 = vx ψ̇,
R
vx
ax = v̇x − vy = v̇x − vy vx R −1 = v̇x − vy ψ̇.
R
can be combined to a model suitable for the sensor configuration at
hand. For instance,
 
ψ
x= , u = vx , y = R −1
R −1
vx R −1
 
ẋ = f (x, u) + w = +w
0

is useful when speed is measured, and a vision system delivers a

local estimate of (inverse) curve radius.
Sensor Fusion, 2014 Lecture 9: 15

Wheel speed sensor

Ideal Toothed wheel
Unideal Toothed Wheel

δi

Sensor

Each tooth passing the sensor (electromagnetic or Hall) gives a

pulse. The number n of clock cycles between a number m of teeth
are registered within each sample interval.
2π 2π m
ω(tk ) = = .
Ncog (tk − tk−1 ) Ncog Tc n

Problems: Angle and time quantization. Synchronization. Angle

offsets δ in sensor teeth.
Sensor Fusion, 2014 Lecture 9: 16

Virtual sensors
Longitudinal velocity, yaw rate and slip on left and right driven
wheel (front wheel driven assumed) can be computed from wheel
angular speeds if the radii are known:
v3 + v4 ω3 r3 + ω4 r4
vx = = ,
2 2
ω3 r3 − ω4 r4
Ψ̇ = ,
B
ω1 r1 ω2 r2
s1 = − 1, s2 = − 1,
v1 v2
s
R −1 B 2

v1 = vx 1+ + (R −1 L)2 ,
2
s
R −1 B 2

v2 = vx 1− + (R −1 L)2 .
2
Sensor Fusion, 2014 Lecture 9: 17

Geometry

The formulas are based on geometry, the relation ψ̇ = vx R −1 and

notation below.

δ2
α2 v 2

ω1 ω2

ψ
R1 R2
L

B
R3 R4
ω3 ω4
Sensor Fusion, 2014 Lecture 9: 18

Odometry
Odometry is based on the virtual sensors
ω3 r3 + ω4 r4
vxm =
2
ω3 r 3
−1
m 2 ω4 r 4
ψ̇ m =vx ω3 r 3 .
B ω r +1
4 4

and the model

Z t
ψt = ψ0 + ψ˙t dt,
0
Z t
Xt = X0 + vtx cos(ψt )dt,
0
Z t
Yt = Y0 + vtx sin(ψt )dt.
0

to dead-reckon the wheel speeds to a relative position in the global

frame.
The position (Xt (r3 , r4 ), Yt (r3 , r4 )) depends on the values of wheel
radii r3 and r4 . Further sources of error come from wheel slip in
longitudinal and lateral direction. More sensors needed for
navigation.
Sensor Fusion, 2014 Lecture 9: 19

Rotational kinematics in 3D

Much more complicated in 3D than 2D! Could be a course in itself.

Coordinate notation for rotations of a body in local coordinate
system (x, y , z) relative to an earth fixed coordinate system:

Motion components Rotation Euler angle Angular speed

Longitudinal forward motion x Roll φ ωx
Lateral motion y Pitch θ ωy
Vertical motion z Yaw ψ ωz
Sensor Fusion, 2014 Lecture 9: 20

Euler orientation in 3D

An earth fixed vector g (for instance the gravitational force) is in

the body system oriented as Qg , where
Q = Qφx Qθy Qψ
z
   
1 0 0 cos θ 0 − sin θ cos ψ sin ψ 0
= 0 cos φ sin φ   0 1 0  − sin ψ cos ψ 0
0 − sin φ cos φ sin θ 0 cos θ 0 0 1
 
cos θ cos ψ cos θ sin ψ − sin θ
= sin φ sin θ cos ψ − cos φ sin ψ sin φ sin θ sin ψ + cos φ cos ψ sin φ cos θ  .
cos φ sin θ cos ψ + sin φ sin ψ cos φ sin θ sin ψ − sin φ cos ψ cos φ cos θ

Note: this result depends on the order of rotations Qφx Qθy Qψz .
Here, the xyz rule is used, but there are other options.
Sensor Fusion, 2014 Lecture 9: 21

Euler rotation in 3D

When the body rotate with ω, the Euler angles change according to
       
ωx φ̇ 0 0
ωy  =  0  + Qφx θ̇ + Qφx Q y  0 
θ
ωz 0 0 ψ̇

The dynamic equation for Euler angles can be derived from this as
    
φ̇ 1 sin(φ) tan(θ) cos(φ) tan(θ) ωx
ψ̇  = 0 cos(φ) − sin(φ)   ωy 
θ̇ 0 sin(φ) sec(θ) cos(φ) sec(θ) ωz

Singularities when θ = ±π/2, can cause numeric divergence!

Sensor Fusion, 2014 Lecture 9: 22

Unit quaternions

Vector representation: q = (q 0 , q 1 , q 2 , q 3 )T .
Norm constraint of unit quaternion: q T q = 1.
Then, the quaternion is defined from the Euler angles as

q 0 = cos(φ/2) cos(θ/2) cos(ψ/2) + sin(φ/2) sin(θ/2) sin(ψ/2),

q 1 = sin(φ/2) cos(θ/2) cos(ψ/2) − cos(φ/2) sin(θ/2) sin(ψ/2),
q 2 = cos(φ/2) sin(θ/2) cos(ψ/2) + sin(φ/2) cos(θ/2) sin(ψ/2),
q 3 = cos(φ/2) cos(θ/2) sin(ψ/2) − sin(φ/2) sin(θ/2) cos(ψ/2).

Pros: no singularity, no 2π ambiguity.

Cons: more complex and non-intuitive algebra, the norm constraint
is a separate challenge. Norm constraint can be handled by
projection or as a virtual measurement with small noise.
Sensor Fusion, 2014 Lecture 9: 23

Quaternion orientation in 3D

The orientation of the vector g in body system is Qg, where

 2
q0 + q12 − q22 − q32

2q1 q2 − 2q0 q3 2q0 q2 + 2q1 q3
Q =  2q0 q3 + 2q1 q2 q02 − q12 + q22 − q32 −2q0 q1 + 2q2 q3 
−2q0 q2 + 2q1 q3 2q2 q3 + 2q0 q1 q02 − q12 − q22 + q32
 2
2q0 + 2q12 − 1 2q1 q2 − 2q0 q3 2q1 q3 + 2q0 q2


= 2q1 q2 + 2q0 q3 2q02 + 2q22 − 1 2q2 q3 − 2q0 q1  .

2q1 q3 − 2q0 q2 2q2 q3 + 2q0 q1 2q02 + 2q32 − 1
Sensor Fusion, 2014 Lecture 9: 24

Quaternion rotation in 3D

Rotation with ω gives a dynamic equation for q which can be

written in two equivalent forms:
1 1
q̇ = S(ω)q = S̄(q)ω,
2
 2 
0 −ωx −ωy −ωz
ωx 0 ωz −ωy 
S(ω) = ωy −ωz
,
0 ωx 
ωz ωy −ωx 0
 
−q1 −q2 −q3
 q0 −q3 q2 
S̄(q) =  .
 q3 q0 −q1 
−q2 q1 q0
Sensor Fusion, 2014 Lecture 9: 25

Sampled form of quaternion dynamics

The ZOH sampling formula

1
q(t + T ) = e 2 S(ω(t))T q(t).

has actually a closed form solution

 
kω(t)kT
 

kω(t)T k

T sin 2
q(t + T ) = cos I4 + kω(t)kT
S(ω(t)) q(t)
2 2
2
 
T
≈ I4 + S(ω(t)) q(t).
2

The approximation coincides with Euler forward sampling

approximation, and has to be used in more complex models where
e.g. ω is part of the state vector.
Sensor Fusion, 2014 Lecture 9: 26

Double integrated quaternion

  1 
q̇(t) 2 S(ω(t))q(t)
= .
ω̇(t) w (t)

There is no known closed form discretized model. However, the

approximate form can be discretized using the chain rule to
  T
I4 2 S(ω(t)) T2 S̄(q(t))

 
q(t + T ) q(t)
≈
ω(t + T ) 03×4 I3 ω(t)
| {z }
F (t)
T3 
S(ω(t))I4
+ 4 v (t).
TI3
Sensor Fusion, 2014 Lecture 9: 27

Rigid body kinematics

A “multi-purpose” model for all kind of navigation problems in 3D

(22 states).
 
ṗ 0 I 0 0 0   p  0 0 0 0

 v̇  0 0 I 0 0  v  0 0 0 0  a 

ȧ
 
 0 v
 0 0 0 0  a  1
  
0 0 0  ω 

 v 
− 12 S(ω)
  
 q̇  = 0 0 0 0  q  +  0
  
0 0 0  acc  .

ω̇
   v
0 0 0 0 0   ω  0 1 0 0
  
v gyro
 
 acc     b acc  0
 ḃ  0 0 0 0 0 0 1 0
ḃ gyro 0 0 0 0 0 b gyro 0 0 0 1

Bias states for the accelerometer and gyro have been added as well.
Sensor Fusion, 2014 Lecture 9: 28

Sensor model for kinematic model

Inertial sensors (gyro, accelerometer, magnetometer) are used as
sensors.

ytacc = R(qt )(at − g) + btacc + etacc , etacc ∼ N 0, Rtacc ,

ytmag = R(qt )m + btmag + etmag , etmag ∼ N 0, Rtmag ,

ytgyro = ωt + btgyro + etgyro , etgyro ∼ N 0, Rtgyro ,

Bias observable, but special calibration routines are recommended:

I Stand-still detection. When kytacc k ≈ g and/or kytgyro k ≈ 0,
the gyro and acc bias is readily read off. Can decrease drift in
dead-reckoning from cubic to linear.
I Ellipse fitting. When “waving the sensor” over short time
intervals, the gyro can be integrated to give accurate
orientation, and acc and magnetometer measurements can be
transformed to a sphere from an ellipse.
Sensor Fusion, 2014 Lecture 9: 29

Tracking models

I Navigation models have access to inertial information, tracking

models have not.
I Orientation mainly the direction of the velocity vector.
I Course (yaw rate) critical parameter.
I Not so big difference of 2D and 3D cases.
Sensor Fusion, 2014 Lecture 9: 30

Coordinated turns in 2D world coordinates

Cartesian velocity Polar velocity

Ẋ = v X Ẋ = v cos(h)
Ẏ = v Y Ẏ = v sin(h)
v̇ X = −ωv Y v̇ = 0
v̇ Y = ωv X ḣ = ω
ω̇ = 0 ω̇ = 0
0 0 cos(h) −v sin(h) 0
 
 
0 0 1 0 0 
 0 0 sin(h) v cos(h)

0 
 0 0 0 1 0   
−v Y
   
 0
 0 0 −ω 

 0
 0 0 0 0 

 0 0 ω 0 vX  
 0

0 0 0 0 0  0 0 0 1 

0 0 0 0 0
vX vY 2v
Xt+T = X + ω
sin(ωT ) − ω
(1 − cos(ωT )) Xt+T = X + ω
sin( ωT
2
) cos(h + ωT
2
)
vX Y 2v
sin( ωT ) sin(h + ωT
Yt+T = Y + ω
(1 − cos(ωT )) + vω sin(ωT ) Yt+T = Y − ω 2 2
)
X
vt+T = v X cos(ωT ) − v Y sin(ωT ) vt+T = v
Y
vt+T = v X sin(ωT ) + v Y cos(ωT ) ht+T = h + ωT
ωt+T = ω ωt+T = ω
Sensor Fusion, 2014 Lecture 9: 31

Sounding rocket
Navigation grade IMU gives accurate dead-reckoning, but drift may
cause return at bad places.
GPS is restricted for high speeds and high accelerations.
Fusion of IMU and GPS when pseudo-ranges are available, with
IMU support to tracking loops inside GPS.
I Loose integration: direct fusion approach yk = pk + ek .
I Tight integration: TDOA fusion approach
yki = kpk − pki k/c + tk + ek .

http://youtu.be/zRHFXfZLQ64
Sensor Fusion, 2014 Lecture 9: 32

MC leaning angle
I Headlight steering, ABS and anti-spin systems require leaning
angle.
I Gyro very expensive for this application.
I Combination of accelerometers investigated, lateral and
downward acc worked fine in EKF.
Model, where zy , zz , a1 , a2 , J are constants relating to geometry
and inertias of the motorcycle, u = vx
x = (ϕ, ϕ̇, ϕ̈, ψ̇, ψ̈, δay , δaz , δϕ̇ )T ,
ux4 − zy x3 + zy x42 tan(x1 ) + g
sin(x1 ) + x6
   
ay
2 2 2
y = h(x) = az  = −ux4 tan(x1 ) − zz x2 + x4 tan (x1 ) + g cos(x1 ) + x7 
ϕ̇ −a1 x3 + a2 x42 tan(x1 ) − ux4 J + x6

http://youtu.be/hT6S1FgHxOc0%
Sensor Fusion, 2014 Lecture 9: 33

Virtual yaw rate sensor

IYaw rate subject to bias, orientation error increases linearly in
time.
I Wheel speeds also give a gyro, where the orientation error
grows linearly in distance,
r

Model, with state vector xk = ψ̇k , ψ̈k , bk , rk,3
k,4
,
yk1 = ψ̇k + bk + ek1 ,
ω3 rk,3
−1
ω3 rnom + ω4 rnom 2 ω4 rk,4
yk2 = + ek2 .
2 B ω3 rk,3
ω4 rk,4
+1

http://youtu.be/d9rzCCIBS9I
Sensor Fusion, 2014 Lecture 9: 34

Friction estimation
I Friction model yk = 1/sµk + δk + ek , where yk is the slip, s is
the friction dependent slip slope, µk is the normalized traction
force from the engine, δk a tire radius dependent offset and ek
is measurement noise.
I Kalman filter estimated s adaptively.
I When s decreases under a threshold, a friction alarm is issued.

http://youtu.be/savmzo-xonk
Sensor Fusion, 2014 Lecture 9: 35

Tire Pressure Monitoring System

I TPMS required in US since 2007, and in EU from 2013.
I Direct systems (with pressure sensors inside tire) most
common, but indirect systems based on wheel speeds (ABS
sensors) also on the market.
I The TPMS by NIRA Dynamics is installed in 4.75 (may 2013)
million cars (Audi, VW, Skoda, Seat).
I Based on many different physical principles and EKF’s,
including the two ones above!
Sensor Fusion, 2014 Lecture 9: 36

Shooter localization
Network with 10 microphones around a ’camp’. Shooter aiming
inside the camp. Supersonic bullet injects a shock wave followed by
an acoustic muzzle blast. Fusion of time differences give shooter
position and aiming point.
Model, with x being the position of the shooter,
ykMB = t0 + bk + c1 ||pk − x|| + ekMB ,
v0
ykSW = t0 + bk + 1r log + c1 ||dk − pk || + ekSW .
v0 − r ||dk − x||
where dk is an implicit function of the state.

http://youtu.be/q-tU6YyJ5gU
Sensor Fusion, 2014 Lecture 9: 37

Shooter localization results

WLS loss function illustrates the information in ykMB − ykSW ,
k = 1, 2, . . . , N = 10.
Both shooter position and aiming direction α well estimated for
each shot.
Also bullet’s muzzle speed and ammunition length can be
estimated!