扩展卡尔曼滤波器

1线性系统与非线性系统

• 线性系统：状态变量和输出变量对于所有可能的输入变量和初始状态都满足叠加原理的系统。一个由线性元部件所组成的系统必是线性系统。但是，相反的命题在某些情况下可能不成立。线性系统的状态变量(或输出变量)与输入变量间的因果关系可用一组线性微分方程或差分方程来描述，这种方程称为系统的数学模型。
• 非线性系统：一个系统，如果其输出不与其输入成正比，则它是非线性的。从数学上看，非线性系统的特征是叠加原理不再成立。叠加原理是指描述系统的方程的两个解之和仍为其解。叠加原理可以通过两种方式失效。其一，方程本身是非线性的。其二，方程本身虽然是线性的，但边界是未知的或运动的。
  上述定义来源 线性系统与非线性系统区别

2 一个非线性卡尔曼滤波器

使用了一个没有状态变换和控制信号的模型，但拥有过程噪声，单个传感器，和单个状态值 ：

• 模型
  $x_k=x_{k-1}+w_k$
  $z_k=c{x}_k+v_k$
• 预测
  ${\hat{x}}_k={\hat{x}}_{k-1}$
  $p_k=p_{k-1}+q$
• 更新
  $g_k=p_{k}c(c{p}_{k}c+r{)}^{-1}$
  ${\hat{x}}_k\leftarrow {\hat{x}}_k+g_k(z_k-c{\hat{x}}_k)$
  $p_k\leftarrow (1-g_{k}c)p_k$
  相关参数说明参考这篇文章
  下面将传感器测量值的影响改为非线性，并且$c_k$代表它对时间步长的一阶导数：
• 模型
  $x_k=x_{k-1}+w_k$
  $z_k=h(x_k)+v_k$
• 预测
  ${\hat{x}}_k={\hat{x}}_{k-1}$
  $p_k=p_{k-1}+q$
• 更新
  $g_k=p_k{c}_k(c{p}_k{c}_k+r{)}^{-1}$
  ${\hat{x}}_k\leftarrow {\hat{x}}_k+g_k(z_k-c{\hat{x}}_k)$
  $p_k\leftarrow (1-g_k{c}_k)p_k$

3 计算导数

现在还有两个问题需要解决：
1.在并不知道显示函数时，如何从实际信号中计算一阶导数。
2. 如何推广我们的单值非线性状态/观测模型到多值系统。
这里求一阶导数如下：
$$f(x{)}^{{}^{\prime }}=\underset{n\to +\infty }{\lim }\frac{f(x+\Delta x)-f(x)}{\Delta x}$$
如果一个信号（像传感器值 $z_k$ ）是另一个信号(状态$x_k$)的函数，我们能够用第一个信号的连续差值除以第二个信号的连续差值来表示不是关于时间的有限差分，而是分子关于分母中信号的有限差分 ：
$$\frac{z_{k+1}-z_k}{x_{k+1}-x_k}$$

4 雅克比矩阵

接下来我们将解决3中的第二个问题，首先还是从线性系统分析，线性系统中传感器分量表示为：
$$z_k=C{x}_k$$对于一个非线性模型，同样存在这样一个矩阵，其行数等于传感器的数量，列数等于状态变量的个数。然而，这个矩阵将会包含传感器值对于相应状态值的一阶导数（线性近似）的当前值。数学家把它叫做偏导数，这些偏导数组成的矩阵称为雅可比矩阵。
上面我们已经将模型中的观测公式推广到非线性，这里，剩下的只有将状态变换公式推广到非线性。换句话说，线性模型中的:
$$x_k=A{x}_{k-1}+w_k$$
变为：
$$x_k=f(x_{k-1})+w_k$$
其中$A$被状态变换函数f的雅克比矩阵所代替，并且使用$H_k$代表传感器函数$h$的雅克比矩阵，将控制信号$u_k$合并到状态变换模型中，我们得到扩展卡尔曼滤波器的完整公式 ：

• 模型：
  $x_k=f(x_{k-1},u_k)+w_k$
  $z_k=h(x_k)+v_k$
• 预测：
  ${\hat{x}}_k=f({\hat{x}}_{k-1},u_k)$
  $P_k=F_k{P}_{k-1}{F}_k^T+Q_k$
• 更新
  $G_k=P_k{H}_k(H_k{P}_k{H}_k^T+R{)}^{-1}$
  ${\hat{x}}_k\leftarrow {\hat{x}}_k+G_k(z_k-h({\hat{x}}_k))$
  $P_k\leftarrow (I-G_k{H}_k)P_k$
  这样就完成了扩展卡尔曼滤波器的构建。