卡尔曼滤波算法

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

1.9 扩展卡尔曼滤波（EKF）

  由卡尔曼滤波算法完整公式可知，经典卡尔曼滤波算法为线性系统，状态空间方程，如式22所示。
  $EKF$是在经典卡尔曼滤波算法的基础上进行非线性操作，为了适配不确定的非线性问题，非线性系统，如式$(78)$所示：
$$\{\begin{array}{l}{x}_k=f\left(x_{k-1},u_{k-1},w_{k-1}\right)\\ z_k=h\left(x_k,v_k\right)\end{array}\text{\hspace{1em}(78)}$$
  $f，h$为非线性表达形式。误差都符合正态分布。
  但正态分布的随机变量通过非线性系统后，就不再是正态分布的形式，如图1.9.1所示：

  因此需要线性化：Linearization。
  线性化工具：泰勒级数，高维需要雅可比矩阵，如式$(79)$与图1.9.2所示为非线性的线性化处理。
$$\{\begin{array}{l}f(x)=f(x_0)+\frac{\partial f}{\partial x}(x-x_0)\\ f(x)=f(x_0)+k(x-x_0)\end{array}\text{\hspace{1em}(79)}$$

  $x_0$为：线性化点。
  因为系统有误差，无法在真实点线性化。处理方法：$f(x_k)$在${\hat{x}}_{k-1}$处线性化。因此，如式$(80)$所示：
$$\{\begin{array}{l}{x}_k=f\left(x_{k-1},u_{k-1},w_{k-1}\right)\\ z_k=h\left(x_k,v_k\right)\end{array}\text{\hspace{1em}(80)}$$
  可转化为，如式$(81)$所示：
$$x_k=f\left({\hat{x}}_{k-1},u_{k-1},w_{k-1}\right)+A\left(x_k-{\hat{x}}_{k-1}\right)+W_k{w}_{k-1}\text{\hspace{1em}(81)}$$
  误差$w_{k-1}$为$0$，$f\left({\hat{x}}_{k-1},u_{k-1},0\right)={\tilde{x}}_R$，如式(82)所示：
$$x_k={\tilde{x}}_R+A\left(x_k-{\hat{x}}_{k-1}\right)+W_k{w}_{k-1}\text{\hspace{1em}(82)}$$
  其中：$A$为雅各比矩阵，如式$(83)$所示：
$$A={\frac{\partial f}{\partial x}|}_{{\hat{x}}_{k-1},u_{k-1}}\text{\hspace{1em}(83)}$$
  求矩阵$A$，如式$(84)$所示：
已知函数：
$$\{\begin{array}{l}{f}_1=x_1+\sin x_2\\ f_2=x_1^2\end{array}$$

雅可比矩阵 ( A ) 为：
$$A=\left[\begin{array}{cc}\frac{\partial f_1}{\partial x_1}& \frac{\partial f_1}{\partial x_2}\\ \frac{\partial f_2}{\partial x_1}& \frac{\partial f_2}{\partial x_2}\end{array}\right]=\left[\begin{array}{cc}1& \cos x_2\\ 2x_1& 0\end{array}\right]{|}_{\begin{array}{c}{\hat{x}}_{1k-1}\\ {\hat{x}}_{2k-1}\end{array}}$$

代入估计值后，( A ) 为：
$$A=\left[\begin{array}{cc}1& \cos \left({\hat{x}}_{2k-1}\right)\\ 2{\hat{x}}_{1k-1}& 0\end{array}\right]\text{\hspace{1em}(84)}$$
  同理：
$$W_k={\frac{\partial f}{\partial w}|}_{{\hat{x}}_{k-1},u_{k-1}}\text{\hspace{1em}(85)}$$
  $z_k=h(x_k,v_k)$在${\hat{x}}_R$处线性化：
$$z_k=h\left({\tilde{x}}_k,v_k\right)+H\left(x_k-{\tilde{x}}_k\right)+V{v}_k\left(v_k=0,{\tilde{z}}_k=h\left({\tilde{x}}_k,0\right)\right)\text{\hspace{1em}(86)}$$
$$\{\begin{array}{l}H={\frac{\partial h}{\partial x}|}_{{\hat{x}}_k}\\ V={\frac{\partial h}{\partial v}|}_{{\hat{x}}_k}\end{array}\text{\hspace{1em}(87)}$$
因此：
$$\{\begin{array}{l}{x}_k={\tilde{x}}_k+A\left(x_k-{\hat{x}}_{k-1}\right)+W{w}_{k-1}\\ z_k={\tilde{z}}_k+H\left(x_k-{\tilde{x}}_k\right)+V{v}_k\end{array}\text{\hspace{1em}(88)}$$
  概率分布：
  $P(w)\sim N(0,Q)$，转化为$P(W{w}_k)\sim N\left(0,WQ{W}^T\right)$
  Eg：
  期望：$E(ax)=aE(x)=0$
  方差：$Var(ax)=a^{2}Var(x)$
  同理：
  $P(v)\sim N(0,R)$，转化为$P(V{v}_k)\sim N\left(0,VR{V}^T\right)$
  因此，非线性卡尔曼滤波算法完整公式为：

参考资料

DR_CAN