Kalman filter Properties and Applications - TSRT78 Digital ... ·...

Summary of lecture 9 (I/II)

� Optimal estimation: For the state space model

x(t + 1) =Ax(t) + w(t)

y(t) =Cx(t) + v(t)

use the measurements y(0); : : : ; y(s) to compute a state estimatex̂(tjs) such that the covariance of the estimation errorx̃(tjs) = x(t) � x̂(tjs) is minimized.

� Kalman filter: Linear filter that solves the optimal estimationproblem by iterating a time update (prediction) and a measurementupdate (correction),

� � �pred�!

(x̂(tjt � 1)P(tjt � 1)

corr�!

(x̂(tjt)P(tjt)

pred�!

(x̂(t + 1jt)P(t + 1jt)

corr�! � � �

F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 1 / 17

Summary of lecture 9 (II/II)

� Input: Models A, C, Q, R, measurements y(0); : : : ; y(N), and initialstate estimate x0 and covariance P0.

� Recursion: For each time step t, iterate� Prediction (Time Update):

x̂(t + 1jt) =Ax̂(tjt) Use model to propagate

P(t + 1jt) =AP(tjt)AT + Q Increase uncertainty

� Correction (Measurement Update):

e(tjt � 1) =y(t) � Cx̂(tjt � 1) Innovation

S(t) =CP(tjt � 1)CT + R Innovation covariance

K (t) =P(tjt � 1)CTS�1(t) Kalman gainx̂(tjt) =x̂(tjt � 1) + K (t)e(tjt � 1) Correct estimateP(tjt) =P(tjt � 1) � K (t)CP(tjt � 1) Decrease uncertainty


Outline Lecture 10

Kalman filter – properties and applications1 Observability2 Known input signal3 The stationary Kalman filter4 Frequency properties of the Kalman filter5 Using the Kalman filter in practice6 Non-linear variants of the Kalman filter


Kalman filter with known input: example� Model in Lec. 9, slide 9, with known input u(t) = [ax(t) ay (t)]T ,

x(t + 1) = Ax(t) +h02 TI2

iTu(t) + w(t)

0 20 40 60 80 100 120

−10

0

10

20

30

40

50

60

70

80

90

100

Measurements

x

y

x(t)y(t)

0 20 40 60 80 100 120

−10

0

10

20

30

40

50

60

70

80

90

100

Result

x

y

x(t)x̂(t|t) w/o u(t)x̂s(t|t) w u(t)

0 20 40 60 80 100 120

−10

0

10

20

30

40

50

60

70

80

90

100

Result

x

y

x(t)x̂(t|t) w/o u(t)x̂s(t|t) w u(t)

� Compare KF with known input (KF1) and without known input(KF2). KF2 cannot match KF1 when both have equal Q (center).

� If we increase Q for KF2 by a factor 20 it can match KF1 in speed,but the “price” is an increased uncertainty P(tjt) (right).

� Position estimates in navigation usually more accurate than intracking.


Stationary Kalman predictor and filter

� Input: Models A, C, Q, R, measurements y(0); : : : ; y(N), and initialstate estimate x0 and covariance P0.

� Stationary prediction covariance: dlqe, dare

P̄p = AP̄pAT � AP̄pC

T�CP̄pC

T + R��1

CP̄pAT + Q| {z }

Stationary Riccati equation

� Stationary gain and filter covariance:

K̄ = P̄pCT�CP̄pC

T + R��1; P̄f = P̄p � K̄CP̄p

� Stationary predictor: x̂(t + 1jt) = (A� AK̄C )x̂(tjt � 1) + AK̄y(t)

� Stationary filter: x̂(tjt) = (A� K̄CA)x̂(t � 1jt � 1) + K̄y(t)

The computational cost is significantly lower compared to the full KF.


Stationary Kalman filter: exampleStationary Kalman filter for the target tracking example in Lecture 9.Different measurement realization, position initialized in origin.

P̄p =

266646:76 0 3:28 00 6:76 0 3:28

3:28 0 2:56 00 3:28 0 2:56

37775

P̄f =

266642:51 0 1:22 00 2:51 0 1:22

1:22 0 1:56 00 1:22 0 1:56

37775

K̄ =

266640:63 00 0:630:3 00 0:3

37775

−10 0 10 20 30 40 50 60−10

0

10

20

30

40

50

60

Result

x

y

x(t)x̂(t|t)x̂s(t|t)P(t|t)P̄f

For this example the full KFconverges quickly to thestationary KF.


KF frequency properties: Stationary KF example 1Same model as Lec 7, slides 5–7, and Lec 9, slides 11–12.

State space model:

x(t) =hpx(t) py(t)

iTA =

"1 00 1

#; Q =

"T 2Q̄ 00 T 2Q̄

#

C =

"1 00 1

#; R =

"R̄ 00 R̄

#10

−2

100

10−2

10−1

100

To:x1

From: y1

10−2

100

10−2

10−1

100

To:x2

ω [rad/s]

10−2

100

10−2

10−1

100

From: y2

10−2

100

10−2

10−1

100

ω [rad/s]

Q/R = 4Q/R = 0.2Q/R = 0.01

� For this example: Low pass filtering effect (compare Lec 7, slide 6).� For this example: Measurements of y -position (y2) has no effect on

estimate of x-position (x1), and vice versa.


KF frequency properties: Stationary KF example 2Same model as Lec 9, slide 9.

x(t) =

26664x(t)y(t)�x(t)�y(t)

37775

� Low pass filteringeffect on positionestimates.

� Approx band passfiltering effect onvelocity estimates.

10−2

100

10−2

100

To:x1

From: y1

10−2

100

10−2

100

To:x2

10−2

100

10−2

100

To:x3

10−2

100

10−2

100

To:x4

ω [rad/s]

10−2

100

10−2

100

From: y2

10−2

100

10−2

100

10−2

100

10−2

100

10−2

100

10−2

100

ω [rad/s]

Q/R = 4Q/R = 0.2Q/R = 0.01

� For this example: Measurements of y -position (y2) has no effect onestimate of x-position and -velocity (x1 and x3), and vice versa.


KF frequency properties: Matlab

nx = size(A,1);ny = size(C,1);[Kbar,Pp,Pf]=dlqe(A,eye(nx),C,Q,R);% Stationary filtersys = ss(A-Kbar*C*A,Kbar,eye(nx),zeros(nx,ny),-1);figurefor iy = 1:ny

[num,den]=ss2tf(sys.A,sys.B,sys.C,sys.D,iy);for ix = 1:nx

[H,W]=freqz(num(ix,:),den,100);subplot(nx,ny,ny*(ix-1)+iy)loglog(W,abs(H))

endend% Stationary predictorsys = ss(A-A*Kbar*C,A*Kbar,eye(nx),zeros(nx,ny),-1);


Standard motion modelsStandard linearized motion models for n-D motion. Let p(t) denote theposition X (1-D), X and Y (2-D), or X , Y and Z (3-D).

State vector Motion model

x(t) = p(t) x(t + T ) = x(t) + Tw(t)

x(t) =

"p(t)v(t)

#x(t + T ) =

"In TIn0n In

#x(t) +

"T 2

2 InTIn

#w(t)

x(t) =

264p(t)v(t)a(t)

375 x(t + T ) =

264 In TIn

T 2

2 In0n In TIn0n 0n In

375 x(t) +

264T 3

6 InT 2

2 InTIn

375w(t)


Kalman filter: implementation issues

� Implementations of the Kalman filter can lead to numerical issues.� Covariance matrix is not symmetric: One remedy is to replace with

0:5(P + PT ).

P=0.5*(P+P’);

� Covariance matrix is not positive definite: One remedy is tocompute a Singular Value Decomposition (SVD) P = UDV T andreplace negative singular values by either zero or a small number �.

[U,S,V]=svd(P);S(S<0)=mu;P=U*S*V’;

� See also Square Root Implementation in the course book (Section8.6 on page 319).


The Extended Kalman Filter (EKF)� Input: Models f (x(t)), h(x(t)), Q, R, measurements y(0); : : : ; y(N),

and initial state estimate x0 and covariance P0.� Recursion: For each time step t, iterate

� Prediction:

x̂(t + 1jt) =f (x̂(tjt)) Use model to propagate

P(t + 1jt) =F (t)P(tjt)FT (t) + Q Increase uncertainty

� Correction:

e(tjt � 1) =y(t) � h (x̂(tjt � 1)) Innovation

S(t) =H(t)P(tjt � 1)HT (t) + R

K (t) =P(tjt � 1)HT (t)S�1(t) Kalman gainx̂(tjt) =x̂(tjt � 1) + K (t)e(tjt � 1) Weighted averageP(tjt) =P(tjt � 1) � K (t)H(t)P(tjt � 1) Decrease uncertainty

� Gradients:

F (t) =df (x)

dx

��x=x̂(tjt)

; H(t) =dh(x)

dx

��x=x̂(tjt)


Peer review

� Detailed information on course homepage,http://www.control.isy.liu.se/student/tsrt78/lab1.html

� Create one author and one reviewer account per group

� Upload pdf (max 2MB)

� The Keycode to sign up as a reviewer is tsrt78reviewer.

� Don’t hesitate to ask if you have any questions regarding the lab, thereport, the review process, or anything else!


MC leaning angle� Headlight steering, ABS and anti-spin systems require leaning angle.� Gyro very expensive for this application.� Combination of accelerometers investigated, lateral and downward acc

worked fine in EKF.Model, where zy ; zz ; a1; a2; J are constants relating to geometry andinertias of the motorcycle, u = vx

x = ('; '̇; '̈; ̇; ̈; �ay ; �az ; �'̇)T ;

y = h(x) =

ayaz'̇

!=

ux4 � zyx3 + zyx2

4 tan(x1) + g sin(x1) + x6�ux4 tan(x1)� zz

�x22 + x2

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

�a1x3 + a2x24 tan(x1)� ux4J + x6

!

Animation of a test drive:http://youtu.be/hT6S1FgHxOc?list=UUccDBFGV7O3Q3HK_mjSIT6g


http://youtu.be/hT6S1FgHxOc?list=UUccDBFGV7O3Q3HK_mjSIT6g

Sounding rocketNavigation grade IMU gives accurate dead-reckoning, but drift may causereturn at bad places.GPS is restricted for high speeds and high accelerations.Fusion of IMU and GPS when pseudo-ranges are available, with IMUsupport to tracking loops inside GPS.� Loose integration: direct fusion approach yk = pk + ek .� Tight integration: TDOA fusion approach y ik = kpk � pikk=c + tk + ek .

Illustration of state estimates from a real launch:http://youtu.be/zRHFXfZLQ64


http://youtu.be/zRHFXfZLQ64

Virtual yaw rate sensor� Yaw rate subject to bias, orientation error increases linearly in time.� Wheel speeds also give a gyro, where the orientation error grows

linearly in distance,

Model, with state vector xk =� ̇k ; ̈k ; bk ;

rk;3rk;4

�,

y1k = ̇k + bk + e1

k ;

y2k =

!3rnom + !4rnom

22B

!3!4

rk;3rk;4

� 1!3!4

rk;3rk;4

+ 1+ e2

k :

Test in Valla-rondellen: http://youtu.be/d9rzCCIBS9I


http://youtu.be/d9rzCCIBS9I

Summary of Lecture 10

� Stationary Kalman filter: Time invariant state space models willconverge to stationary covariances and Kalman gain. Solution providedby the stationary Riccati equation.

� Frequency properties of KF: The stationary Kalman filters havefrequency selective characteristics.

� Motion model: A type of dynamic model, describes motion of e.g. apedestrian or a car.

� Extended Kalman Filter: A Kalman filter for state space modelswith non-linear dynamic model and/or non-linear measurement model.Approximates a solution using first order Taylor expansion.


Kalman filter Properties and Applications - TSRT78 Digital ... ·...

Documents

Transcript of Kalman filter Properties and Applications - TSRT78 Digital ... ·...