卡尔曼滤波推导思路总结

推导思路一：
(1) 混合高斯
一维高斯函数形式：
$$\mathcal{N}(x,\mu ,\sigma )=\frac{1}{\sigma \sqrt{2\pi }}{e}^{-}\frac{(x-\mu {)}^2}{2{\sigma }^2}\text{\hspace{1em}(1)}$$

两个高斯函数相乘（未归一化）：
$$\mathcal{N}(x,{\mu }_0,{\sigma }_0)\times \mathcal{N}(x,{\mu }_1,{\sigma }_1){=}^{?}\mathcal{N}(x,\mu {}^{\prime },\sigma {}^{\prime })\text{\hspace{1em}(2)}$$

混合后高斯函数其均值和方差求解如下：

$$\begin{array}{rl}\mu {}^{\prime }& ={\mu }_0+\frac{{\sigma }_0^2({\mu }_1-{\mu }_0)}{{\sigma }_0^2+{\sigma }_1^2}\\ \sigma {{}^{\prime }}^2& ={\sigma }_0^2-\frac{{\sigma }_0^4}{{\sigma }_0^2+{\sigma }_1^2}\end{array}\text{\hspace{1em}(3)}$$

则上式可简化如下：
$$\begin{array}{rl}k& =\frac{{\sigma }_0^2}{{\sigma }_0^2+{\sigma }_1^2}\\ \mu {}^{\prime }& ={\mu }_0+k({\mu }_1-{\mu }_0)\\ \sigma {{}^{\prime }}^2& ={\sigma }_0^2-k{\sigma }_0^2\end{array}\text{\hspace{1em}(4)}$$

将上式改写为矩阵形式：
$$\begin{array}{rl}K& ={\Sigma }_0({\Sigma }_0+{\Sigma }_1{)}^{-1}\\ \vec{\mu }{}^{\prime }& =\overrightarrow{u_0}+K(\overrightarrow{{\mu }_1}-\overrightarrow{u_0})\\ \Sigma {}^{\prime }& ={\Sigma }_0-K{\Sigma }_0\end{array}\text{\hspace{1em}(5)}$$

其中${\Sigma }_i$表示协方差矩阵，$\overrightarrow{{\mu }_i}$表示均值。$K$表示卡尔曼增益。

(2)卡尔曼滤波
假定现有两个分布，一个是预测分布和观测分布：
$$\begin{array}{rl}({\mu }_0,{\Sigma }_0)& =(H_k\widehat{x_k}，H_k{P}_k{H}_k^K)\\ ({\mu }_1,{\Sigma }_1)& =(\overrightarrow{z_k},R_k)\end{array}\text{\hspace{1em}(6)}$$
结合公式5，可得如下：
$$\begin{array}{rl}{H}_k\overrightarrow{x_k{}^{\prime }}& =H_k\overrightarrow{x_k}+K(\overrightarrow{z_k}-H_k\widehat{x_k})\\ H_k{P}_k{}^{\prime }{H}_k^T& =H_k{P}_k{H}_k^T-K{H}_k{P}_k{H}_k^T\end{array}\text{\hspace{1em}(7)}$$

卡尔曼增益如下：
$$K=H_k{P}_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\text{\hspace{1em}(8)}$$

式(7)、式(8)等式左右约去$H_k$，注意约去$K$中隐藏的$H_k$，可将式(7)、式(8)写为如下：

$$\begin{array}{rl}{\vec{x}}_k{}^{\prime }& =\overrightarrow{x_k}+K{}^{\prime }(\overrightarrow{z_k}-H_k\widehat{x_k})\\ P_k{}^{\prime }& =P_k-K{}^{\prime }{H}_k{P}_k\\ K& =P_k{H}_k^T(H_k{P}_k{H}_k^T+R_k{)}^{-1}\end{array}\text{\hspace{1em}(9)}$$

式(9)便给了我们完整的更新步骤。

同时，我们附上一下的两个公式：
$$\begin{array}{rl}{\hat{x}}_k& =F_k{\hat{x}}_{k-1}+B_k{\vec{\mu }}_k\\ P_k& =F_k{P}_{k-1}{F}_k^T+Q_k\end{array}\text{\hspace{1em}(10)}$$

以上便是经典卡尔曼滤波中相关的5个公式。式(9)和式(10)中部分定义如下：

• $H_k$为转换矩阵
• $B_k$为控制矩阵
• $R_k$为测量误差
• $Q_k$为系统误差
• ${\vec{\mu }}_k$为控制向量

流程图如下：

推导思路一细节参见：http://www.bzarg.com/p/how-a-kalman-filter-works-in-pictures/#mjx-eqn-update

---

推导思路二：
由博文可知有预测方程和测量方程如下：
$$\{\begin{array}{l}{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$表示预测值，$z_k$表示测量值，$u_k$表示控制向量，$w_{k-1}$表示系统噪声，$v_k$表示测量噪声。

针对小车加速运行问题，上式可表示为：

$$\{\begin{array}{l}\left[\begin{array}{c}{x}_t\\ {\dot{x}}_t\end{array}\right]=\left[\begin{array}{cc}1& \Delta t\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_{t-1}\\ {\dot{x}}_{t-1}\end{array}\right]+\left[\begin{array}{c}\frac{\Delta t^2}{2}\\ \Delta t\end{array}\right]\times \alpha \\ z_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{x}_k\\ {\dot{x}}_k\end{array}\right]\end{array}\text{\hspace{1em}(2)}$$

公式2中$\alpha$表示加速度。

假定系统噪声$w_k$和测量噪声$v_k$皆服从于高斯分布，即$p(w)\sim N(0,Q)$和$p(v)\sim N(0,R)$。以$Q=\left[\begin{array}{cc}0& 0\\ 0& 0.01\end{array}\right]$为例，$Q$表明系统误差的协方差速度的方差为0.01，位移上标准差为0，速度和方差之间无关联。

现有$\widehat{x_k}{}^{\prime }$为预测值(先验)，${\hat{x}}_k$为估计值，${\hat{z}}_k$为观测值(后验)。由一般的反馈思想有：

$$\begin{array}{rl}{\hat{x}}_k& ={\hat{x}}_k{}^{\prime }+K_k(z_k-{\hat{z}}_k)\\ & ={\hat{x}}_k{}^{\prime }+K_k(z_k-H{\hat{x}}_k{}^{\prime })\end{array}\text{\hspace{1em}(3)}$$

其中$z_k$为真实值，$(z_k-H{\hat{x}}_k{}^{\prime })$为测量值与真实值之间的残差，其中求取$K_k$为关键。

假定估计值与真实值之间的协方差为：

$$\begin{array}{rl}{P}_k& =E[e_k{e}_k^T]=E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_k{)}^T]\\ & =\left[\begin{array}{cc}E(S_{err}{S}_{err}^T)& E(S_{err}{V}_{err}^T)\\ E(V_{err}{S}_{err}^T)& E(V_{err}{V}_{err}^T)\end{array}\right]\end{array}\text{\hspace{1em}(4)}$$

上式中$S_{err}$表示位移误差，$V_{err}$表示速度误差。

将式(3)代入式(4)得：
$$P_k=[(I-K_{k}H)(x_k-{\hat{x}}_k{}^{\prime })-K_k{v}_k][(I-K_{k}H)(x_k-{\hat{x}}_k{}^{\prime })-K_k{v}_k{]}^T\text{\hspace{1em}(5)}$$

同理得到预测值与真实值之间的协方差。

$$P_k{}^{\prime }=E[e_k{}^{\prime }{e}_k{{}^{\prime }}^T]=E[(x_k-{\hat{x}}_k{}^{\prime })(x_k-{\hat{x}}_k{}^{\prime }{)}^T]\text{\hspace{1em}(6)}$$

注意系统状态$x_k$与测量噪声$v_k$之间是相互独立的。

将式(5)展开可得：

$$\begin{array}{rl}{P}_k& =(I-K_{k}H)E[(x_k-{\hat{x}}_k{}^{\prime })(x_k-{\hat{x}}_k{}^{\prime }{)}^T](I-K_{k}H{)}^T+K_{k}E[v_k{v}_k^T]K_k^T\\ & =(I-K_{k}H)P_k{}^{\prime }(I-K_{k}H{)}^T+K_{k}R{K}_k^T\\ & =P_k{}^{\prime }-K_{k}H{P}_k{}^{\prime }-P_k{}^{\prime }{H}^T{K}_k^T+K_k(H{P}_k{}^{\prime }{H}^T+R)K_k^T\end{array}\text{\hspace{1em}(7)}$$

结合均方差的意义，利用矩阵的迹对式(7)进行操作得：

$$\begin{array}{r}T[P_k]=T[P_k{}^{\prime }]-2T[K_{k}H{P}_k{}^{\prime }]+T[K_k(H{P}_k{}^{\prime }{H}^T+R)K_k^T]\end{array}\text{\hspace{1em}(8)}$$

最小均方差，对$K_k$进行求导，令导函数为0。则有下式：

$$\begin{array}{rl}\frac{dT[P_k]}{d{K}_k}& =-2(H{P}_k{}^{\prime }{)}^T+2K_k(H{P}_k^{{}^{\prime }}{H}^T+R)=0\\ \therefore K_k& =P_k{}^{\prime }{H}^T(H{P}_k^{\prime }{H}^T+R{)}^{-1}\end{array}\text{\hspace{1em}(9)}$$

其中$R$为测量噪声协方差矩阵。假定上面的所有维度都为$1\times 1$维，令$H=1$，且$P_k{}^{\prime }̸=0$，则公式(9)可以简化如下：
$$K_k=\frac{P_k{}^{\prime }}{P_k{}^{\prime }+R}=\frac{1}{1+\frac{R}{P_k{}^{\prime }}}$$

分析上式可得以下结论：

• 若$K_k$随着$P_k{}^{\prime }$增大而增大，说明卡尔曼增益越大，越重视反馈。
• 若$P_k{}^{\prime }=0$，说明预测值等于真实值。
• 若$K_k=0$，说明估计值等于预测值。
• 注意，$P_k$为估计值与真实值之间的协方差，$P_k{}^{\prime }$为预测值与真实值之间的误差。

将计算出的$K_k$反代入公式 7中,化简可得：

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

上式中$P_k{}^{\prime }$的递推计算如下，注意$P_k{}^{\prime }$为预测值与真实值之间的协方差矩阵。

首先有预测值的递推形式： $\hat{x}{{}^{\prime }}_{k+1}=A\widehat{x_k}+B{u}_k$，结合公式(1)可得：

$$\begin{array}{rl}P{{}^{\prime }}_{k+1}& =E[e{{}^{\prime }}_{k+1}e{{}^{\prime }}_{k+1}^T]=E[(x_{k+1}-\hat{x}{{}^{\prime }}_{k+1})(x_{k+1}-\hat{x}{{}^{\prime }}_{k+1}{)}^T]\\ & =E[[A(x_k-{\hat{x}}_k)+w_k][A(x_k-{\hat{x}}_k)+w_k{]}^T]\end{array}\text{\hspace{1em}(11)}$$

注意系统状态$x_k$和系统噪声之间相互独立。

所以公式(11)可简化如下：

$$\begin{array}{rl}P{{}^{\prime }}_{k+1}& =E[(A{e}_{k+1})(A{e}_{k+1}{)}^T]+E[w_k{w}_k^T]\\ & =A{P}_k{A}^T+Q\end{array}\text{\hspace{1em}(12)}$$

由此，也获得了$P{{}^{\prime }}_{k+1}$的递推公式。仅需设置最初的$P_k$，便能迭代下去。其中$Q$表示系统噪声协方差矩阵。

现将卡尔曼滤波推导思路二总结如下：

$$\{\begin{array}{l}{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1}\\ P_k^{-}=A{P}_{k-1}{A}^T+Q\end{array}\text{\hspace{1em}(13)}$$

由公式(13)便可计算卡尔曼增益和估计值如下：

$$\{\begin{array}{l}{K}_k=P_k^{-}{H}^T(H{P}_k^{-}{H}^T+R{)}^{-1}\\ {\hat{x}}_k={\hat{x}}_k^{-}+K_k(z_k-H{x}_k^{-})\end{array}\text{\hspace{1em}(14)}$$

最后计算估计值和真实值之间的误差协方差矩阵为下次递推作准备：

$$P_k=(I-K_{k}H)P_k^{-}$$

推导思路一细节参见：https://blog.csdn.net/heyijia0327/article/details/17487467#commentBox

---

以上便是对卡尔曼滤波公式推导的两种总结。。。