卡尔曼滤波公式推导
卡尔曼滤波公式推导
1.运动方程及观测方程
已知时刻 k − 1 k-1 k−1位置状态向量为 X k − 1 X_{k-1} Xk−1，下一时刻 k k k位置状态向量为 X k X_k Xk，两者之间的转换关系如下:
X k = F k X k − 1 + n m X_k = F_kX_{k-1} + n_m Xk=FkXk−1+nm
其中， F k F_k Fk表示从 k − 1 k-1 k−1时刻到 k k k时刻之间位置状态向量的转移关系， n m n_m nm表示运动模型噪声，假设服从于正态分布 N ( 0 , Q ) N(0, Q) N(0,Q)
此外，在不同的位置观测到不同的路标点 Z k Z_k Zk，因此观测方程关系如下：
Z k = H k X k + n w Z_k = H_kX_k+n_w Zk=HkXk+nw
其中， H x H_x Hx表示根据位置向量生成观测 Z k Z_k Zk的观测矩阵， n w n_w nw表示观测噪声，假设服从于正态分布 N ( 0 , R ) N(0,R) N(0,R)
联立运动方程和观测方程可得
X k = F k X k − 1 + n m Z k = H k X k + n w X_k = F_kX_{k-1} + n_m\\ Z_k = H_kX_k+n_w Xk=FkXk−1+nmZk=HkXk+nw
需要注意的是，此处方程都是线性系统的表达形式，实际问题中， F k F_k Fk和 H k H_k Hk应当是关于 X X X的非线性函数，对于这种非线性系统，需要做线性化，即扩展卡尔曼滤波
2.目标函数构建
卡尔曼滤波实际上可以看做是优化问题的迭代求解，因此，以优化问题的角度看，应当先构建优化目标函数。
从结果来看，我们想要求解出满足运动方程和观测方程的最优位置状态向量 X k ∗ X_k^* Xk∗，因此，若假设 X k − X_k^- Xk−表示基于运动方程得到的预测位置， X k + X_k^+ Xk+表示基于观测方程得到的观测位置，那么，由于噪声不可测量，先忽略运动方程与观测方程之间的噪声，理想情况下目标函数应该是
L ( X k ) = ( X k − ( X k − + X k + ) / 2 ) T ( X k − ( X k − + X k + ) / 2 ) L(X_k) = (X_k - (X_k^-+X_k^+)/2)^T(X_k - (X_k^-+X_k^+)/2) L(Xk)=(Xk−(Xk−+Xk+)/2)T(Xk−(Xk−+Xk+)/2)
上式是个简单的最小二乘问题，可以求出解析解为
X ^ k = ( X k − + X k + ) / 2 \hat{X}_k =(X_k^-+X_k^+)/2 X^k=(Xk−+Xk+)/2
其中， X k − = F k X k − 1 X_k^-=F_kX_{k-1} Xk−=FkXk−1， X k + = H k − 1 Z k X_k^+=H_k^{-1}Z_k Xk+=Hk−1Zk
显然，理想情况下最优结果是多个观测结果的均值，但在实际问题中，必然存在着噪声，各个观测之间的不确定性是不同的，因此优化估计得到的位置状态向量应该写成如下形式
X ^ k = X k − + G k ( X k + − X k − ) \hat{X}_k = X_k^- + G_k(X_k^+-X_k^-) X^k=Xk−+Gk(Xk+−Xk−)
其中，权重因子 G k G_k Gk取值范围从0到1，趋于0时更相信运动方程预测的结果，趋于1时更相信观测方程的结果
进一步地，需要确定权重因子 G k G_k Gk的值；显然，最优的权重因子应该使得估计位置状态向量 X ^ k \hat{X}_k X^k最接近真实位置状态向量 X k X_k Xk，即构造目标函数如下：
L ( G k ) = E ( ( X k − X ^ k ) T ( X k − X ^ k ) ) = E ( e k T e k ) L(G_k) = E((X_k-\hat{X}_k)^T(X_k-\hat{X}_k)) = E(e_k^Te_k) L(Gk)=E((Xk−X^k)T(Xk−X^k))=E(ekTek)
其中，
e k = X k − X ^ k = X k − X k − − G k ( X k + − X k − ) = X k − （ I − G k ） X k − 1 − G k H k − 1 Z k = X k − ( I − G k ) X k − 1 − G k X k − G k H k − 1 n w = ( I − G k ) ( X k − X k − 1 ) − G k H k − 1 n w e_k = X_k-\hat{X}_k \\ = X_k - X_k^--G_k(X_k^+-X_k^-) \\ = X_k - （I-G_k）X_k^{-1} - G_kH_k^{-1}Z_k \\ = X_k - (I-G_k)X_k^{-1} - G_kX_k - G_kH_k^{-1}n_w \\ = (I-G_k)(X_k-X_k^{-1}) - G_kH_k^{-1}n_w ek=Xk−X^k=Xk−Xk−−Gk(Xk+−Xk−)=Xk−（I−Gk）Xk−1−GkHk−1Zk=Xk−(I−Gk)Xk−1−GkXk−GkHk−1nw=(I−Gk)(Xk−Xk−1)−GkHk−1nw
即，将上式代入目标函数可写成
L ( G k ) = E ( e k T e k ) = E { [ ( I − G k ) ( X k − X k − 1 ) ] T [ ( I − G k ) ( X k − X k − 1 ) ] } − E { [ ( I − G k ) ( X k − X k − 1 ) ] T G k H k − 1 n w } − E { [ G k H k − 1 n w ] T [ ( I − G k ) ( X k − X k − 1 ) ] } + E { [ G k H k − 1 n w ] T G k H k − 1 n w } = ( I − G k ) E ( e k − T e k − ) ( I − G k ) T + ( G k H k − 1 ) E ( n w T n w ) ( G k H k − 1 ) T = G k [ E ( e k − T e k − ) + H k − 1 E ( n w T n w ) H k − 1 T ] G k T − 2 G k E ( e k − T e k − ) + E ( e k − T e k − ) L(G_k) = E(e_k^Te_k) \\ = E\{[(I-G_k)(X_k-X_k^{-1})]^T[(I-G_k)(X_k-X_k^{-1})] \} - E\{[(I-G_k)(X_k-X_k^{-1})]^TG_kH_k^{-1}n_w\} \\- E\{[G_kH_k^{-1}n_w]^T[(I-G_k)(X_k-X_k^{-1})] \} + E\{[G_kH_k^{-1}n_w]^TG_kH_k^{-1}n_w\} \\ = (I-G_k)E({e_k^-}^Te_k^-)(I-G_k)^T + (G_kH_k^{-1})E(n_w^Tn_w)(G_kH_k^{-1})^T\\ = G_k[E({e_k^-}^Te_k^-) + H_k^{-1}E(n_w^Tn_w){H_k^{-1}}^T]G_k^T -2G_kE({e_k^-}^Te_k^-) + E({e_k^-}^Te_k^-) L(Gk)=E(ekTek)=E{[(I−Gk)(Xk−Xk−1)]T[(I−Gk)(Xk−Xk−1)]}−E{[(I−Gk)(Xk−Xk−1)]TGkHk−1nw}−E{[GkHk−1nw]T[(I−Gk)(Xk−Xk−1)]}+E{[GkHk−1nw]TGkHk−1nw}=(I−Gk)E(ek−Tek−)(I−Gk)T+(GkHk−1)E(nwTnw)(GkHk−1)T=Gk[E(ek−Tek−)+Hk−1E(nwTnw)Hk−1T]GkT−2GkE(ek−Tek−)+E(ek−Tek−)
其中， e k − e_k^- ek−实质上可以表示 X k − X_k^- Xk−协方差；第三个等号处，中间两项包含噪声的期望都为0，可以消掉
3.目标函数求解
对目标函数等式(9)求导可得
∂ L ∂ G k = 2 G K [ E ( e k − T e k − ) + H k − 1 E ( n w T n w ) H k − 1 T ) ] − 2 E T ( e k − T e k − ) \frac{\partial L}{\partial G_k} = 2G_K[E({e_k^-}^Te_k^-) + H_k^{-1}E(n_w^Tn_w){H_k^{-1}}^T)]-2E^T({e_k^-}^Te_k^-) ∂Gk∂L=2GK[E(ek−Tek−)+Hk−1E(nwTnw)Hk−1T)]−2ET(ek−Tek−)
令导数为0可得，
G K = [ E ( e k − T e k − ) + H k − 1 E ( n w T n w ) H k − 1 T ) ] − 1 E T ( e k − T e k − ) = P k − P k − + H k − 1 R H k − 1 T G_K =[E({e_k^-}^Te_k^-) + H_k^{-1}E(n_w^Tn_w){H_k^{-1}}^T)]^{-1}E^T({e_k^-}^Te_k^-) \\ =\frac{P_k^-}{P_k^-+H_k^{-1}R{H_k^{-1}}^T} GK=[E(ek−Tek−)+Hk−1E(nwTnw)Hk−1T)]−1ET(ek−Tek−)=Pk−+Hk−1RHk−1TPk−
其中， P k P_k Pk表示预测公式推导出协方差， R R R表示噪声 n w n_w nw的协方差，由此，求解出权重因子 G k G_k Gk
需要注意的是， G k G_k Gk即常说的卡尔曼增益因子，这里形式与常规公式不一致，是因为等式(6)中将 H k H_k Hk放到括号里面，实质上是一致的
为了和一般公式一致，对等式(6)和等式(11)进行改写
X ^ k = X k − + G k ( Z k − H k X k − ) G k = P k − H T H k P k − H k T + R \hat{X}_k = X_k^-+G_k(Z_k-H_kX_k^-)\\ G_k = \frac{P_k^-H^T}{H_kP_k^-H_k^T+R} X^k=Xk−+Gk(Zk−HkXk−)Gk=HkPk−HkT+RPk−HT
整理可得，
-
预测方程:
X k − = F k X ^ k − 1 P k − = F k P ^ k − 1 F k T + Q X_k^- = F_k\hat{X}_{k-1} \\ P_k^- = F_k\hat{P}_{k-1}F_k^T+Q\ Xk−=FkX^k−1Pk−=FkP^k−1FkT+Q -
卡尔曼增益:
G k = P k − H k T H P k − H T + R G_k = \frac{P_k^-H_k^T}{HP_k^-H^T+R} Gk=HPk−HT+RPk−HkT -
更新方程:
X ^ k = X k − + G k ( Z k − H k X k − ) P ^ k = ( I − G k H k ) P k − \hat{X}_k = X_k^- + G_k(Z_k-H_kX_k^-)\\ \hat{P}_k = (I-G_kH_k)P_k^- X^k=Xk−+Gk(Zk−HkXk−)P^k=(I−GkHk)Pk−
4.实际使用经验
- 初值怎么给
- 噪声方差如何确定