Optimal Estimation Sensor Fusion Study - Ch6. Alternate Kalman filter formulations [Jinhyuk Song]

1. AI Robotics KR study
Sensor Fusion Study
Ch6. Alternative Kalman FILTER formulations

2. Alternative Kalman filters overview
1. Sequential Kalman filter 2. Information filtering
4. U-D filtering3. Square root filtering
𝜎2
𝑆 = 𝜆 𝑆 𝑇
𝑆
𝑃 = 𝑆𝑆 𝑇
𝑃 = 𝑈𝐷𝑈 𝑇(U-D factorization)

3. Sequential Kalman filter overview
𝑅 𝑘 =
𝑅1𝑘 ⋯ 0
⋮ ⋱ ⋮
0 ⋯ 𝑅 𝑟𝑘
<Comparison on Kalman filter and sequential Kalman filter>
식의 변수들이 전부 scalar로
표현되므로 matrix
inversion이 필요하지 않음.
Assume that

4. Sequential Kalman filter process
1. 아래와 같은 dynamic system이 주어졌다고 가정하자.
2. Kalman filter initialization
3. Time-update equations
4. Measurement-update equation
4-1) priori estimate and covariance initialization
4-2) 1부터 r까지 반복적으로 계산을 수행.
4-3) 1부터 r까지 반복적으로 계산을 수행.

5. Sequential Kalman filter process
R이 diagonal하지 않다면 대각화. 아래의 식에서 yk 대신에 𝑦 𝑘, Hk 대신에 𝐻 𝑘, 𝑅을 사용.
위와 같은 R이 time-varying하다면 대각화를 위해 매번 많은 양의 계산이 필요함. R이 constant하다면
이 과정에서 필터가 작동하기 전에 offline으로 Jordan 분해가 가능함.
정리하자면 sequential Kalman filter는 다음의 두가지 조건에서 사용하기에 좋음.
1. Measurement noise covariance인 𝑅 𝑘가 diagonal.
2. Measurement noise covariance인 𝑅 𝑘이 상수로 주어짐.

6. Information filtering derivation
이 방식은 P가 아닌 P의 역행렬을 propagation하는 방식임.
P→0, I→∞ means perfect knowledge of x,
P→∞, I→0 means zero knowledge of x
𝑃𝑘
+
= (𝑃𝑘
−
)−1 + 𝐻 𝑘
𝑇
𝑅 𝑘
−1
𝐻 𝑘
−1
(measurement update equation)
𝑃𝑘
−
= 𝐹𝑘−1 𝑃𝑘−1
+
𝐹𝑘−1
𝑇
+ 𝑄 𝑘−1 (time-update equation)
From kalman filter equations…

7. Information filter process
1. 아래와 같은 dynamic system이 주어졌다고 가정하자.
3. 각 time step에 대해 equation 적용.
2. Kalman filter initialization
r ≫ 𝑛일 때에 계산적인 면에서 information
filter가 기존 칼만 필터에 비해 효율적임.
Initial uncertainty→∞, 𝑃0
+
= ∞, 𝐼0
+
= 0
Initial uncertainty→0, 𝑃0
+
= 0, 𝐼0
+
= ∞
𝐾𝑘 = 𝑃𝑘
+
𝐻 𝑘
𝑇
𝑅 𝑘
−1
r x r inversion?

8. Square root filtering derivation
Condition number: 𝜅 𝑃 =
𝜎 𝑚𝑎𝑥 𝑃
𝜎 𝑚𝑖𝑛 𝑃
≥ 1
Singular value: 𝜎2
𝑃 = 𝜆 𝑃 𝑇
𝑃 = 𝜆(𝑃𝑃 𝑇
)
P 행렬이 n x n 행렬로 n개의 singular value 𝜎를 갖는다면,
𝜅 𝑃 → ∞, poorly(ill) conditinoned, P 가 singular matrix가 됨.
Basic idea of square filtering: 𝑃 = 𝑆𝑆 𝑇
(ex)

9. Square root filtering – time update
n-state discrete LTI system,
𝑆 𝑘−1
+
가 𝑃𝑘−1
+
의 square root이므로 아래 식이 성립.

10. Square root filtering – measurement update
식 (5.19)에서부터 유도된 measurement update equation
6.1에서 유도된 sequential Kalman filter 식을 이용. P를 square root로 치환한다면 우측의 식을 얻음.

11. Measurement-update algorithm
1. Initialize.
2. 각 measurement에 대해 과정을 반복.
2-1) 𝐻𝑖𝑘, 𝑦𝑖𝑘, 𝑅𝑖𝑘를 정의.
2-2) 𝑖째 계산이 process된 후에 다음의 값들을 계산.
2-3) Kalman gain을 계산.
2-4) i번째 measurement에 대한 state estimate를 계산.
3. Posteriori estimate와 covariance square root를 업데이트

12. Alternate method - triangularization
Find orthogonal (n+r) x (n+r)matrix 𝑇
=
𝑇를 찾을 수 있다면, 𝑃𝑘
+
의 square root를 찾을 수 있음.

13. Orthogonal transformation algorithms
1. Householder algorithm
1-1) 𝑇𝐴 1
=
𝑊
0
, W를 찾는다.
1-2) 1부터 n까지 다음의 과정을 반복.
(a) (b)
(c) 1부터 2n까지 반복. (d) 1부터 n까지 반복.
(e) 2n x n 행렬을 계산.
1-3) 𝐴 𝑛+1
=
𝑊
0
2. Modified Gram-Schmidt algorithm
2-1) 𝑇𝐴 1 =
𝑊
0
, W를 찾는다.
2-2) 1부터 n까지 다음의 과정을 반복.
(a) (b)
(c)
(d) If (k<n),

14. U-D filtering derivation (measurement)
𝑃 = 𝑈𝐷𝑈 𝑇
(U-D factorization)
𝑃𝑘
+
= 𝐼 − 𝐾𝑘 𝐻 𝑘 𝑃𝑘
−
(measurement update equation)
𝑃𝑘
−
= 𝐹𝑘−1 𝑃𝑘−1
+
𝐹𝑘−1
𝑇
+ 𝑄 𝑘−1 (time-update equation)
where 𝐾𝑘 = 𝑃𝑘
−
𝐻 𝑘
𝑇
𝐻 𝑘 𝑃𝑘
−
𝐻 𝑘
𝑇
+ 𝑅 𝑘
−1
𝑈𝑖 = 𝑈𝑖−1 𝑈
𝐷𝑖 = 𝐷

15. U-D filtering derivation (time-update)
𝑃𝑘
+
= 𝐼 − 𝐾𝑘 𝐻 𝑘 𝑃𝑘
−
(measurement update equation)
𝑃𝑘
−
= 𝐹𝑘−1 𝑃𝑘−1
+
𝐹𝑘−1
𝑇
+ 𝑄 𝑘−1 (time-update equation)
where 𝐾𝑘 = 𝑃𝑘
−
𝐻 𝑘
𝑇
𝐻 𝑘 𝑃𝑘
−
𝐻 𝑘
𝑇
+ 𝑅 𝑘
−1
𝑈−
𝐷−
𝑈− 𝑇
= 𝑊 𝐷𝑊 𝑇
𝑊 𝑇 = [𝑤1
𝑇
⋯ 𝑤 𝑛
𝑇]

16. U-D filtering process – measurement
1. Start with a priori estimation 𝑃0 = 𝑃−
2. 각 measurement에 대해 과정을 반복.
2-1) 𝐻𝑖, 𝑅𝑖를 정의. 𝛼𝑖 = 𝐻𝑖 𝑃𝑖−1 𝐻𝑖
𝑇
+ 𝑅𝑖
2-2) Ui−1, Di−1을 구하기 위해 𝑃𝑖−1의 factorization
2-3)
2-4) 𝑈𝑖와 𝐷𝑖를 구한다.
𝑈𝑖 = 𝑈𝑖−1 𝑈
𝐷𝑖 = 𝐷
3. Posteriori estimation covariance
𝑃+
= 𝑈𝑟 𝐷𝑟 𝑈𝑟
𝑇
• U-D filtering 과정에서 sequential
filtering에 의존하므로 𝑅 𝑘가
diagonal하거나 constant여야 한다.

17. U-D filtering process – time update
1. Start with measurement update equation
2. 행렬을 정의.
3. 𝐷과 orthogonal한 𝑣𝑖를 만들기 위해 Gram-Schmidt
orthogonalization 진행.
𝑊 = 𝑈−
𝑉
𝑃+
= 𝑈+
𝐷+
𝑈+ 𝑇 4. 𝑣𝑖를 열로 갖는 V 행렬을 정의.
5. 𝐷을 이용해 upper triangular matrix, 𝑈−를 구한다.
6. 𝐷− = 𝑉 𝐷𝑉 𝑇
• Standard kalman filter에 비해 2배정도
정확하지만, square root filter와 비교했을
때에는 계산량이 적음.

18. Summary
• Sequential Kalman filter: matrix inversion을 피하므로 계산량과 계산 시간이 줄어듬.
(embedded system에 적절.) 하지만 noise covariance가 diagonal하거나 상수여야
사용하기 적절함.
• Information filtering: Covariance의 inverse를 이용하므로 state에 비해
measurement가 많을 때에 사용하기 적절함.
• Square root filtering과 U-D filtering: 계산량이 많지만, 정확도가 상승되기 때문에
divergence나 instability 문제를 개선할 수 있음.
• 이 방식들 외에 Kalman filter를 유도하기 위한 다른 방식들이 많이 존재함.