a crash course on kalman filtering - cleveland state university · 2014-09-18 · a crash course on...

A Crash Course on Kalman Filtering

Dan Simon

Cleveland State University

Fall 2014

1 / 64

Outline

Linear Systems

Probability

State Means and Covariances

Least Squares Estimation

The Kalman Filter

Unknown Input Estimation

The Extended Kalman Filter

2 / 64

Linear Systems

KVL: u = vc + L diL/dt KCL: ic + ir1 = ir2 + iLic = C dvc/dtir1 = vc/Rir2 = (u − vc)/R=⇒ dvc/dt = −2vc/RC + iL/C + u/RCDefine xT =

[vc iL

]x =

[−2/RC 1/C−1/L 0

]x +

[1/RC1/L

]u = Ax + Bu

What is the “output”?Suppose y = vr2 = L diL/dt =

[−1 0

]x + u = Cx + Du

3 / 64

Linear Systems

Let R = C = L = 1 and u(t) = step function.How can we simulate the system in Matlab?

Control System Toolbox

Simulink

m-file

4 / 64

Linear Systems: Continuous-Time Simulation

Control System Toolbox:R = 1; L = 1; C = 1;A = [-2/R/C, 1/C ; -1/L, 0];B = [1/R/C ; 1/L];C = [-1, 0];D = 1;sys = ss(A, B, C, D);step(sys)

Simulink: LinearRLC1Model.slx

m-file: LinearRLC1.mWhat time step should we use?

5 / 64

Linear Systems: Discretization

Continuous time: x = Ax + Bu, y = Cx + Du

Discrete time: xk+1 = Fx + Gu, y = Cx + Du

F = exp(A∆t)

G = (F − I )A−1B

where ∆t is the integration step size; I is the identity matrix

6 / 64

Linear Systems: Discrete-Time Simulation

Control System Toolbox:dt = 0.1;F = expm(A*dt);G = (F - eye(2)) / A * B;sysDiscrete = ss(F, G, C, D, dt);step(sysDiscrete)

Simulink: LinearRLC1DiscreteModel.slx

m-file: LinearRLC1Discrete.m

7 / 64

Outline

Linear Systems

Probability



The Kalman Filter



8 / 64

Cumulative Distribution Function

X = random variable

CDF: FX (x) = P(X ≤ x)

FX (x) ∈ [0, 1]

FX (−∞) = 0

FX (∞) = 1

FX (a) ≤ FX (b) if a ≤ b

P(a < X ≤ b) = FX (b)− FX (a)

9 / 64

Probability Density Function

PDF: fX (x) =dFX (x)

dx

FX (x) =

∫ x

−∞fX (z) dz

fX (x) ≥ 0∫ ∞−∞

fX (x) dx = 1

P(a < x ≤ b) =

∫ b

afX (x) dx

Expected value: E [g(X )] =

∫ ∞−∞

g(x)fX (x) dx

E (X ) = x = µx = mean

E[(X − x)2

]= σ2x = variance

σx = standard deviation

10 / 64

Probability

Random numbers in Matlab: rand and randn

Random number seedHow can we create a random vector with given covariance R?

Probability Density Functions

Uniform DistributionGaussian, or Normal, Distribution

11 / 64

Multiple Random Variables

CDF: FXY (x , y) = P(X ≤ x ,Y ≤ y)

PDF: fXY (x , y) =∂2FXY (x , y)

∂x∂y

Independence:

P(X ≤ x ,Y ≤ y) = P(X ≤ x)P(Y ≤ y) for all x , y

Covariance: CXY = E [(X − X )(Y − Y ]

= E (XY )− X Y

Correlation: RXY = E (XY )

12 / 64

Random Vectors

X =[X1 X2

]CDF: FX (x) = P(X1 ≤ x1,X2 ≤ x2)

pdf: fX (x) =∂2FX (x)

∂x1∂x2

Autocorrelation: RX = E [XXT ] > 0

Autocovariance: CX = E [(X − X )(X − X )T ] > 0

Gaussian RV:

PDF(x) =1

(2π)n/2|CX |1/2exp

[−1

2(x − x)TC−1X (x − x)

]If Y = AX + b, then Y ∼ N(Ax + b,ACXA

T )

13 / 64

Stochastic Processes

A stochastic process X (t) is an RV that varies with time

If X (t1) and X (t2) are independent ∀ t1 6= t2 thenX (t) is white

Otherwise, X (t) is colored

Examples:

The high temperature on a given day

The closing price of the stock market

Measurement noise in a voltmeter

The amount of sleep you get each night

14 / 64

Outline

Linear Systems

Probability



The Kalman Filter



15 / 64


xk = Fk−1xk−1 + Gk−1uk−1 + wk−1

wk ∼ (0,Qk)

xk = E (xk)

= Fk−1xk−1 + Gk−1uk−1

(xk − xk)(· · · )T = (Fk−1xk−1 + Gk−1uk−1 + wk−1 − xk)(· · · )T

= [Fk−1(xk−1 − xk−1) + wk−1][· · · ]T

= Fk−1(xk−1 − xk−1)(xk−1 − xk−1)TFTk−1 +

wk−1wTk−1 + Fk−1(xk−1 − xk−1)wT

k−1 +

wk−1(xk−1 − xk−1)TFTk−1

Pk = E[(xk − xk)(· · · )T

]= Fk−1Pk−1F

Tk−1 + Qk−1

This is the discrete-time Lyapunov Equation, or Stein Equation

16 / 64

Outline

Linear Systems

Probability



The Kalman Filter



17 / 64


Suppose x is a constant vector

Vector measurement at time k: yk = Hkx + vk , vk ∼ (0,Rk)

Estimate: xk = xk−1 + Kk(yk − Hk xk−1)

This is a recursive estimatorre·cur·sive: adjective, meaning recursive

Our goal: Find the “best” estimator gain Kk

18 / 64


What is the mean of the estimation error?

E (εx ,k) = E (x − xk)

= E [x − xk−1 − Kk(yk − Hk xk−1)]

= E [εx ,k−1 − Kk(Hkx + vk − Hk xk−1)]

= E [εx ,k−1 − KkHk(x − xk−1)− Kkvk ]

= (I − KkHk)E (εx ,k−1)− KkE (vk)

E (εx ,k) = 0 if E (vk) = 0 and E (εx ,k−1) = 0, regardless of Kk

Unbiased estimator

19 / 64


Objective function: Jk = E [(x1 − x1)2] + · · ·+ E [(xn − xn)2]

= E(ε2x1,k + · · ·+ ε2xn,k

)= E

(εTx ,kεx ,k

)= E

[Tr(εx ,kε

Tx ,k)]

= TrPk

20 / 64


Pk = E (εx ,kεTx ,k)

= E{

[(I − KkHk)εx ,k−1 − Kkvk ][· · · ]T}

= (I − KkHk)E (εx ,k−1εTx ,k−1)(I − KkHk)T −

KkE (vkεTx ,k−1)(I − KkHk)T − (I − KkHk)E (εx ,k−1v

Tk )KT

k +

KkE (vkvTk )KT

k

= (I − KkHk)Pk−1(I − KkHk)T + KkRkKTk

21 / 64


Recall that ∂Tr(ABAT )∂A = 2AB if B is symmetric

∂Jk∂Kk

= 2(I − KkHk)Pk−1(−HTk ) + 2KkRk

= 0

KkRk = (I − KkHk)Pk−1HTk

Kk(Rk + HkPk−1HTk ) = Pk−1H

Tk

Kk = Pk−1HTk (HkPk−1H

Tk + Rk)−1

22 / 64

Recursive least squares estimation of a constant

1 Initialization: x0 = E (x), P0 = E [(x − x0)(x − x0)T ]If no knowledge about x is available before measurements aretaken, then P0 =∞I . If perfect knowledge about x isavailable before measurements are taken, then P0 = 0.

2 For k = 1, 2, · · · , perform the following.1 Obtain measurement yk :

yk = Hkx + vk

where vk ∼ (0,Rk) and E (vivk) = Rkδk−i (white noise)2 Measurement update of estimate:


Tk + Rk)−1

xk = xk−1 + Kk(yk − Hk xk−1)

Pk = (I − KkHk)Pk−1(I − KkHk)T + KkRkKTk

23 / 64

Alternate Estimator Equations


Tk + Rk)−1

= PkHTk R−1k


= (P−1k−1 + HTk R−1k Hk)−1

= (I − KkHk)Pk−1 (Valid only for optimal Kk)

Example: RLS.m

24 / 64

Outline

Linear Systems

Probability



The Kalman Filter



25 / 64

The Kalman filter

xk = Fk−1xk−1 + Gk−1uk−1 + wk−1

yk = Hkxk + vk

wk ∼ (0,Qk)

vk ∼ (0,Rk)

E [wkwTj ] = Qkδk−j

E [vkvTj ] = Rkδk−j

E [vkwTj ] = 0

x+k = E [xk |y1, y2, · · · , yk ] = a posteriori estimate

P+k = E [(xk − x+k )(xk − x+k )T ]

x−k = E [xk |y1, y2, · · · , yk−1] = a priori estimate

P−k = E [(xk − x−k )(xk − x−k )T ]

xk|k+N = E [xk |y1, y2, · · · , yk , · · · , yk+N ] = smoothed estimate

xk|k−M = E [xk |y1, y2, · · · , yk−M ] = predicted estimate

26 / 64

Time Update Equations

Initialization: x+0 = E (x0)

x−1 = F0x+0 + G0u0

P−1 = F0P+0 FT

0 + Q0

Generalize: x−k = Fk−1x+k−1 + Gk−1uk−1

P−k = Fk−1P+k−1F

Tk−1 + Qk−1

These are the Kalman filter time update equations

27 / 64

Measurement Update Equations

Recall the RLS estimate of a constant x :


Tk + Rk)−1



xk−1,Pk−1 = estimate and covariance before measurement ykxk ,Pk = estimate and covariance after measurement yk

Least squares estimator Kalman filter

xk−1 = estimate before yk =⇒ x−k = a priori estimatePk−1 = covariance before yk =⇒ P−k = a priori covariancexk = estimate after yk =⇒ x+k = a posteriori estimatePk = covariance after yk =⇒ P+

k = a posteriori covariance

28 / 64

Measurement Update Equations

Recursive Least Squares:


Tk + Rk)−1



Kalman Filter:

Kk = P−k HTk (HkP

−k HT

k + Rk)−1

x+k = x−k + Kk(yk − Hk x−k )

P+k = (I − KkHk)P−k (I − KkHk)T + KkRkK

Tk

These are the Kalman filter measurement update equations

29 / 64

Kalman Filter Equations

1 State equations:

xk = Fk−1xk−1 + Gk−1uk−1 + wk−1

yk = Hkxk + vk

E (wkwTj ) = Qkδk−j ,E (vkv

Tj ) = Rkδk−j ,E (wkv

Tj ) = 0

2 Initialization: x+0 = E (x0), P+0 = E [(x0 − x+0 )(x0 − x+0 )T ]

3 For each time step k = 1, 2, · · ·P−k = Fk−1P

+k−1F

Tk−1 + Qk−1

Kk = P−k HTk (HkP

−k HT

k + Rk)−1 = P+k HT

k R−1k

x−k = Fk−1x+k−1 + Gk−1uk−1 = a priori state estimate

x+k = x−k + Kk(yk − Hk x−k ) = a posteriori state estimate

P+k = (I − KkHk)P−k (I − KkHk)T + KkRkK

Tk

=[(P−k )−1 + HT

k R−1k Hk

]−1= (I − KkHk)P−k

30 / 64

Kalman Filter Properties

Define estimation error xk = xk − xkProblem: minE

[xTk Sk xk

], where Sk > 0

If {wk} and {vk} are Gaussian, zero-mean, uncorrelated, andwhite, then the Kalman filter solves the problem.

If {wk} and {vk} are zero-mean, uncorrelated, and white, thenthe Kalman filter is the best linear solution to the problem.

If {wk} and {vk} are correlated or colored, then the Kalmanfilter can be easily modified to solve the problem.

For nonlinear systems, the Kalman filter can be modified toapproximate the solution to the problem.

31 / 64

Kalman Filter Example: DiscreteKFEx1.m

rva

=

0 1 00 0 10 0 0

rva

+ w =⇒ x = Ax + w

xk+1 = Fxk + wk

F = exp(AT ) =

1 T T 2/20 1 T0 0 1

wk ∼ (0,Qk)

x−k = F x+k−1

P−k = FP+k−1F

T + Qk−1

yk = Hkxk + vk

=[

1 0 0]xk + vk

vk ∼ (0,Rk),Rk = σ2

32 / 64

Kalman Filter Divergence

Kalman filter theory is based on several assumptions.How to improve filter performance in the real world:

Increase arithmetic precision

Square root filtering

Use a fading-memory Kalman filter

Use fictitious process noise

Use a more robust filter (e.g., H-infinity)

33 / 64

Modeling Errors

True System:

x1,k+1 = x1,k + x2,k

x2,k+1 = x2,k

yk = x1,k + vk

vk ∼ (0, 1)

Incorrect Model:

x1,k+1 = x1,k

yk = xk + vk

wk ∼ (0,Q), Q = 0

vk ∼ (0, 1)

34 / 64

Modeling Errors

0 10 20 30 40 500

100

200

300

400

500

600

time

true stateestimated state

Figure: Kalman filter divergence due to mismodeling

35 / 64

Fictitious Process Noise

0 10 20 30 40 500

100

200

300

400

500

600

time

true statexhat (Q = 1)xhat (Q = 0.1)xhat (Q = 0.01)xhat (Q = 0)

Figure: Kalman filter improvement due to fictitious process noise

36 / 64

Fictitious Process Noise

0 10 20 30 40 500

0.1

0.2

0.3

0.4

0.5

0.6

0.7

time

Q = 1Q = 0.1Q = 0.01Q = 0

Figure: Kalman gain for various values of process noise

37 / 64

The Continuous-Time Kalman Filter

x = Ax + Bu + w

y = Cx + v

w ∼ (0,Qc)

v ∼ (0,Rc)

Discretize:

xk = Fxk−1 + Guk−1 + wk−1

yk = Hxk + vk

F = exp(AT ) ≈ (I + AT ) for small T

G = (exp(AT )− I )A−1B ≈ BT for small T

H = C

wk ∼ (0,Q), Q = QcT

vk ∼ (0,R), R = Rc/T

38 / 64


Recall the discrete-time Kalman gain:

Kk = P−k HT (HP−k HT + R)−1

= P−k CT (CP−k CT + Rc/T )−1

Kk

T= P−k CT (CP−k CTT + Rc)−1

limT→0

Kk

T= P−k CTR−1c

39 / 64


Recall the discrete-time estimation error covariance equations:

P+k = (I − KkH)P−k

P−k+1 = FP+k FT + Q

= (I + AT )P+k (I + AT )T + QcT , for small T

= P+k + (AP+

k + P+k AT + Qc)T + AP+

k ATT 2

= (I − KkC )P−k + AP+k ATT 2 +

[A(I − KkC )P−k + (I − KkC )P−k AT + Qc ]T

P−k+1 − P−kT

=−KkCP

−k

T+ AP+

k ATT +

(AP−k + AKkCP−k + P−k AT − KkCP

−k AT + Qc)

P = limT→0

P−k+1 − P−kT

= −PCTR−1c CP + AP + PAT + Qc

40 / 64


Recall the discrete-time state estimate equations:

x−k = F x+k−1 + Guk−1

x+k = x−k + Kk(yk − Hx−k )

= F x+k−1 + Guk−1 + Kk(yk − HF x+k−1 − HGuk−1)

≈ (I + AT )x+k−1 + BTuk−1 +

Kk(yk − C (I + AT )x+k−1 − CBTuk−1), for small T

= x+k−1 + ATx+k−1 + BTuk−1 +

PCTR−1c T (yk − Cx+k−1 − CATx+k−1 − CBTuk−1)

˙x = limT→0

x+k − x+k−1T

= Ax + Bu + PCTR−1c (y − Cx)

˙x = Ax + Bu + K (y − Cx)

K = PCTR−1c

41 / 64


Continuous-time system dynamics and measurement:

x = Ax + Bu + w

y = Cx + v

w ∼ (0,Qc)

v ∼ (0,Rc)

Continuous-time Kalman filter equations:

x(0) = E [x(0)]

P(0) = E [(x(0)− x(0))(x(0)− x(0))T ]

K = PCTR−1c

˙x = Ax + Bu + K (y − Cx)

P = −PCTR−1c CP + AP + PAT + Qc

What if y includes the input also? That is, y = Cx + Du?

42 / 64

Example: Regenerative Knee Prosthesis

43 / 64

System Equations

44 / 64

Input Torque M

D. Winter, Biomechanics and Motor Control of Human Movement, 4th Edition, Wiley, 2009, Appendix A

www.wiley.com/WileyCDA/WileyTitle/productCd-0470398183.html

bcs.wiley.com/he-bcs/Books?action=resource&bcsId=5453&itemId=0470398183&resourceId=19492

45 / 64

State Equations

x1 = φk

x2 = φk

x3 = iA

x4 = vC

JT = JG + n2JM

x =

0 1 0 0

−K/JT 0 −na/JT 0

0 an/LA −RA/LA −u/LA

0 0 u/C 0

x +

0

1/JT

0

0

MK + w

y =[

1 0 0 0]x + v

Matlab program: RegenerationKalman.m

46 / 64

Outline

Linear Systems

Probability



The Kalman Filter



47 / 64


Continuous-time system dynamics and measurement:

x = Ax + Bu + f + w

y = Cx + v

w ∼ (0,Qc), v ∼ (0,Rc)

Consider f as a state:

z =[xT f T

]Tz =

[A I0 0

]z +

[B0

]u +

[ww ′

]y =

[C 0

]z +

[v0

]w ∼ (0, Q), Q = diag(Qc ,Q

′)

v ∼ (0, R), R = diag(Rc , 0)

w ′ is fictitious process noise, and Q ′ is a tuning parameter

48 / 64

Unknown Input Estimation: Rowing Machine

θ = position, ω = velocity, q = capacitor charge

k = spring constant, J = inertia, a = motor constant

R = resistance, u = power converter ratio, C = capacitance

r = radius, φ = friction

49 / 64


System model:

θ = ω

ω = −k

Jθ − a2

RJω +

au

RCJq +

r

JF − φ(θ, ω)

J

q =au

Rω − u2

RCq, φ(·, ·) = 0.12sign(ω)

State space model, assuming ω > 0:

x =

0 1 0 0

−k/J −a2/RJ au/RCJ r/J

0 au/R u2/RC 0

0 0 0 0

x +

0

−0.12

0

0

+ w

w ∼ (0,Q), Q = diag[

q1, q2, q3, q4

]

y = Cx + v =

1 0 0 0

0 1 0 0

0 0 1 0

x + v

v ∼ (0,R), R = diag[

0.012, 0.012, (0.01C)2]

(q = CV )50 / 64


Q = diag([0.012, 0.012, 12, 12]) - not responsive enough51 / 64


Q = diag([0.012, 0.012, 12, 100002]) - too responsive52 / 64


Q = diag([0.012, 0.012, 12, 1002]) - just about right53 / 64


How can we improve our results?

We have modeled F as a noisy constant: F = w

Instead we can model F as a ramp:

F = Fv + w1

Fv = w2

This increases the number of states by 1 but gives the Kalmanfilter more flexibility to estimate a value for F that matchesthe measurements

RMS force estimation error decreases from 0.8 N to 0.4 N

54 / 64

Outline

Linear Systems

Probability



The Kalman Filter



55 / 64

Nonlinear Kalman Filtering

Nonlinear system:

x = f (x , u,w , t)

y = h(x , v , t)

w ∼ (0,Q)

v ∼ (0,R)

Linearization:

x ≈ f (x0, u0,w0, t) +∂f

∂x

∣∣∣∣0

(x − x0) +∂f

∂u

∣∣∣∣0

(u − u0) +

∂f

∂w

∣∣∣∣0

(w − w0)

= f (x0, u0,w0, t) + A∆x + B∆u + L∆w

y ≈ h(x0, v0, t) +∂h

∂x

∣∣∣∣0

(x − x0) +∂h

∂v

∣∣∣∣0

(v − v0)

= h(x0, v0, t) + C∆x + M∆v

56 / 64

Nonlinear Kalman Filtering

x0 = f (x0, u0,w0, t)

y0 = h(x0, v0, t)

∆x = x − x0

∆y = y − y0

∆x = A∆x + Lw

= A∆x + w

w ∼ (0, Q), Q = LQLT

∆y = C∆x + Mv

= C∆x + v

v ∼ (0, R), R = MRMT

We have a linear system with state ∆x and measurement ∆y

57 / 64

The Linearized Kalman Filter

System equations:

x = f (x , u,w , t), w ∼ (0,Q)

y = h(x , v , t), v ∼ (0,R)

Nominal trajectory:

x0 = f (x0, u0, 0, t), y0 = h(x0, 0, t)

Compute partial derivative matrices:

A = ∂f /∂x |0 , L = ∂f /∂w |0 ,C = ∂h/∂x |0 ,M = ∂h/∂v |0Compute Q = LQLT , R = MRMT , ∆y = y − y0Kalman filter equations:

∆x(0) = 0,P(0) = E[(∆x(0)−∆x(0))(∆x(0)−∆x(0))T

]∆ ˙x = A∆x + K (∆y − C∆x),K = PCT R−1

P = AP + PAT + Q − PCT R−1CP

x = x0 + ∆x58 / 64


Combine the x0 and ∆ ˙x equations:

x0 + ∆ ˙x = f (x0, u0,w0, t) + A∆x + K [y − y0 − C (x − x0)]

Choose x0(t) = x(t), so ∆x(t) = 0 and ∆ ˙x(t) = 0

Then the nominal measurement becomes

y0 = h(x0, v0, t)

= h(x , v0, t)

and the first equation above becomes

˙x = f (x , u,w0, t) + K [y − h(x , v0, t)]

59 / 64


System equations:

x = f (x , u,w , t), w ∼ (0,Q)

y = h(x , v , t), v ∼ (0,R)


A = ∂f /∂x |x , L = ∂f /∂w |x ,C = ∂h/∂x |x ,M = ∂h/∂v |x

Compute Q = LQLT , R = MRMT

Kalman filter equations:

x(0) = E [x(0)], P(0) = E[(x(0)− x(0))(x(0)− x(0))T

]˙x = f (x , u, 0, t) + K [y − h(x , 0, t)] , K = PCT R−1


60 / 64

Robot State Estimation

Robot dynamics:

u = Mq + Cq + g + R + F

u = control forces/torques, q = joint coordinatesM(q) = mass matrix, C (q, q) = Coriolis matrixg(q) = gravity vector, R(q) = friction vectorF (q) = external forces/torquesState space model:

q = M−1(u − Cq − g − R − F )

x =[q1 q2 q3 q1 q2 q3

]T=[xT1 xT2

]Tx =

[x2

M−1(u − Cq − g − R − F )

]= f (x , u,w , t)

y = q3 =[

0 0 1 0 0 0]x + v = h(x , v , t)

The detailed model is available at:www.sciencedirect.com/science/article/pii/S0307904X14003096

dynamicsystems.asmedigitalcollection.asme.org/article.aspx?articleid=1809665

61 / 64

Robot State Estimation

System equations:

x = f (x , u,w , t), w ∼ (0,Q)

y = h(x , v , t), v ∼ (0,R)


A = ∂f /∂x |x , L = ∂f /∂w |x ,C = ∂h/∂x |x ,M = ∂h/∂v |x

Compute Q = LQLT , R = MRMT

Kalman filter equations:

x(0) = E [x(0)], P(0) = E[(x(0)− x(0))(x(0)− x(0))T

]˙x = f (x , u, 0, t) + K [y − h(x , 0, t)] , K = PCT R−1


62 / 64

Robot State Estimation: Robot.zip

First we write a simulation for the dynamic system model:simGRF.mdl and statederCCFforce.m

Then we write a controller: PBimpedanceCCFfull.m

Then we calculate the A matrix: CalcFMatrix.m andEvaluateFMatrix.m

Then we write a Kalman filter: zhatdot.m

Run the program:

Run setup.m

Run simGRF.mdlLook at the output plots:

Run plotter.m to see control performanceOpen “Plotting - 1 meas” block to see estimator performanceOpen the “q1, q1hat” scope to see hip positionOpen the “q2, q2hat” scope to see thigh angleOpen the “q3meas, q3, q3hat” scope to see knee angle

63 / 64

Additional Estimation Topics

Nonlinear estimation

Iterated EKFSecond-order EKFUnscented Kalman filterParticle filterMany others ...

Parameter estimation

Smoothing

Adaptive filtering

Robust filtering (H∞ filtering)

Constrained filtering

64 / 64

a crash course on kalman filtering - cleveland state university · 2014-09-18 · a crash course on...

Documents