Kalman Filter 卡尔曼滤波详解

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

数据融合

引入例子，当通过俩方法测量一物体质量时，结果和对应的方差为 $z_1$ 和 $z_2$ ,对应的方差是 ${\delta }_1和{\delta }_2$, 数据融合是通过这俩个数据来估计真实值。
$$z=z_1+k(z_2-z_1)$$
求k使得 ${\delta }_z$ 最小
$$\begin{gathered} {\delta }_z^2=Var(z_1+k(z_2-z_1)) \\ =Var((1-k)z_1)+Var(k{z}_2) \\ =(1-k{)}^{2}var(z1)+k^{2}Var(z_1) \end{gathered}$$
然后利用导数为零求出
$$k=\frac{{\delta }_1}{{\delta }_1^2+{\delta }_2^2}$$

协方差矩阵

三个变量的协方差矩阵形式如下
$$P=\left[\begin{array}{ccc}{\delta }_1^2& {\delta }_1{\delta }_2& {\delta }_1{\delta }_3\\ {\delta }_1{\delta }_2& {\delta }_2^2& {\delta }_2{\delta }_3\\ {\delta }_1{\delta }_3& {\delta }_2{\delta }_3& {\delta }_3^2\end{array}\right]$$
方差和协方差在一个矩阵表现出来，体现了变量之间的联动关系

${\delta }_1{\delta }_2$为正说明，俩变量正相关，反正负相关.

状态转移方程

[外链图片转存失败,源站可能有防盗链机制,建议将图片保存下来直接上传(img-DzU4hO6w-1682312199872)(C:\Users\家有二乔\AppData\Roaming\Typora\typora-user-images\1666547367785.png)]

引入例子，k为弹簧，B为阻尼器

牛顿定理得 $F=m\ddot{x}+B\dot{x}+kx=u,其中\ddot{x}是加速度，\dot{x}速度，x是位移$, 设

$$\begin{gathered} x_1=x \\ {x}_2=\dot{x} \end{gathered}$$
所有
$$\begin{gathered} {\dot{x}}_1=\dot{x}=x_2 \\ {\dot{x}}_2=\ddot{x}=-\frac{B}{m}{x}_2-\frac{k}{m}{x}_1+\frac{u}{m} \end{gathered}$$
矩阵的形式
$$\begin{gathered} \left[\begin{array}{c}{\dot{x}}_1\\ {\dot{x}}_2\end{array}\right]=\left[\begin{array}{cc}0& 1\\ -\frac{k}{m}& -\frac{B}{m}\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right]+\left[\begin{array}{c}0\\ \frac{1}{m}\end{array}\right]u \\ \left[\begin{array}{c}{z}_1\\ z_2\end{array}\right]=\left[\begin{array}{cc}1& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_1\\ x_2\end{array}\right] \end{gathered}$$
得到状态转移方程
$$\begin{gathered} \dot{X}(t)=AX(t)+Bu \\ Z(t)=HX(t) \end{gathered}$$
该式为连续的表达，但计算机只能处理线性的问题，所以需要进行离散化
$$\begin{gathered} X_k=A{X}_{k-1}+B{u}_{k-1}+w_{k-1} \\ {Z}_k=H{X}_k+v_k \end{gathered}$$
其中$w_{k-1}$为过程误差， $v_k$为测量误差，都属于自然界的误差，满足期望为0的正态分布

下表k为采样时间，其俩次采样时间差为采样周期，当采样周期$T\to 0$ , 转变成连续。

卡尔曼滤波

规定 ${\hat{x}}_k^{-}$为先验误差，即先通过理论方程得到的结果，里面会会有过程误差$w,P(w)\sim (0,Q),Q为过程误差的协方差矩阵$，$x_k$为测量值，即通过仪器等手段测量出来，里面会有机器带有的测量噪声$v,P(v)\sim (0,R),R$为测量误差协方差矩阵。${\hat{x}}_k$为最后得估计值，也就是我们所求

由状态转移方程得
$$\begin{gathered} {\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1} \\ {z}_k=H{x}_k \end{gathered}$$
由之前讲过得数据融合得到估计值

$ \hat x_k = \hat x^-_k + G(H^-z_k-\hat x^-_k)$

这里使用下面得标准形式 $G=k_{k}H$, $k_k\in [0,H^{-}]$
$${\hat{x}}_k={\hat{x}}_k^{-}+k_k(z_k-H{\hat{x}}_k^{-})$$
设$e_k=x_k-{\hat{x}}_k$

$x_k$为真实值，所以e为后验误差
$$P(e)\sim (0,P),P=E[e{e}^T]=\left[\begin{array}{cc}\delta e_1^2& \delta e_1\delta e_2\\ \delta e_1\delta e_2& \delta e_2^2\end{array}\right]$$
求后验误差得方差最小也就是求协方差矩阵得迹最小$tr§ = \delta e_1^2 +\delta e_2^2 $最小
$$\begin{array}{rl}{p}_k=& E[e_k{e}_k^T]\\ =& E[(x_k-{\hat{x}}_k)(x_k-{\hat{x}}_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\\ tr(p_k)=& tr(p_k^{-})-2tr(k_{k}H{P}_k^{-})+tr(k_{k}H{p}_k^{-}{H}^T{K}_k^T)+tr(K_{k}R{K}_k^T)\\ \frac{dtr(p_k)}{d{k}_k}& =0\end{array}$$
最后得到
$$K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R}$$
在$K_k公式还有未知量先验误差协方差P_k^{-}$

求$P_k^{-}$
$$\begin{array}{rl}{P}_k^{-}=& E[e_k^{-}{e}_k^{-T}]\\ =& A{P}_{k-1}{A}^T+Q\end{array}$$
所以
$$P_k=(I-k_{k}H)P_k^{-}$$
到此卡尔曼滤波的5个重要公式都求出来了，总结
$$\begin{gathered} 预测 \\ 先验估计{\hat{x}}_k^{-}=A{\hat{x}}_{k-1}+B{u}_{k-1} \\ 先验误差协方差P_k^{-}=A{P}_{k-1}{A}^T+Q \\ 校正 \\ 卡尔曼增益K_k=\frac{P_k^{-}{H}^T}{H{P}_k^{-}{H}^T+R} \\ 后验估计{\hat{x}}_k={\hat{x}}_k^{-}+k_k(z_k-H{\hat{x}}_k^{-}) \\ 后验误差协方差P_k=(I-k_{k}H)P_k^{-} \end{gathered}$$