卡尔曼滤波原理及实践
-
公式
x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 \hat x_{k|k-1} = F_k \hat x_{k-1|k-1} x^k∣k−1=Fkx^k−1∣k−1
P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_k Pk∣k−1=FkPk−1∣k−1FkT+Qk
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_k = P_{k|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat x_{k|k} = \hat x_{k|k-1} + K_k (z_k - H_k \hat x_{k|k-1}) x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1)
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1 -
https://github.com/balzer82/Kalman
比较清晰的kalman框架和相应的Python代码，用来理解学习很方便。
主要是:KF_CV KF_CA AKF_CV EKF_CTRV EKF_CHCV EKF_CTRA -
https://github.com/udacity/CarND-Extended-Kalman-Filter-Project.git
-
https://github.com/udacity/CarND-Unscented-Kalman-Filter-Project.git
-
https://github.com/udacity/SFND_Unscented_Kalman_Filter.git
udacity的kalman的C++框架，具体实现要补充，可在github搜相应的项目，有别人完善好的,如：https://github.com/williamhyin/SFND_Unscented_Kalman_Filter.git -
从贝叶斯滤波到卡尔曼滤波：从贝叶斯滤波理论直接推导
-
卡尔曼滤波算法详细推导：非常详细，公式推导写的很明白
-
卡尔曼滤波-*：*写的也非常好，有关于最优卡尔曼增益的简化推导
卡尔曼滤波公式详解
已知假设，卡尔曼滤波模型假设k时刻的真实状态是从（k − 1）时刻的状态演化而来，符合下式：
x
k
=
F
k
x
k
−
1
+
B
k
u
k
+
w
k
x_{k} = F_k x_{k-1} + B_ku_k + w_k
xk=Fkxk−1+Bkuk+wk
- F k F_k Fk为状态变换模型（矩阵/向量），运动学一般是矩阵（状态转移矩阵）。
- B k B_k Bk是作用在控制器向量 u k u_k uk上的输入-控制模型。一般运动学中没有这项。
- w k ∼ N ( 0 , Q k ) w_k \sim N(0,Q_k) wk∼N(0,Qk)是过程噪声。均值为0，可不写这项。
k时刻真实状态
x
k
x_k
xk的一个测量：
z
k
=
H
k
∗
x
k
+
v
k
z_k = H_k * x_k + v_k
zk=Hk∗xk+vk
-
H
k
H_k
Hk为观测模型(观测矩阵)，它把真实状态空间映射成观测空间，
v
k
∼
N
(
0
,
R
k
)
v_k \sim N(0,R_k)
vk∼N(0,Rk)是观测噪声
初始状态以及每一时刻的噪声 x 0 , w 1 , . . . , w k , v 1... v k {x_0, w_1, ..., w_k, v1 ... v_k} x0,w1,...,wk,v1...vk都认为是互相独立的
预测：
x ^ k ∣ k − 1 = F k x ^ k − 1 ∣ k − 1 \hat x_{k|k-1} = F_k \hat x_{k-1|k-1} x^k∣k−1=Fkx^k−1∣k−1
- 预测状态，状态方程。
- x ^ k ∣ k − 1 \hat x_{k|k-1} x^k∣k−1为k-1时刻对k时刻的状态预测。
- x ^ k − 1 ∣ k − 1 \hat x_{k-1|k-1} x^k−1∣k−1在时刻k-1的状态估计。
- F k F_k Fk为状态转移矩阵
P k ∣ k − 1 = F k P k − 1 ∣ k − 1 F k T + Q k P_{k|k-1} = F_k P_{k-1|k-1} F^T_k + Q_k Pk∣k−1=FkPk−1∣k−1FkT+Qk
- 预测估计协方差矩阵，先验概率
- P k − 1 ∣ k − 1 P_{k-1|k-1} Pk−1∣k−1为k-1时刻的后验估计误差协方差矩阵，度量估计值的精确程度。
- P k ∣ k − 1 P_{k|k-1} Pk∣k−1为k-1时刻到k时刻的估计误差协方差矩阵。
- Q k Q_k Qk为过程噪声协方差矩阵，越大说明越不相信预测。实际工程中过程噪声由厂商提供，也可以自己调参。
更新：
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 K_k = P_{k|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1
- K k K_k Kk为最优卡尔曼增益
-
R
k
R_k
Rk为测量噪声协方差矩阵，越大说明越不相信观测。实际工程中可自己调参，也可以自适应（AKF）。
x ^ k ∣ k = x ^ k ∣ k − 1 + K k ( z k − H k x ^ k ∣ k − 1 ) \hat x_{k|k} = \hat x_{k|k-1} + K_k (z_k - H_k \hat x_{k|k-1}) x^k∣k=x^k∣k−1+Kk(zk−Hkx^k∣k−1) - 更新状态估计
- 可以使用测量残差来简化表达公式 y ^ = z k − H k x ^ k ∣ k − 1 \hat y = z_k - H_k \hat x_{k|k-1} y^=zk−Hkx^k∣k−1， S k = c o v ( y ^ ) = H k P k ∣ k − 1 H k T + R k S_k = cov(\hat y) = H_k P_{k|k-1} H^T_k + R_k Sk=cov(y^)=HkPk∣k−1HkT+Rk
- 即卡尔曼增益可以简化为
K
k
=
P
k
∣
k
−
1
H
k
T
S
k
−
1
K_k = P_{k|k-1}H^T_k S_k^{-1}
Kk=Pk∣k−1HkTSk−1，状态估计更新可以简化为
x
^
k
∣
k
=
x
^
k
∣
k
−
1
+
K
k
y
^
\hat x_{k|k} = \hat x_{k|k-1} + K_k \hat y
x^k∣k=x^k∣k−1+Kky^
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k) P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1 - 更新协方差估计,后验概率
公式推导
已知：
Q
k
Q_k
Qk和
R
k
R_k
Rk一般为固定值，高级卡尔曼滤波可以用自适应。
P
k
∣
k
−
1
=
c
o
v
(
x
k
−
x
^
k
∣
k
−
1
)
=
E
(
(
x
k
−
x
^
k
∣
k
−
1
)
(
x
k
−
x
^
k
∣
k
−
1
)
T
)
P_{k|k-1} = cov(x_k - \hat x_{k|k-1}) = E((x_k - \hat x_{k|k-1})(x_k - \hat x_{k|k-1})^T)
Pk∣k−1=cov(xk−x^k∣k−1)=E((xk−x^k∣k−1)(xk−x^k∣k−1)T)
P
k
∣
k
=
c
o
v
(
x
k
−
x
^
k
∣
k
)
=
E
(
(
x
k
−
x
^
k
∣
k
)
(
x
k
−
x
^
k
∣
k
)
T
)
P_{k|k} = cov(x_k - \hat x_{k|k}) = E((x_k - \hat x_{k|k})(x_k - \hat x_{k|k})^T)
Pk∣k=cov(xk−x^k∣k)=E((xk−x^k∣k)(xk−x^k∣k)T)
S
k
=
c
o
v
(
y
^
)
=
c
o
v
(
z
k
−
H
k
x
^
k
∣
k
−
1
)
S_k = cov(\hat y) = cov(z_k - H_k \hat x_{k|k-1})
Sk=cov(y^)=cov(zk−Hkx^k∣k−1)
- 从状态方程推 P k ∣ k − 1 P_{k|k-1} Pk∣k−1：
P
k
∣
k
−
1
=
c
o
v
(
x
k
−
x
^
k
∣
k
−
1
)
P_{k|k-1} = cov(x_k - \hat x_{k|k-1})
Pk∣k−1=cov(xk−x^k∣k−1)
P
k
∣
k
−
1
=
c
o
v
(
F
k
x
k
−
1
+
w
k
−
F
k
x
^
k
−
1
∣
k
−
1
)
P_{k|k-1} = cov(F_k x_{k-1} + w_k - F_k \hat x_{k-1|k-1})
Pk∣k−1=cov(Fkxk−1+wk−Fkx^k−1∣k−1)
P
k
∣
k
−
1
=
c
o
v
(
F
k
(
x
k
−
1
−
x
^
k
−
1
∣
k
−
1
)
)
+
c
o
v
(
w
k
)
P_{k|k-1} = cov(F_k (x_{k-1}-\hat x_{k-1|k-1})) + cov(w_k)
Pk∣k−1=cov(Fk(xk−1−x^k−1∣k−1))+cov(wk)
P
k
∣
k
−
1
=
F
k
c
o
v
(
x
k
−
1
−
x
^
k
−
1
∣
k
−
1
)
F
k
T
+
Q
k
P_{k|k-1} = F_k cov(x_{k-1}-\hat x_{k-1|k-1})F_k^T + Q_k
Pk∣k−1=Fkcov(xk−1−x^k−1∣k−1)FkT+Qk
P
k
∣
k
−
1
=
F
k
P
k
−
1
∣
k
−
1
F
k
T
+
Q
k
P_{k|k-1} = F_k P_{k-1|k-1} F_k^T + Q_k
Pk∣k−1=FkPk−1∣k−1FkT+Qk
- 从状态估计更新方程推 P k ∣ k P_{k|k} Pk∣k：
P
k
∣
k
=
c
o
v
(
x
k
−
x
^
k
∣
k
)
P_{k|k} = cov(x_k - \hat x_{k|k})
Pk∣k=cov(xk−x^k∣k)
P
k
∣
k
=
c
o
v
(
x
k
−
x
^
k
∣
k
−
1
−
K
k
(
z
k
−
H
k
x
^
k
∣
k
−
1
)
)
P_{k|k} = cov(x_k - \hat x_{k|k-1} - K_k (z_k - H_k \hat x_{k|k-1}))
Pk∣k=cov(xk−x^k∣k−1−Kk(zk−Hkx^k∣k−1))
P
k
∣
k
=
c
o
v
(
x
k
−
x
^
k
∣
k
−
1
−
K
k
(
H
k
x
k
+
v
k
−
H
k
x
^
k
∣
k
−
1
)
)
P_{k|k} = cov(x_k - \hat x_{k|k-1} - K_k (H_k x_k + v_k - H_k \hat x_{k|k-1}))
Pk∣k=cov(xk−x^k∣k−1−Kk(Hkxk+vk−Hkx^k∣k−1))
P
k
∣
k
=
c
o
v
(
x
k
−
x
^
k
∣
k
−
1
−
K
k
H
k
(
x
k
−
x
^
k
∣
k
−
1
)
−
K
k
v
k
)
P_{k|k} = cov(x_k - \hat x_{k|k-1} - K_k H_k( x_k - \hat x_{k|k-1}) - K_k v_k)
Pk∣k=cov(xk−x^k∣k−1−KkHk(xk−x^k∣k−1)−Kkvk)
P
k
∣
k
=
c
o
v
(
(
I
−
K
k
H
k
)
(
x
k
−
x
^
k
∣
k
−
1
)
−
K
k
v
k
)
P_{k|k} = cov((I - K_k H_k)( x_k - \hat x_{k|k-1}) - K_k v_k)
Pk∣k=cov((I−KkHk)(xk−x^k∣k−1)−Kkvk)
P
k
∣
k
=
c
o
v
(
(
I
−
K
k
H
k
)
(
x
k
−
x
^
k
∣
k
−
1
)
)
+
c
o
v
(
K
k
v
k
)
P_{k|k} = cov((I - K_k H_k)( x_k - \hat x_{k|k-1})) + cov(K_k v_k)
Pk∣k=cov((I−KkHk)(xk−x^k∣k−1))+cov(Kkvk)
P
k
∣
k
=
(
I
−
K
k
H
k
)
c
o
v
(
x
k
−
x
^
k
∣
k
−
1
)
(
I
−
K
k
H
k
)
T
+
K
k
c
o
v
(
v
k
)
K
k
T
P_{k|k} = (I - K_k H_k)cov( x_k - \hat x_{k|k-1})(I - K_k H_k)^T + K_k cov(v_k) K_k^T
Pk∣k=(I−KkHk)cov(xk−x^k∣k−1)(I−KkHk)T+Kkcov(vk)KkT
P
k
∣
k
=
(
I
−
K
k
H
k
)
P
k
∣
k
−
1
(
I
−
K
k
H
k
)
T
+
K
k
R
k
K
k
T
P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T
Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT
这一公式对于任何卡尔曼增益 K k {K_k} Kk都成立。如果 K k {K_k} Kk是最优卡尔曼增益，则可以进一步简化
- 基于上式推导最优卡尔曼增益 K k {K_k} Kk：
卡尔曼滤波器是最小均方误差估计器，后验状态误差估计是指 P k ∣ k = c o v ( x k − x ^ k ∣ k ) P_{k|k} = cov(x_k - \hat x_{k|k}) Pk∣k=cov(xk−x^k∣k)，当 P k ∣ k P_{k|k} Pk∣k矩阵的均方误差为最小时，即可求出最优卡尔曼增益。
矩阵的均方误差为矩阵的迹。求矩阵的最小均方误差，即是求矩阵的迹的最小值，对矩阵的迹求导，导数为0时，迹最小。矩阵的迹求导有相关公式，网上查。协方差 P k ∣ k P_{k|k} Pk∣k是对称矩阵。
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT
P k ∣ k = ( P k ∣ k − 1 − K k H k P k ∣ k − 1 ) ( I − ( K k H k ) T ) + K k R k K k T P_{k|k} = (P_{k|k-1} - K_k H_k P_{k|k-1})(I - (K_k H_k)^T) + K_k R_k K_k^T Pk∣k=(Pk∣k−1−KkHkPk∣k−1)(I−(KkHk)T)+KkRkKkT
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k H k P k ∣ k − 1 ( K k H k ) T + K k R k K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k H_k P_{k|k-1}(K_k H_k)^T + K_k R_k K_k^T Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+KkHkPk∣k−1(KkHk)T+KkRkKkT
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT
t r ( P k ∣ k ) = t r ( P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + + K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pk∣k)=tr(Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T++Kk(HkPk∣k−1HkT+Rk)KkT)
t r ( P k ∣ k ) = t r ( P k ∣ k − 1 ) − t r ( K k H k P k ∣ k − 1 ) − t r ( P k ∣ k − 1 ( K k H k ) T ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} )- tr(K_k H_k P_{k|k-1}) -tr( P_{k|k-1} (K_k H_k)^T) + tr( K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pk∣k)=tr(Pk∣k−1)−tr(KkHkPk∣k−1)−tr(Pk∣k−1(KkHk)T)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)
t r ( P k ∣ k ) = t r ( P k ∣ k − 1 ) − 2 t r ( K k H k P k ∣ k − 1 ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) tr(P_{k|k}) = tr(P_{k|k-1} )- 2tr(K_k H_k P_{k|k-1})+ tr( K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T) tr(Pk∣k)=tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)
d t r ( P k ∣ k ) d K k = d [ t r ( P k ∣ k − 1 ) − 2 t r ( K k H k P k ∣ k − 1 ) + t r ( K k ( H k P k ∣ k − 1 H k T + R k ) K k T ) ] d K k \frac {d \ tr(P_{k|k})} {d\ K_k} =\frac {d\ {[tr(P_{k|k-1}) - 2tr(K_k H_k P_{k|k-1}) + tr(K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T)]}} {d\ K_k} d Kkd tr(Pk∣k)=d Kkd [tr(Pk∣k−1)−2tr(KkHkPk∣k−1)+tr(Kk(HkPk∣k−1HkT+Rk)KkT)]
d t r ( P k ∣ k ) d K k = − 2 ( H k P k ∣ k − 1 ) T + 2 K k ( H k P k ∣ k − 1 H k T + R k ) \frac {d \ tr(P_{k|k})} {d\ K_k} =-2 (H_k P_{k|k-1})^T + 2K_k(H_k P_{k|k-1} H^T_k + R_k) d Kkd tr(Pk∣k)=−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)
当矩阵导数是0的时候得到
P
k
∣
k
P_{k|k}
Pk∣k的迹（trace）的最小值：
t
r
(
P
k
∣
k
)
d
K
k
=
−
2
(
H
k
P
k
∣
k
−
1
)
T
+
2
K
k
(
H
k
P
k
∣
k
−
1
H
k
T
+
R
k
)
=
0
\frac {tr(P_{k|k})} {dK_k} =-2 (H_k P_{k|k-1})^T + 2K_k(H_k P_{k|k-1} H^T_k + R_k) = 0
dKktr(Pk∣k)=−2(HkPk∣k−1)T+2Kk(HkPk∣k−1HkT+Rk)=0
K k = P k ∣ k − 1 H k T ( H k P k ∣ k − 1 H k T + R k ) − 1 = P k ∣ k − 1 H k T S k − 1 K_k = P_{k|k-1}H^T_k {(H_k P_{k|k-1} H^T_k + R_k)}^{-1} = P_{k|k-1}H^T_k S_k^{-1} Kk=Pk∣k−1HkT(HkPk∣k−1HkT+Rk)−1=Pk∣k−1HkTSk−1
- 最优卡尔曼增益化简 P k ∣ k P_{k|k} Pk∣k
K k S k = ( H k P k ∣ k − 1 ) T K_kS_k = (H_k P_{k|k-1})^T KkSk=(HkPk∣k−1)T
K k S k K k T = ( H k P k ∣ k − 1 ) T K k T = P k ∣ k − 1 ( K k H k ) T K_k S_k K^T_k = (H_k P_{k|k-1})^T K^T_k = P_{k|k-1} (K_k H_k)^T KkSkKkT=(HkPk∣k−1)TKkT=Pk∣k−1(KkHk)T
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k ( H k P k ∣ k − 1 H k T + R k ) K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k (H_k P_{k|k-1} H^T_k + R_k )K_k^T Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Kk(HkPk∣k−1HkT+Rk)KkT
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + K k S k K k T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + K_k S_k K^T_k Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+KkSkKkT
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 − P k ∣ k − 1 ( K k H k ) T + P k ∣ k − 1 ( K k H k ) T P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} - P_{k|k-1} (K_k H_k)^T + P_{k|k-1} (K_k H_k)^T Pk∣k=Pk∣k−1−KkHkPk∣k−1−Pk∣k−1(KkHk)T+Pk∣k−1(KkHk)T
P k ∣ k = P k ∣ k − 1 − K k H k P k ∣ k − 1 P_{k|k} = P_{k|k-1} - K_k H_k P_{k|k-1} Pk∣k=Pk∣k−1−KkHkPk∣k−1
P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k )P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1
该简化式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 P_{k|k} = (I - K_k H_k )P_{k|k-1} Pk∣k=(I−KkHk)Pk∣k−1只能在最优卡尔曼增益时才成立。如果算术精度总是很低而导致数值稳定性出现问题，或者特意使用非最优卡尔曼增益，则必须使用上面未简化后的后验误差协方差公式 P k ∣ k = ( I − K k H k ) P k ∣ k − 1 ( I − K k H k ) T + K k R k K k T P_{k|k} = (I - K_k H_k) P_{k|k-1} (I - K_k H_k)^T + K_k R_k K_k^T Pk∣k=(I−KkHk)Pk∣k−1(I−KkHk)T+KkRkKkT。