lecture9: modelingandmotionmodelsusers.isy.liu.se/rt/fredrik/edu/sensorfusion/lecture9.pdf ·...

Sensor Fusion 2014 Lecture 9 1

Lecture 9 Modeling and motion models

WhiteboardI Principles and some examples

SlidesI Sampling formulasI Noise modelsI Standard motion models

I Position as integrated velocity acceleration in nDI Orientation as integrated angular speed in 2D and 3D

I OdometryI (Extended) Kalman filter applications


Lecture 8 SummarySimultaneous Localization And Mapping (SLAM)

I Joint estimation of trajectory x1k and map parameters θ insensor model yk = h(xk θ) + ek

I AlgorithmsI NLS SLAM iterate between filtering and mappingI EKF-SLAM EKF (information form) on augmented state

vector zk = (xTk θ

T )T I FastSLAM MPF on augmented state vector zk = (xT

k θT )T

httpyoutubeVQlaCHl3Yc4 httpyoutubehA_NZeuoy9Y


Modeling

First problem physics give continuous time model filters require(linear or nonlinear) discrete time modelClassification Nonlinear LinearContinuous time x = a(x u) + v x = Ax + Bu + v

y = c(x u) + e y = Cx + Du + eDiscrete time xk+1 = f (x u) + v xk+1 = Fx + Gu + v

y = h(x u) + e y = Hx + Ju + eBut first sampling overview


Sampling formulasLTI state space model

x = Ax + Bu xk+1 = Fx + Guy = Cx + Du yk = Hx + Ju

u is either input or process noise (then J denotes cross-correlatednoise)Zero order hold sampling assuming the input is piecewise constant

x(t + T ) = eAT︸︷︷︸F

x(t) +

int T

0eAτB︸︷︷︸G

u(t + T minus τ)dτ

= eAT x(t) +

int T

0eAτdτBu(t)

First order hold sampling assuming the input is piecewise linear isanother option


Sampling formulas

Bilinear transformation assumes band-limited input

2T

z minus 1z + 1

x(t) asymp sx(t) = Ax + Bu

gives

M = (Inx minus T2A)minus1

F = M(Inx + T2A)

G = T2MBH = CM

J = D + HG


Sampling of nonlinear models

There are two optionsI Discretized linearization (general)

1 Linearization

A = aprimex(x u) B = aprimeu(x u) C = c primex(x u) D = c prime

u(x u)

2 Sampling F = eAT G =int T0 eAtdtB H = C and J = D

I Linearized discretization (best when possible)1 Nonlinear sampling

x(t + T ) = f (x(t) u(t)) = x(t) +

int t+T

ta(x(τ) u(τ))dτ

2 DiscretizationF = f primex (xk uk) and G = f prime

u(xk uk)


Sampling of state noise

a vt is continuous white noise with variance Q

Qa =

int T

0ef primeτQe(f

prime)T τ dτ

b vt = vk is a stochastic variable which is constant ineach sample interval and has variance QT

Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ

c vt is a sequence of Dirac impulses active immediatelyafter a sample is taken Loosely speaking we assumex = f (x) +

sumk vkδkTminust where vk is discrete white

noise with variance TQ

Qc =Tef primeTQe(fprime)TT


d vt is white noise such that its total influence duringone sample interval is TQ

Qd =TQ

e vt is a discrete white noise sequence with varianceTQ That is we assume that the noise entersimmediately after a sample time sox(t + T ) = f (x(t) + v(t))

Qe =Tg prime(x)Qg prime(x)T

Recommendation the simple solution in d works fine butremember the scaling with T


ModelsContinuous time (physical) and discrete time counterparts

x(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 rsquoBlack-boxrsquomulti-purpose

I Translation kinematics describes position often based onF = ma

I Rotational kinematics describes orientationI Rigid body kinematics combines translational and rotational

kinematicsI Constrained kinematics Coordinated turns (circular path

motion)I Application specific force modelsI Gray-box models where θ has to be calibrated (estimated

identified) from data


Translational motion with n integrators

Translational kinematics models in nD where p(t) denotes theposition X (1D) X Y (2D) and X Y Z (3D) respectivelyAnother 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 amotion input signal in position velocity and accelerationrespectively for the case of using sensed motion as an input ratherthan as a measurement

State Continuous time x = Discrete time x(t + T ) =x = p w x + Tw

x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva

0n In 0n0n 0n In0n 0n 0n

x +

0n0nIn

w

In TIn T2

2 In0n In TIn0n 0n In

x +

T3

6 InT2

2 InTIn

w


Different sampled models of a double integrator

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

Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models

I Navigation models have access to inertial informationI 2D orientation (course or yaw rate) much easier than 3D

orientation


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 fortranslational kinematics with p(t) = ψ and n = 1


Coordinated turns in 2D body coordinatesBasic motion equations

ψ =vx

R= vxRminus1

ay =v2xR

= v2xRminus1 = vx ψ

ax = vx minus vyvx

R= vx minus vyvxRminus1 = vx minus vy ψ

can be combined to a model suitable for the sensor configuration athand For instance

x =

(ψ

Rminus1

) u = vx y = Rminus1

x = f (x u) + w =

(vxRminus1

0

)+ w

is useful when speed is measured and a vision system delivers alocal estimate of (inverse) curve radius


Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel

Unideal Toothed Wheel

Each tooth passing the sensor (electromagnetic or Hall) gives apulse The number n of clock cycles between a number m of teethare registered within each sample interval

ω(tk) =2π

Ncog(tk minus tkminus1)=

2πNcogTc

mn

Problems Angle and time quantization Synchronization Angleoffsets δ in sensor teeth


Virtual sensors

Longitudinal velocity yaw rate and slip on left and right drivenwheel (front wheel driven assumed) can be computed from wheelangular speeds if the radii are known

vx =v3 + v4

2=ω3r3 + ω4r4

2

Ψ =ω3r3 minus ω4r4

B

s1 =ω1r1v1minus 1 s2 =

ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx

radic(1minus Rminus1B

2

)2

+ (Rminus1L)2


Geometry

The formulas are based on geometry the relation ψ = vxRminus1 andnotation below

L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω


OdometryOdometry is based on the virtual sensors

vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt

to dead-reckon the wheel speeds to a relative position in the globalframeThe position (Xt(r3 r4)Yt(r3 r4)) depends on the values of wheelradii r3 and r4 Further sources of error come from wheel slip inlongitudinal and lateral direction More sensors needed fornavigation



Much more complicated in 3D than 2D Could be a course in itselfCoordinate notation for rotations of a body in local coordinatesystem (x y z) relative to an earth fixed coordinate system

Motion components Rotation Euler angle Angular speedLongitudinal forward motion x Roll φ ωx

Lateral motion y Pitch θ ωy

Vertical motion z Yaw ψ ωz


Euler orientation in 3D

An earth fixed vector g (for instance the gravitational force) is inthe body system oriented as Qg where

Q = QxφQy

θQzψ

=

1 0 00 cosφ sinφ0 minus sinφ cosφ

cos θ 0 minus sin θ0 1 0

sin θ 0 cos θ

cosψ sinψ 0minus sinψ cosψ 0

0 0 1

=

cos θ cosψ cos θ sinψ minus sin θsinφ sin θ cosψ minus cosφ sinψ sinφ sin θ sinψ + cosφ cosψ sinφ cos θcosφ sin θ cosψ + sinφ sinψ cosφ sin θ sinψ minus sinφ cosψ cosφ cos θ

Note this result depends on the order of rotations QxφQ

yθQ

zψ

Here the xyz rule is used but there are other options


Euler rotation in 3D

When the body rotate with ω the Euler angles change according toωxωyωz

=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ

The dynamic equation for Euler angles can be derived from this asφψ

θ

=

1 sin(φ) tan(θ) cos(φ) tan(θ)0 cos(φ) minus sin(φ)0 sin(φ) sec(θ) cos(φ) sec(θ)

ωxωyωz

Singularities when θ = plusmnπ2 can cause numeric divergence


Unit quaternions

Vector representation q = (q0 q1 q2 q3)T Norm constraint of unit quaternion qTq = 1Then the quaternion is defined from the Euler angles as

q0 = cos(φ2) cos(θ2) cos(ψ2) + sin(φ2) sin(θ2) sin(ψ2)

q1 = sin(φ2) cos(θ2) cos(ψ2)minus cos(φ2) sin(θ2) sin(ψ2)

q2 = cos(φ2) sin(θ2) cos(ψ2) + sin(φ2) cos(θ2) sin(ψ2)

q3 = cos(φ2) cos(θ2) sin(ψ2)minus sin(φ2) sin(θ2) cos(ψ2)

Pros no singularity no 2π ambiguityCons more complex and non-intuitive algebra the norm constraintis a separate challenge Norm constraint can be handled byprojection or as a virtual measurement with small noise


Quaternion orientation in 3D

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

Q =

q20 + q2

1 minus q22 minus q2

3 2q1q2 minus 2q0q3 2q0q2 + 2q1q32q0q3 + 2q1q2 q2

0 minus q21 + q2

2 minus q23 minus2q0q1 + 2q2q3

minus2q0q2 + 2q1q3 2q2q3 + 2q0q1 q20 minus q2

1 minus q22 + q2

3

=

2q20 + 2q2

1 minus 1 2q1q2 minus 2q0q3 2q1q3 + 2q0q22q1q2 + 2q0q3 2q2

0 + 2q22 minus 1 2q2q3 minus 2q0q1

2q1q3 minus 2q0q2 2q2q3 + 2q0q1 2q20 + 2q2

3 minus 1


Quaternion rotation in 3D

Rotation with ω gives a dynamic equation for q which can bewritten in two equivalent forms

q =12S(ω)q =

12S(q)ω

S(ω) =

0 minusωx minusωy minusωzωx 0 ωz minusωyωy minusωz 0 ωxωz ωy minusωx 0

S(q) =

minusq1 minusq2 minusq3q0 minusq3 q2q3 q0 minusq1minusq2 q1 q0


Sampled form of quaternion dynamics

The ZOH sampling formula

q(t + T ) = e12S(ω(t))Tq(t)

has actually a closed form solution

q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)

The approximation coincides with Euler forward samplingapproximation and has to be used in more complex models whereeg ω is part of the state vector


Double integrated quaternion

(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)

There is no known closed form discretized model However theapproximate form can be discretized using the chain rule to(

q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)


Rigid body kinematics

A ldquomulti-purposerdquo model for all kind of navigation problems in 3D(22 states)

pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro

Bias states for the accelerometer and gyro have been added as well


Sensor model for kinematic modelInertial sensors (gyro accelerometer magnetometer) are used assensors

yacct = R(qt)(at minus g) + bacc

t + eacct eacc

t sim N(0Racc

t)

ymagt = R(qt)m + bmag

t + emagt emag

t sim N(0Rmag

t)

ygyrot = ωt + bgyro

t + egyrot egyro

t sim N(0Rgyro

t)

Bias observable but special calibration routines are recommendedI Stand-still detection When yacc

t asymp g andor ygyrot asymp 0

the gyro and acc bias is readily read off Can decrease drift indead-reckoning from cubic to linear

I Ellipse fitting When ldquowaving the sensorrdquo over short timeintervals the gyro can be integrated to give accurateorientation and acc and magnetometer measurements can betransformed to a sphere from an ellipse


Tracking models

I Navigation models have access to inertial information trackingmodels have not

I Orientation mainly the direction of the velocity vectorI Course (yaw rate) critical parameterI Not so big difference of 2D and 3D cases


Coordinated turns in 2D world coordinates

Cartesian velocity Polar velocityX = vX

Y = vY

vX = minusωvY

vY = ωvX

ω = 0

X = v cos(h)Y = v sin(h)v = 0h = ωω = 0

0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0

0 0 cos(h) minusv sin(h) 0

0 0 sin(h) v cos(h) 0

0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY

ω(1minus cos(ωT ))

Yt+T = Y + vX

ω(1minus cos(ωT )) + vY

ωsin(ωT )

vXt+T = vX cos(ωT )minus vY sin(ωT )

vYt+T = vX sin(ωT ) + vY cos(ωT )ωt+T = ω

Xt+T = X + 2vω

sin(ωT2 ) cos(h + ωT

2 )

Yt+T = Y minus 2vω

sin(ωT2 ) sin(h + ωT

2 )vt+T = vht+T = h + ωTωt+T = ω


Sounding rocketNavigation grade IMU gives accurate dead-reckoning but drift maycause return at bad placesGPS is restricted for high speeds and high accelerationsFusion of IMU and GPS when pseudo-ranges are available withIMU support to tracking loops inside GPS

I Loose integration direct fusion approach yk = pk + ek I Tight integration TDOA fusion approach

y ik = pk minus pi

kc + tk + ek

httpyoutubezRHFXfZLQ64


MC leaning angleI Headlight steering ABS and anti-spin systems require leaning

angleI Gyro very expensive for this applicationI Combination of accelerometers investigated lateral and

downward acc worked fine in EKFModel where zy zz a1 a2 J are constants relating to geometryand inertias of the motorcycle u = vx

x = (ϕ ϕ ϕ ψ ψ δay δaz δϕ)T

y = h(x) =

ayazϕ

=

ux4 minus zyx3 + zyx24 tan(x1) + g sin(x1) + x6

minusux4 tan(x1)minus zz(x22 + x2

4 tan2(x1))+ g cos(x1) + x7

minusa1x3 + a2x24 tan(x1)minus ux4J + x6

httpyoutubehT6S1FgHxOc0


Virtual yaw rate sensorI Yaw rate subject to bias orientation error increases linearly in

timeI Wheel speeds also give a gyro where the orientation error

grows linearly in distanceModel with state vector xk =

(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k

httpyoutubed9rzCCIBS9I


Friction estimation

I Friction model yk = 1smicrok + δk + ek where yk is the slip s isthe friction dependent slip slope microk is the normalized tractionforce from the engine δk a tire radius dependent offset and ekis measurement noise

I Kalman filter estimated s adaptivelyI When s decreases under a threshold a friction alarm is issued

httpyoutubesavmzo-xonk


Tire Pressure Monitoring System

I TPMS required in US since 2007 and in EU from 2013I Direct systems (with pressure sensors inside tire) most

common but indirect systems based on wheel speeds (ABSsensors) also on the market

I The TPMS by NIRA Dynamics is installed in 475 (may 2013)million cars (Audi VW Skoda Seat)

I Based on many different physical principles and EKFrsquosincluding the two ones above


Shooter localizationNetwork with 10 microphones around a rsquocamprsquo Shooter aiminginside the camp Supersonic bullet injects a shock wave followed byan acoustic muzzle blast Fusion of time differences give shooterposition and aiming pointModel with x being the position of the shooter

yMBk = t0 + bk + 1

c ||pk minus x ||+ eMBk

ySWk = t0 + bk + 1

r logv0

v0 minus r ||dk minus x ||+ 1

c ||dk minus pk ||+ eSWk

where dk is an implicit function of the state

httpyoutubeq-tU6YyJ5gU


Shooter localization resultsWLS loss function illustrates the information in yMB

k minus ySWk

k = 1 2 N = 10Both shooter position and aiming direction α well estimated foreach shotAlso bulletrsquos muzzle speed and ammunition length can beestimated

Lecture 9 Outline

Lecture 8 Summary

Sampling of process models


Standard models

Navigation models

Odometry

Tracking models

EKF applications


Lecture 8 SummarySimultaneous Localization And Mapping (SLAM)

I Joint estimation of trajectory x1k and map parameters θ insensor model yk = h(xk θ) + ek

I AlgorithmsI NLS SLAM iterate between filtering and mappingI EKF-SLAM EKF (information form) on augmented state

vector zk = (xTk θ

T )T I FastSLAM MPF on augmented state vector zk = (xT

k θT )T

httpyoutubeVQlaCHl3Yc4 httpyoutubehA_NZeuoy9Y


Modeling








x(t + T ) = eAT︸︷︷︸F

x(t) +

int T

0eAτB︸︷︷︸G


= eAT x(t) +

int T

0eAτdτBu(t)



Sampling formulas


2T

z minus 1z + 1


gives


F = M(Inx + T2A)

G = T2MBH = CM

J = D + HG




1 Linearization


u(x u)



x(t + T ) = f (x(t) u(t)) = x(t) +

int t+T

ta(x(τ) u(τ))dτ


u(xk uk)




Qa =

int T

0ef primeτQe(f

prime)T τ dτ


Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ







Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Modeling








x(t + T ) = eAT︸︷︷︸F

x(t) +

int T

0eAτB︸︷︷︸G


= eAT x(t) +

int T

0eAτdτBu(t)



Sampling formulas


2T

z minus 1z + 1


gives


F = M(Inx + T2A)

G = T2MBH = CM

J = D + HG




1 Linearization


u(x u)



x(t + T ) = f (x(t) u(t)) = x(t) +

int t+T

ta(x(τ) u(τ))dτ


u(xk uk)




Qa =

int T

0ef primeτQe(f

prime)T τ dτ


Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ







Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications





x(t + T ) = eAT︸︷︷︸F

x(t) +

int T

0eAτB︸︷︷︸G


= eAT x(t) +

int T

0eAτdτBu(t)



Sampling formulas


2T

z minus 1z + 1


gives


F = M(Inx + T2A)

G = T2MBH = CM

J = D + HG




1 Linearization


u(x u)



x(t + T ) = f (x(t) u(t)) = x(t) +

int t+T

ta(x(τ) u(τ))dτ


u(xk uk)




Qa =

int T

0ef primeτQe(f

prime)T τ dτ


Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ







Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Sampling formulas


2T

z minus 1z + 1


gives


F = M(Inx + T2A)

G = T2MBH = CM

J = D + HG




1 Linearization


u(x u)



x(t + T ) = f (x(t) u(t)) = x(t) +

int t+T

ta(x(τ) u(τ))dτ


u(xk uk)




Qa =

int T

0ef primeτQe(f

prime)T τ dτ


Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ







Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




1 Linearization


u(x u)



x(t + T ) = f (x(t) u(t)) = x(t) +

int t+T

ta(x(τ) u(τ))dτ


u(xk uk)




Qa =

int T

0ef primeτQe(f

prime)T τ dτ


Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ







Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




Qa =

int T

0ef primeτQe(f

prime)T τ dτ


Qb =1T

int T

0ef primeτ dτQ

int T

0e(f

prime)T τ dτ







Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications



Qd =TQ







x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




x(t + T ) = f (x(t) u(t)w(t) θT )











x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications





x =

(pv

) (0n In0n 0n

)x +

(0nIn

)w

(In TIn0n In

)x +

(T2

2 InTIn

)w

x =

pva


x +

0n0nIn

w

In TIn T2


x +

T3

6 InT2

2 InTIn

w




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




Continuous time A =

(0n In0n 0n

)B =

(0n

In

)C = (In 0n) D = 0n

ZOH F =

(In TIn0n In

)G =

(T2

2 InTIn

)H = (In 0n) J = 0n

FOH F =

(In TIn0n In

)G =

(T 2InTIn

)H = (In 0n) J = T2

6 In

BIL F =

(In TIn0n In

)G =

(T2

4 InT2 In

)H =

(In T

2 In)

J = T2

2 In


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Navigation models


orientation




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




ψ(t) = w(t)




ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications



ψ =vx

R= vxRminus1

ay =v2xR


ax = vx minus vyvx



x =

(ψ

Rminus1


x = f (x u) + w =

(vxRminus1

0

)+ w



Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Wheel speed sensor

Sensor

α

δi

Ideal Toothed wheel



ω(tk) =2π


2πNcogTc

mn



Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Virtual sensors


vx =v3 + v4

2=ω3r3 + ω4r4

2


B


ω2r2v2minus 1

v1 = vx

radic(1 +

Rminus1B2

)2

+ (Rminus1L)2

v2 = vx


2

)2

+ (Rminus1L)2


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Geometry


L

B

1R

2R

4R

3R

2δ

2α 2

v

ψ

2ω

1ω

3ω

4ω



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications



vmx =

ω3r3 + ω4r42

ψm =vmx

2B

ω3ω4

r3r4minus 1

ω3ω4

r3r4

+ 1

and the model

ψt = ψ0 +

int t

0ψtdt

Xt = X0 +

int t

0vxt cos(ψt)dt

Yt = Y0 +

int t

0vxt sin(ψt)dt











Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications










Q = QxφQy

θQzψ

=



sin θ 0 cos θ


0 0 1

=



yθQ

zψ





=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




=

φ00

+ Qxφ

0θ0

+ QxφQ

yθ

00ψ


θ

=


ωxωyωz



Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Unit quaternions










Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




Q =

q20 + q2



0 minus q21 + q2



1 minus q22 + q2

3

=

2q20 + 2q2




3 minus 1




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




q =12S(ω)q =

12S(q)ω

S(ω) =


S(q) =





q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




q(t + T ) = e12S(ω(t))Tq(t)


q(t + T ) =

cos(ω(t)T

2

)I4 +

T2

sin(ω(t)T

2

)ω(t)T

2

S(ω(t))

q(t)

asymp(I4 +

T2S(ω(t))

)q(t)




(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications



(q(t)ω(t)

)=

(12S(ω(t))q(t)

w(t)

)


q(t + T )ω(t + T )

)asymp((

I4 T2 S(ω(t))

) T2 S(q(t))

03times4 I3

)︸︷︷︸

F (t)

(q(t)ω(t)

)

+

(T 3

4 S(ω(t))I4TI3

)v(t)




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




pvaqω

bacc

bgyro

=

0 I 0 0 00 0 I 0 00 0 0 0 00 0 0 minus 1

2S(ω) 00 0 0 0 00 0 0 0 00 0 0 0 0

pvaqω

bacc

bgyro

+

0 0 0 00 0 0 01 0 0 00 0 0 00 1 0 00 0 1 00 0 0 1

va

vωvacc

vgyro





t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




t + eacct eacc

t sim N(0Racc

t)


t + emagt emag

t sim N(0Rmag

t)


t + egyrot egyro

t sim N(0Rgyro

t)






Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Tracking models






Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




Y = vY

vX = minusωvY

vY = ωvX

ω = 0


0 0 1 0 00 0 0 1 00 0 0 minusω minusvY

0 0 ω 0 vX

0 0 0 0 0



0 0 0 0 0

0 0 0 0 1

0 0 0 0 0

Xt+T = X + vX

ωsin(ωT )minus vY


Yt+T = Y + vX


ωsin(ωT )



Xt+T = X + 2vω


2 )

Yt+T = Y minus 2vω






y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications




y ik = pk minus pi

kc + tk + ek







y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications






y = h(x) =

ayazϕ

=



4 tan2(x1))+ g cos(x1) + x7







(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications





(ψk ψk bk

rk3rk4

)

y1k = ψk + bk + e1

k

y2k =

ω3rnom + ω4rnom

22B

ω3ω4

rk3rk4minus 1

ω3ω4

rk3rk4

+ 1+ e2

k



Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications


Friction estimation












yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications









yMBk = t0 + bk + 1


ySWk = t0 + bk + 1

r logv0







k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications



k minus ySWk


Lecture 9 Outline

Lecture 8 Summary



Standard models

Navigation models

Odometry

Tracking models

EKF applications

lecture9: modelingandmotionmodelsusers.isy.liu.se/rt/fredrik/edu/sensorfusion/lecture9.pdf ·...

Documents