Kalman filter Properties and Applications - TSRT78 Digital ... ·...
Transcript of 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
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 2 / 17
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
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 3 / 17
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.
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 4 / 17
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.
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 5 / 17
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.
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 6 / 17
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.
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 7 / 17
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.
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 8 / 17
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);
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 9 / 17
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)
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 10 / 17
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).
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 11 / 17
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)
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 12 / 17
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!
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 13 / 17
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
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 14 / 17
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
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 15 / 17
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
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 16 / 17
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.
F. Gustafsson (LiU) Digital Signal Processing, Lecture 10 2015 17 / 17