卡尔曼滤波算法

  卡尔曼滤波是一种基于“预测–更新”递归框架的最优线性估计器：以状态转移模型先验递推系统状态及其协方差，再以观测数据通过最小化估计协方差进行后验校正，从而在过程与测量噪声并存的环境下给出最小均方误差意义下的最优估计。

1.3 协方差矩阵

  在讲解协方差矩阵的时候，先来看下方差的计算，如式$(14)$所示：
$$\begin{array}{rl}\bar{x}& =\frac{1}{k}\sum _{i=1}^k{x}_i\\ {\sigma }_{xx}^2& =\frac{1}{k}\sum _{i=1}^k{\left(x_i-\bar{x}\right)}^2\end{array}\text{\hspace{1em}(14)}$$
  根据式$(14)$扩展得到协方差的计算，如式$(15)$所示：
$$\begin{array}{rl}\bar{x}& =\frac{1}{k}\sum _{i=1}^k{x}_i,\bar{y}=\frac{1}{k}\sum _{i=1}^k{y}_i\\ {\sigma }_{xy}^2& ={\sigma }_{yx}^2=\frac{1}{k}\sum _{i=1}^k\left(x_i-\bar{x}\right)\left(y_i-\bar{y}\right)\end{array}\text{\hspace{1em}(15)}$$

  Eg2：如下系统：

  系统微分方程如式$(16)$所示：
$$F-kx-B\dot{x}=m\ddot{x}\text{\hspace{1em}(16)}$$
  取状态量$\{\begin{array}{l}{x}_1=x\\ x_2=\dot{x}\end{array}$，则系统状态空间方程：关键每一行为一阶微分方程，如式$(17)$所示：
$$\begin{array}{rl}& F-kx-B\dot{x}=m\ddot{x}⟹\ddot{x}=-\frac{k}{m}x-\frac{B}{m}\dot{x}+\frac{F}{m}\\ & \{\begin{array}{l}{x}_1=x\\ x_2=\dot{x}\end{array}\\ & \{\begin{array}{l}{\dot{x}}_1=\dot{x}=x_2\\ {\dot{x}}_2=\ddot{x}=-\frac{k}{m}x-\frac{B}{m}\dot{x}+\frac{F}{m}\end{array}⟹\\ & \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]F\end{array}\text{\hspace{1em}(17)}$$
测量量定义，如式$(18)$所示：
  $z_1$：所测量的位置量；
  $z_2$：所测量的速度量；
$$\{\begin{array}{l}{z}_1=x=x_1\\ z_2=\dot{x}=x_2\end{array}\text{\hspace{1em}(18)}$$
  化成一般形式，如式$(19)$所示：
$$\begin{array}{c}\{\begin{array}{l}\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]F⟹\dot{X}(t)=AX(t)+BU(t)\end{array}\\ \{\begin{array}{l}\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]⟹Z(t)=HX(t)\end{array}\end{array}\text{\hspace{1em}(19)}$$
  离散化，如式$(20)$所示：
$$\{\begin{array}{l}{X}_k=A{X}_{k-1}+B{U}_k\\ Z_k=H{X}_k\end{array}\text{\hspace{1em}(20)}$$
  考虑系统不确定性与时间连续性：即系统包含过程噪声$w_{k-1}$和测量噪声$v_k$，如式(21)所示：
$$\{\begin{array}{l}{\dot{X}}_k=A{X}_{k-1}+B{U}_k+w_{k-1}\\ Z_k=H{X}_k+v_k\end{array}\text{\hspace{1em}(21)}$$
  问题：如何通过计算结果$X_k$和测量$Z_k$估计相对更为精确的结果：${\hat{X}}_k$。
  解决方案：计算结果$X_k$和测量$Z_k$曲线融合 Fusion。

参考资料

DR_CAN