卡尔曼滤波器

卡尔曼滤波器

在B站看了一个卡尔曼滤波器的推导视频，感觉很详细，做一个笔记。

状态和测量方程

$$\begin{array}{rl}{X}_k& =A{X}_{k-1}+B{u}_{k-1}+w_{k-1}\\ Z_k& =H{X}_k+v_k\end{array}\text{\hspace{1em}(1)}$$

式(1)中$A$,$B$和$H$分别是状态矩阵、控制矩阵和测量矩阵，$X_k$，$u_k$，$w_k$, $Z_k$ 和 $v_k$分别是状态变量，控制变量，过程噪声，测量变量和测量噪声，其中测量噪声和过程噪声是不确定的，一般假设满足正态分布，$w$满足分布$p(w)\sim (0,Q)$，$v$满足分布$p(v)\sim (0,R)$。

过程噪声和测量噪声实际应用时是不知道的，忽略掉噪声可以得到先验估计和测量结果,分别为：
$$\begin{array}{rl}\widetilde{X_k}& =A\widehat{X_{k-1}}+B{u}_{k-1}\\ \widetilde{X_{k_{mea}}}& =H^{-1}{Z}_k\end{array}\text{\hspace{1em}(2)}$$

卡尔曼的作用就是结合先验$\widetilde{X_k}$和测量$\widetilde{X_{k_{mea}}}$结果得到比较准确的估计值$\widehat{X_k}$,即：
$$\widehat{X_k}=\widetilde{X_k}+G_k(H^{-1}{Z}_k-\widetilde{X_k})\text{\hspace{1em}(3)}$$

为了简化计算，令$K_k=G_{k}H$,则，
$$\widehat{X_k}=\widetilde{X_k}+K_k(Z_k-H\widetilde{X_k})\text{\hspace{1em}(4)}$$

其中$K_k\in [0,H^{-1}]$,当$K_k=0$时,完全相信根据状态方程估计的先验结果，当$K_k=H^{-1}$时,完全相信测量结果，取到两者之间时，则融合两个结果。

卡尔曼增益推导

从卡尔曼增益融合两个结果后使得误差最小入手，估计卡尔曼增益，定义误差为，
$$e_k=X_k-\widehat{X_k}\text{\hspace{1em}(5)}$$

假设$e_k$满足分布$p(e_k)\sim (0,P)$,协方差P定义为，
P = E { e k e k T } (6) P=E\{e_ke_k^T\} \tag{6} P=E{ek​ekT​}(6)

将式(1)、式(2)、式(4)和式(5)带入后得，

$$\begin{array}{rl}{e}_k& =X_k-\widehat{X_k}\\ & =X_k-(\widetilde{X_k}+K_k(Z_k-H\widetilde{X_k}))\\ & =X_k-(\widetilde{X_k}+K_k(H{X}_k+v_k-H\widetilde{X_k}))\\ & =(I-K_{k}H)(X_k-\widetilde{X_k})+K_k{v}_k\\ & =(I-K_{k}H)\widetilde{e_k}+K_k{v}_k\end{array}\text{\hspace{1em}(7)}$$

其中$\widetilde{e_k}$代表先验误差，
$$\widetilde{e_k}=X_k-\widetilde{X_k}\text{\hspace{1em}(8)}$$

将式(7)带入式(6)得，
P k = E { e k e k T } = E { [ ( I − K k H ) e k ~ + K k v k ] [ ( I − K k H ) e k ~ + K k v k ] T } = E { ( I − K k H ) e k ~ e k ~ T ( I − K k H ) T + ( I − K k H ) e k ~ v k T K k T + K k v k e k ~ T ( I − K k H ) T + K k v k v k T K k T } = ( I − K k H ) E { e k ~ e k ~ T } ( I − K k H ) T + K k E { v k v k T } K k T = ( I − K k H ) P k ~ ( I − K k H ) T + K k R K k T = ( P k ~ − K k H P k ~ ) ( I − K k H ) T + K k R K k T = P k ~ − K k H P k ~ − P k ~ H T K k T + K k H P k ~ H T K k T + K k R K k T (9) \begin{aligned}P_k &=E\{e_ke_k^T\} \\ &=E\{[(I - K_kH)\widetilde{e_k} + K_kv_k][(I - K_kH)\widetilde{e_k} + K_kv_k]^T\} \\ &= E\{(I - K_kH)\widetilde{e_k}\widetilde{e_k}^T(I - K_kH)^T + (I - K_kH)\widetilde{e_k} v_k^TK_k^T + K_kv_k\widetilde{e_k}^T(I - K_kH)^T + K_kv_kv_k^TK_k^T \} \\ &=(I - K_kH) E\{\widetilde{e_k}\widetilde{e_k}^T \} (I - K_kH)^T + K_kE\{v_kv_k^T\}K_k^T \\ &=(I - K_kH)\widetilde{P_k}(I - K_kH)^T + K_kRK_k^T \\ &=(\widetilde{P_k} - K_kH\widetilde{P_k})(I - K_kH)^T + K_kRK_k^T \\ &=\widetilde{P_k} - K_kH\widetilde{P_k} - \widetilde{P_k}H^TK_k^T + K_kH\widetilde{P_k}H^TK_k^T + K_kRK_k^T \end{aligned} \tag{9} Pk​​=E{ek​ekT​}=E{[(I−Kk​H)ek​ ​+Kk​vk​][(I−Kk​H)ek​ ​+Kk​vk​]T}=E{(I−Kk​H)ek​ ​ek​ ​T(I−Kk​H)T+(I−Kk​H)ek​ ​vkT​KkT​+Kk​vk​ek​ ​T(I−Kk​H)T+Kk​vk​vkT​KkT​}=(I−Kk​H)E{ek​ ​ek​ ​T}(I−Kk​H)T+Kk​E{vk​vkT​}KkT​=(I−Kk​H)Pk​ ​(I−Kk​H)T+Kk​RKkT​=(Pk​ ​−Kk​HPk​ ​)(I−Kk​H)T+Kk​RKkT​=Pk​ ​−Kk​HPk​ ​−Pk​ ​HTKkT​+Kk​HPk​ ​HTKkT​+Kk​RKkT​​(9)

上式化简时利用了$\widetilde{e_k}$和$v_k$不相关的期望为0的性质，其中$\widetilde{P_k}$代表先验误差协方差，为了使误差方差最小，即最小化$P_k$的迹$tr(P_k)$,求导得，
中$$\frac{\partial tr(P_k)}{\partial K_k}=-2(H\widetilde{P_k}{)}^T+2K_{k}H\widetilde{P_k}{H}^T+2K_{k}R\text{\hspace{1em}(10)}$$

令导数等于0，得到
$$K_k=\frac{\widetilde{P_k}{H}^T}{H\widetilde{P_k}{H}^T+R}\text{\hspace{1em}(11)}$$

其中$\widetilde{P_k}$代表先验误差估计,且$\widetilde{P_k}={\widetilde{P_k}}^T$,定义为：

P k ~ = E { e k ~ e k ~ T } (12) \widetilde{P_k} = E\{\widetilde{e_k}\widetilde{e_k}^T\} \tag{12} Pk​ ​=E{ek​ ​ek​ ​T}(12)

观察式(11),可以发现当测量噪声的协方差$R$趋近于无穷大时，卡尔曼增益趋近于0，结合式(4),可以发现估计的结果等于(基于模型的)先验结果,测量噪声大时，相信模型的先验结果；当测量噪声的协方差$R$趋近零时，卡尔曼增益趋近于$H^{-1}$,估计的结果等于测量结果，测量噪声小时更相信于测量结果。

误差矩阵推导

将式(1)、式(2)、式(8)带入式(12)得,
P k ~ = E { e k ~ e k ~ T } = E { e k ~ e k ~ T } = E { ( ( A X k − 1 + B u k − 1 + w k − 1 ) − ( A X k − 1 ^ + B u k − 1 ) ) e k ~ T } = E { ( A e k − 1 ^ + w k − 1 ) ( A e k − 1 ^ + w k − 1 ) T } = E { A e k − 1 ^ e k − 1 ^ T A } + E { A e k − 1 ^ w k − 1 T } + E { w k − 1 T A e k − 1 ^ T } + E { w k − 1 w k − 1 T } = A P k − 1 ^ A + Q (13) \begin{aligned}\widetilde{P_k} &= E\{\widetilde{e_k}\widetilde{e_k}^T\} \\ &= E\{\widetilde{e_k}\widetilde{e_k}^T\} \\ &= E\{((AX_{k-1} + Bu_{k-1} + w_{k-1})-(A\widehat{X_{k-1}} + Bu_{k-1}))\widetilde{e_k}^T \} \\ &=E\{ (A\widehat{e_{k-1}}+w_{k-1})(A\widehat{e_{k-1}}+w_{k-1})^T \} \\ &=E\{A\widehat{e_{k-1}}\widehat{e_{k-1}}^TA\} + E\{A\widehat{e_{k-1}}w_{k-1}^T\} + E\{w_{k-1}^TA\widehat{e_{k-1}}^T\} + E\{w_{k-1}w_{k-1}^T\} \\ &= A\widehat{P_{k-1}}A + Q \end{aligned} \tag{13} Pk​ ​​=E{ek​ ​ek​ ​T}=E{ek​ ​ek​ ​T}=E{((AXk−1​+Buk−1​+wk−1​)−(AXk−1​ ​+Buk−1​))ek​ ​T}=E{(Aek−1​ ​+wk−1​)(Aek−1​ ​+wk−1​)T}=E{Aek−1​ ​ek−1​ ​TA}+E{Aek−1​ ​wk−1T​}+E{wk−1T​Aek−1​ ​T}+E{wk−1​wk−1T​}=APk−1​ ​A+Q​(13)

上式中利用了$\widehat{e_{k-1}}$ 和$w_{k-1}$不相关的期望为0的性质，观察式(1),其中$w_{k-1}$作用于$X_k$,也就间接作用于$e_k$,而$\widehat{e_{k-1}}$是上一帧的误差估计，因此两者不相关。
式(13)说明，先验误差估计可以从上一帧的误差估计推理得到，因此需要继续推导误差估计，将式(11)带入式(9)中可得，

$$\begin{array}{rl}{P}_k& =\widetilde{P_k}-K_{k}H\widetilde{P_k}-\widetilde{P_k}{H}^T{K}_k^T+K_{k}H\widetilde{P_k}{H}^T{K}_k^T+K_{k}R{K}_k^T\\ & =\widetilde{P_k}-K_{k}H\widetilde{P_k}-\widetilde{P_k}{H}^T{K}_k^T+K_k(H\widetilde{P_k}{H}^T+R)K_k^T\\ & =\widetilde{P_k}-K_{k}H\widetilde{P_k}-\widetilde{P_k}{H}^T{K}_k^T+\frac{\widetilde{P_k}{H}^T}{H\widetilde{P_k}{H}^T+R}(H\widetilde{P_k}{H}^T+R)K_k^T\\ & =\widetilde{P_k}-K_{k}H\widetilde{P_k}\\ & =(I-K_{k}H)\widetilde{P_k}\end{array}\text{\hspace{1em}(14)}$$

卡尔曼预测校正步骤

由于每一次的预测都依赖上一次的结果，$\widetilde{P_k}$依赖于$\widehat{P_{k-1}}$,$\widetilde{X_k}$依赖于$\widehat{X_{k-1}}$,因此迭代前需要先设置初始值$\widehat{X_0}$,$\widehat{P_0}$。

预测过程

1. 先验更新：
   $$\widetilde{X_k}=A\widehat{X_{k-1}}+B{u}_{k-1}$$

2. 先验误差矩阵更新：
   $$\widetilde{P_k}=A\widehat{P_{k-1}}A+Q$$

校正

3. 计算卡尔曼增益
   $$K_k=\frac{\widetilde{P_k}{H}^T}{H\widetilde{P_k}{H}^T+R}$$

4. 后验估计
   $$\widehat{X_k}=\widetilde{X_k}+K_k(Z_k-H\widetilde{X_k})$$

5. 更新误差矩阵
   $$\widehat{P_k}=(I-K_{k}H)\widetilde{P_k}$$

参考

DR_CAN卡尔曼滤波器

张贤达. 矩阵分析与应用