扩展卡尔曼滤波详解

Extened Kalman Filter

简单介绍

卡尔曼滤波详解讲解的基本的卡尔曼滤波算法是通过一个线性随机差分方程来估计当前状态，但是如果状态估计关系以及测量关系使非线性的怎么办，而且在实际使用中大部分的问题都是非线性的，所以提出了扩展卡尔曼滤波算法来解决非线性问题。

基本模型

扩展卡尔曼滤波算法和基本的线性卡尔曼滤波算法的基本模型在形式上很相近，也是包含状态方程和测量方程两个，只不过变量间的计算关系不是线性的。

扩展卡尔曼滤波的状态方程，利用非线性随机差分方程（non-Linear Stochastic Difference equation）利用上一个系统状态估计当前系统状态：
$$x_k=f(x_{k-1},u_{k-1},w_{k-1})$$
如果像卡尔曼一样忽略 $u$ 控制输入（在下面都是忽略该项，不重复了），得到：
$$x_k=f(x_{k-1},0,w_{k-1})(1.1)$$
加上对于当前状态的测量方程：
$$z_k=h(x_k,v_k)(1.2)$$
这里的参数含义就不解释了，和上面的相同，主要是：

$k-1$ 和 $k$ 分别表示上一状态和当前状态；

$x\in R^n$ 表示要估计的状态；

$u\in R^l$ 表示可选的控制输入，一般在实际使用中忽略；

$z\in R^m$ 表示测量值；

$w\in R^n$ 表示过程噪声，主要是从上一状态进入到当前状态时，会有许多外界因素的干扰；

$v\in R^m$ 表示测量噪声，主要是任何测量仪器都会有一定的误差；

$f$ 表示从上一状态到当前状态的非线性映射方程；

$h$ 表示状态和测量之间的非线性映射方程；

在上面转换矩阵 $A$ $B$ $H$ 是随着状态变化的，在这里没有添加下标，假设是不变的。

上面提到的过程噪声 $w$ 和测量噪声 $v$ 假设是相互独立的（之间没有关系，无相互影响），且是高斯白噪声，意思是这些噪声在离散的状态上是没有关系的（互相独立的， 每个时刻的噪声都是独立的）且 服从高斯分布：
$$\begin{gathered} p(w)\sim N(0,Q)(1.3) \\ p(v)\sim N(0,R)(1.4) \end{gathered}$$

$Q\in R^{n\times n}$ 表示过程噪声 $w$ 的协方差矩阵，表示 $w$ 向量元素之间的相关关系；

$R\in R^{m\times m}$ 表示测量噪声 $v$ 的协方差矩阵，表示 $v$ 向量元素之间的相关关系；

在上面协方差矩阵 $Q$ $R$ 是随着状态变化的，在这里假设是不变的。

基本公式

上面是给出了扩展卡尔曼滤波的整体思路和基本模型，在这里一步步分解来看扩展卡尔曼滤波的推导过程。首先确定了扩展卡尔曼滤波的基本模型，那么下一步是怎样根据这个模型来不断求取后面的状态。

首先定义几个变量：

$\overline{{\hat{x}}_k}$ 表示仅仅利用过程先验知识求出的当前状态的先验状态估计(a priori state estimate)，不考虑过程噪声的情况下；

${\hat{x}}_k$ 表示利用测量值 $z_k$ 求出的当前状态的后验状态估计(a posteriori state estimate)，也是最终求得的状态；

需要注意的是，卡尔曼滤波的最终目标是求 ${\hat{x}}_k$ ，这是对当前状态的最优估计。

根据上面的定义忽略过程噪声可以得到：
$$\overline{{\hat{x}}_k}=f({\hat{x}}_{k-1},u_{k-1},0)$$
通常都是忽略 $u$ 这个可选的控制输入（以后加入该变量的话也是非常方便的）：
$$\overline{{\hat{x}}_k}=f({\hat{x}}_{}$$