Optimal State Estimation , Sensor Fusion Study - Ch5. The discrete-time Kalman filter

1. 2020 AI Robotics KR
Sensor Fusion Study
Ch5. The discrete-time
Kalman Filter
박정은

2. Overview
1. Mathematical description of a dynamic system whose state we want to estimate
2. Implement equations that describe how the mean and variance of the state and
the covariance of the state propagate with time. These equations, derived in Ch.4
themselves form a dynamic system.
3. We take dynamic systems that describes the propagation of the state mean and
covariance, and implement equations on a computer.
(a) The mean of the state is the Kalman filter estimate of the state
(b) The covariance of the state is the covariance of the Kalman filter state estimate
4. Every time that we get a measurement, we update the mean and covariance of
the state.

3. 5-1. Derivation of the Discrete-Time Kalman Filter
<Assumtion>
Linear discrete-time system
(5.1)
{w_k},{v_k} white,zero mean, uncorrelated, known variane Q_k, R_k
참고 (2.96), (2.85)
(5.2)

4. 5-1. Derivation of the Discrete-Time Kalman Filter
<Types of Estimates>
Figure 5.1
smoothed estimate: k+N개의 관찰 값을 이용해 k번째 값을 예측
predicted estimate: k-m개의 관찰 값을 이용해 k번째 값을 예측

5. 5-1. Derivation of the Discrete-Time Kalman Filter
<Priori, Posteriori>
Initial estimate x0
(5.8) reasonable!
P_k : covariance of estimation error
Figure 5.2
(k-1) time
Estimation 𝑥 𝑘−1
+
Covariance of estimate 𝑃𝑘−1
+
(k) time
Compute Estimation 𝑥 𝑘
−
Covariance of estimate 𝑃𝑘
−
(k) time
Measurement
Resulting estimate 𝑥 𝑘
+
Covariance of estimate 𝑃𝑘
+

6. 5-1. Derivation of the Discrete-Time Kalman Filter
<Time update equation for 𝑥 >
Best estimate of Initial state x0 (assumption?)
𝑤𝑒 𝑤𝑎𝑛𝑡 𝑡𝑜 𝑠𝑒𝑡: 𝑥1
−
= 𝐸(𝑥1)
위의 식에 따라서
좀 더 일반적으로
Time update equation for 𝑥. Do not need additional Measurement

7. 5-1. Derivation of the Discrete-Time Kalman Filter
<Time update equation for P>
Best estimate of Initial state P0+
If we know initial state perfect P=0, no idea the P is infinity
Represents uncertainty in our initial estimate of x0
Want to estimate 𝑃1
−
에 따라서 일반적인 식으로
Time update equation for P

8. 5-1. Derivation of the Discrete-Time Kalman Filter
<Meausrement update equations>
Given 𝑥 𝑘
−
->estimate 𝑥 𝑘
+
Takes measurement of 𝑦 𝑘
-recursive least squares development from section 3.3

9. 5-1. Derivation of the Discrete-Time Kalman Filter
<Meausrement update equations>
Replacement!

10. 5-1. Derivation of the Discrete-Time Kalman Filter
<Summary>
First expression for Pk+: Joseph stabilized version of the covariance measurement update equation
always be symmetric positive definite
Third expression for Pk+: simpler than first one
does not guarantee symmetry or positive definitess
Second expression for Pk+: rarely used . But useful for information filter
Second expression for Kk: we need expression for pk+ that does not depend on Kk
Calculation of 𝑃𝑘
+
, 𝐾𝑘, 𝑃𝑘
−
does not depend on the measurement yk
-> Kalman Gain Kk can be calculated offline before the system operates and saved in memory
-> 𝑥 𝑘 equation need to be implemented in real time
the performance of the filter can be investigated and evaluated before the filter fun

11. 5-2. Kalman Filter Properties
We want to find a causal filter that results in a state estimate 𝑥 𝑘
Error between true state and estimated state 𝑥 𝑘 = 𝑥 𝑘 − 𝑥 𝑘
Objective -> min 𝐸[𝑥 𝑘
𝑇
𝑆 𝑘 𝑥 𝑘] (minimize weighted two-norm)
(S is a positive definite user-defined weighting matrix)
-> optimal linear filter in any case

12. 5-3. One-step Kalman Filter Equations
① 𝑥 𝑘+1
−
in expression for 𝑥 𝑘
−
② 𝑃𝑘+1
−
in expression for 𝑃𝑘
−
discrete Riccati equation
③ 𝑥 𝑘+1
+
in expression for 𝑥 𝑘
+
, 𝑃𝑘+1
+
in expression for 𝑃𝑘
+

13. Example 5.1
System dynamics
𝑥 𝑘+1 = 𝐹𝑥 𝑘
Kalman filter for system
covariance increases between time

14. Example 5.1
After measurement?
covariance
decreases!

15. Example 5.1
Overall graph

16. 5.4 Alternative Propagation of Covariance
1) Multiple state systems
-Used to find a closed form equation for scalar Kalman filter
-Used to find a fast solution to the steady-state estimation-error covariance
Recall the equation
Factorize n*n matrix Pk- where A and B are n*n matrix to be determined
Then A and B are propagated as follows

17. 5.4 Alternative Propagation of Covariance
1) Multiple state systems
<Proof>
From above,
𝐴 𝑘+1 = 𝐹𝑘 + 𝑄 𝑘 𝐹𝑘
−𝑇
𝐻 𝑘 𝑅 𝑘
−1
𝐻 𝑘 𝐴 𝑘 + 𝑄 𝑘 𝐹𝑘
−𝑇
𝐵 𝑘
정리하면,
Matrix inverse Lemma를 사용하면 𝐴 = 𝐼, 𝐵 = −𝐻 𝑘, 𝐷 = 𝑅 𝑘, 𝐶 = 𝐻 𝑘 𝑃𝑘
−

18. 5.4 Alternative Propagation of Covariance
1) Multiple state systems
<Steady State Kalman Gain>
𝐴1 = 𝑃1
−1
, 𝐵1 = 𝐼, Ψ2𝑝
가 수렴 할 때 까지 곱하면, 아래와 같다. (왜 2p?)
Steady State Covariance가 𝑃∞
−
= 𝐴∞ 𝐵∞이므로 steady state Kalman Gain을 구할 수 있다.
𝐾∞ = 𝑃∞
−1 𝐻 𝑇 𝐻𝑃∞
−1 𝐻 𝑇 + 𝑅 −1

19. 5.4 Alternative Propagation of Covariance
2) Scalar systems
-Used to find a closed form equation for scalar Kalman filter
-Used to find a fast solution to the steady-state estimation-error covariance
Suppose that F,Q,H,R are constant scalars
Eigen values 𝜆1, 𝜆2, Eigen vector matrix M
𝐴1 = 𝑃1
−1
, 𝐵1 = 𝐼

20. 5.4 Alternative Propagation of Covariance
2) Scalar systems
열심히 잘 계산하면…
Steady State Value of 𝑃𝑘
−
(𝜇2 < 𝜇1 이므로 k가 증가 할 때 𝜇2
𝑘
가 더 빨리 감소)

21. Example 5.2
(Scalar System)
F=H=Q=R=1 -> for Priori Estimation Covariance and Kalman Gain
Posteriori Estimation Covariance : 𝑃𝑘
+
= 𝐼 − 𝐾𝑘 𝐻 𝑘 𝑃𝑘
−

22. 5.5 Divergence Issues
In real system it may NOT work!
① finite precision arithmetic : only a certain number of bits are used to represent
Kalman filter Equations (유한 정밀도 산술)
② Modeling errors:
-model may not be Precisely known
-Noise is may not be pure white with zero mean, completely uncorrelated

23. 5.5 Divergence Issues
<Remedy>
1. Increase arithmetic precision
-메모리를 더 할당해!
-simply forces the digital implementation of the filter to more closely match the analog theory
-OK for PC, not good for microcontroller
2. Square Root Filtering
-effectively increases arithmetic precision
-but needs more computational efforts

24. 5.5 Divergence Issues
<Remedy>
3. Symmetrize P at each time step P=(P+PT)/2
4. Initialize P approximately to avoid changes in P
-do not result in major improvements
-can prevent numerical problems easily
-𝑃𝑘
−
𝑖𝑠 𝑎𝑙𝑤𝑎𝑦𝑠 𝑠𝑦𝑚𝑚𝑒𝑡𝑟𝑖𝑐, 𝑃𝑘
+
may not always be symmetric
-mathematically equivalent but not numerically
-𝑃𝑘
+ 𝑛𝑒𝑤
= (Pk
+
+ Pk
+T
)/2
-다른 방법도 있음 책에 두가지 방법 더 소개

25. 5.5 Divergence Issues
<Remedy>
5. Use a fading memory filter
-forget the measurements in the distant past and place more emphasis on recent measurements
-theoretically results in loss of optimality
-restore convergence and stability
-work due to modeling error
6. The use of fictitious process noise
-mathematically equivalent to the 5
-less confidence on system model, emphasis on measurements

26. Example 5.3
<Fictitious Noise>
assume process noise 0
앞선 식 들에 따라 계산하면,
하지만!
True system은 다음과 같다

27. Example 5.3
<Fictitious Noise>
Estimation of 𝑥1,𝑘 를 비교해보면, 발산하는 것을 볼 수 있다.
만약 fictitious process noise를 추가한다면, 관찰 값에
더 가중치를 것이다.
물론 모델이 완벽하다면 성능이 떨어지겠지…
if Q becomes larger, filter will be more responsive to
measurements
오른쪽의 그래프를 보면 Q=1 일 때 가장 실재와 비슷함

28. Example 5.3
<Fictitious Noise>
가장 좋은 Q값은 모델에 따라 달라진다.
일반적으로 process noise는 좋지만 노이즈가 너무 많아지면 state를 추정하기 어려워진다.
Need to balance our confidence in our model.

29. 5.5 Divergence Issues
Fictitious process noise compensates for modeling error
<If Qk is small>
In example 3
-𝑄 𝑘 = 0 이므로 𝐹𝑘 = 1, 𝑃𝑘
−
= 𝑃𝑘
+
. Measurement 가 진행되므로 𝑃𝑘
−
> 𝑃𝑘
+
-𝑃𝑘
−
는 0으로 수렴한다.
-𝐾𝑘는 0으로 수렴한다.
-measurement is completely ignored!
-measurement noise covariance R will be infinitely larger than process noise Q

30. 5.5 Divergence Issues
Fictitious process noise compensates for modeling error
<If Qk is large>
-Measurement 가 진행되므로 𝑃𝑘
−
> 𝑃𝑘
+
-P는 큰 어떠한 값으로 수렴하게 된다.
-K는 어떠한 큰 값으로 수렴하게 된다.
-larger K means that measurement equation will include a larger emphasis on the measurement
Q값을 적절하게 정해야 한다.

31. -끝-
Ch5. The discrete-time
Kalman Filter
박정은