视觉SLAM十四讲 第9讲后端 卡尔曼滤波器推导

卡尔曼滤波器

看十四讲以及课件没看懂，好多推导都是基于贝叶斯公式的那就更难以理解了。突然想到以前学控制的时候在B站看到过Up主DR_CAN的卡尔曼滤波器的推导，感觉比较清晰，于是听完之后记录一下推导过程。

状态空间方程：
$$\{\begin{array}{l}{x}_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}\\ z_k=C{x}_k+v_k\end{array}$$
其中：过程噪声$w\sim N(0,R)$，测量噪声$v\sim N(0,Q)$，R，Q为协方差矩阵

先验估计值 ${\hat{x}}_k^{-}$:
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$
测量结果$x_k$：

由
$$z_k=C{x}_k$$
得：
$${\hat{x}}_{kmea}=C^{-1}{z}_k$$
设后验结果${\hat{x}}_k$
$${\hat{x}}_k={\hat{x}}_k^{-}+G(C^{-1}{z}_k-{\hat{x}}_k^{-})$$
分析上式：当G=0时，${\hat{x}}_k={\hat{x}}_k^{-}$；当G=1时，${\hat{x}}_k=C^{-1}{z}_k$。

若 G=KC，K为卡尔曼增益，得
$${\hat{x}}_k={\hat{x}}_k^{-}+K(z_k-C{\hat{x}}_k^{-})$$
现在的目标为寻找K，使得$\hat{x}_k $趋近于$x_k$(即估计值趋近于真实值)

令误差 $e_k=x_k-{\hat{x}}_k$，$e_k\sim N(0,P)$，$P=E[e{e}^T]$

若估计值与实际值差距越小，则误差的方差越小，因此目标转化为寻找K，使得P矩阵的迹最小。
$$P=E[e{e}^T]=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]$$

$$x_k-{\hat{x}}_k=x_k-({\hat{x}}_k^{-}+K(z_k-C{\hat{x}}_k^{-}))=...=(I-KC)e_k^{-}-K{v}_k$$

所以
$$P_k=E[((I-KC)e_k^{-}-K{v}_k)((I-KC)e_k^{-}-K{v}_k{)}^T]=...=P_k^{-}-KC{P}_k^{-}-P_k^{-}{C}^T{K}^T+KC{P}_k^{-}{C}^T{K}^T+KQ{K}^T$$
现在求迹：
$$tr(P_k)=tr(P_k^{-})-2tr(KC{P}_k^{-})+tr(KC{P}_k^{-}{C}^T{K}^T)+tr(KQ{K}^T)$$
对迹求导：
$$\frac{dtr(P_k)}{dK}=0$$
得到：
$$-P_k^{-}{C}^T+K(C{P}_k^{-}{C}^T+Q)=0$$
化简得：
$$K=P_k^{-}{C}^T(C{P}_k^{-}{C}^T+Q{)}^{-1}$$
现在求$P_k^{-}$：
$$P_k^{-}=E(e_k^{-}{e}_k^{-T})$$
其中：
$$e_k^{-}=x_k-{\hat{x}}_k^{-}=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}-A{\hat{x}}_{k-1}-B{u}_{k-1}=A(x_{k-1}-{\hat{x}}_{k-1})+w_{k-1}=A{e}_{k-1}+w_{k-1}$$
所以：
$$P_k^{-}=E(e_k^{-}{e}_k^{-T})=...=A{\hat{P}}_{k-1}{A}^T+R$$
总结卡尔曼滤波五大公式：

1.预测

先验：
$${\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}$$
先验误差协方差：
$$P_k^{-}=A{\hat{P}}_{k-1}{A}^T+R$$
2.校正

卡尔曼增益：
$$K=P_k^{-}{C}^T(C{P}_k^{-}{C}^T+Q{)}^{-1}$$
后验估计：
$${\hat{x}}_k={\hat{x}}_k^{-}+K(z_k-C{\hat{x}}_k^{-})$$
更新误差协方差：
$${\hat{P}}_k=(I-KC)P_k^{-}$$