一看就懂的卡尔曼滤波五个公式

一看就懂的卡尔曼滤波五个公式

任何物体的运动都有一个运动方程：
$$\widehat{x_k}=A{x}_{k-1}+u+w,w\in (0,Q)\text{\hspace{1em}(1)}$$式中，k为某一个时刻，u为输入量，x为k-1时刻系统真实状态,w为方差Q的高斯误差，$\widehat{x_k}$为k时刻估计状态。
至此，根据这个系统状态我们有一个测量方程：
$$\widehat{z_k}=H\widehat{x_k}\text{\hspace{1em}(2)}$$式（2）被人称为先验。
这两个方程由于实际上 很多情况达不到标准导致了会有误差，然后方程变为：
$${z}_k=H\widehat{x_k}+v,v\in (0,R)\text{\hspace{1em}(3)}$$式中，v为方差R的高斯误差。
一般来说，式（3）中的$z_k$是通过传感器测量出来的（测量值），式（2）中的$\widehat{z_k}$是我们通过运动方程转化过来的（理论值）。针对这两个值我们到底应该相信谁呢？当然是谁的表现好谁的权重大，从而可以建立一个方程：
$$H{X}_{new}=(1-K)H\widehat{x_k}+K{z}_k=H\widehat{x_k}+K(z_k-H\widehat{x_k})\text{\hspace{1em}(5)}$$式中，$H{X}_{new}$为后验，K为权重。然后会发现K如何取值，前面说了谁的表现好就取谁的多一点，而衡量一个好学生表现好的标准是他成绩好然后保持稳定，成绩好就是卡尔曼算法的初值需要好，稳定就是协方差要小，说白了就是需要保持在90分以上而不是一会儿70一会儿100的。所以我们需要对协方差建模，首先是$\widehat{x_k}$的协方差$\overline{p_k}$通过（1）式可得：
$$\overline{p_k}=A{e}_{k-1}{e}_{k-1}^{\prime }{A}^{\prime }+Q=A{p}_{k-1}{A}^{\prime }+Q\text{\hspace{1em}(6)}$$然后zk的协方差$p_k$也可以根据（3）求出：
$$p_k=H\widehat{e_k}{\widehat{e_k}}^{\prime }{H}^{\prime }+R=H\overline{p_k}{H}^{\prime }+R\text{\hspace{1em}(7)}$$而先验协方差$\widehat{p_k}$很容易从式（2）得出：
$$\widehat{p_k}=H\widehat{e_k}{\widehat{e_k}}^{\prime }{H}^{\prime }=H\overline{p_k}{H}^{\prime }$$从而我们可以建立一个权重表达式：
$$K=\widehat{p_k}/p_k==H\overline{p_k}{H}^{\prime }/(H\overline{p_k}{H}^{\prime }+R)\text{\hspace{1em}(8)}$$从该市可看出理论值的协方差$\widehat{p_k}$越大，K越大，式（5）中理论值的比重越小，间接说明了式（5）的正确性，把K值代入式（5）我们可以得到：
$$H{X}_{new}=H\widehat{x_k}+H\overline{p_k}{H}^{\prime }/(H\overline{p_k}{H}^{\prime }+R)\ast (z_k-H\widehat{x_k})\text{\hspace{1em}(9)}$$但是我们想要估计的是状态X_{new}，设$K^{\prime }=\overline{p_k}{H}^{\prime }/(H\overline{p_k}{H}^{\prime }+R)$，从而式（5）可以两边除以H可得：
$$X_{new}=\widehat{x_k}+K^{\prime }(z_k-H\widehat{x_k})\text{\hspace{1em}(10)}$$式中。$X_{new}$估计值与真实值X_t的误差为：
$$x_t-X_{new}=x_t-\widehat{x_k}-K^{\prime }(H{x}_t-H\widehat{x_k}+v)=e_k-K^{\prime }H{e}_k+K^{\prime }v\text{\hspace{1em}(11)}$$从而更新协方差方程：
$$Pk=（I-K^{\prime }H）\overline{p_k}\text{\hspace{1em}(12)}$$
随便弄个跟踪图：