扩展卡尔曼滤波详解

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}}_{k-1},0,0)(2.1)$$
然后同理忽略测量噪声可以得到：
$$\overline{z_k}=h(\overline{{\hat{x}}_k},0)$$

$\overline{z_k}$ 表示忽略测量噪声根据当前先验状态得到的无噪声测量值。

然后可以重写上面的模型（很好理解，就是把缺失的那部分在累加上，这样变成一个线性模型更好处理）：
$$\begin{gathered} x_k\approx \overline{{\hat{x}}_k}+A(x_{k-1}-{\hat{x}}_{k-1})+W{w}_{k-1} \\ {z}_k\approx \overline{z_k}+H(x_k-\overline{{\hat{x}}_k})+V{v}_k \end{gathered}$$

$x_k$ 和 $z_k$ 表示真实状态和测量向量；

$\overline{{\hat{x}}_k}$ 和 $\overline{z_k}$ 表示利用上面两个公式计算的无噪声情况下的状态和测量向量；

$w_k$ 和 $v_k$ 表示过程和测量噪声；

$A$ 表示函数 $f$ 相对于 $x$ 的偏导数的雅克比矩阵：
$$A_{[i,j]}=\frac{\partial f_{[i]}}{\partial x_{[j]}}({\hat{x}}_{k-1},0,0)$$
$W$ 表示函数 $f$ 相对于 $w$ 的偏导数的雅克比矩阵：
$$W_{[i,j]}=\frac{\partial f_{[i]}}{\partial w_{[j]}}({\hat{x}}_{k-1},0,0)$$
$H$ 表示函数 $h$ 相对于 $x$ 的偏导数的雅克比矩阵：
$$H_{[i,j]}=\frac{\partial h_{[i]}}{\partial x_{[j]}}(\overline{{\hat{x}}_k},0,0)$$
$V$ 表示函数 $h$ 相对于 $v$ 的偏导数的雅克比矩阵：
$$V_{[i,j]}=\frac{\partial h_{[i]}}{\partial v_{[j]}}(\overline{{\hat{x}}_k},0,0)$$
注意因为 $x$ 是随着 $k$ 变化的，所以上面的雅克比矩阵 $A,W,H,V$ 也都是随着 $k$ 变化的（应该是有下标的，在这里忽略了，下面的公式也都忽略了），和线性卡尔曼滤波算法是不同的。

然后可以定义先验估计误差：
$$\overline{e_{x_k}}\equiv x_k-\overline{{\hat{x}}_k}$$
同时定义测量残差：
$$\overline{e_{z_k}}\equiv z_k-{\bar{z}}_k$$
把上面的新模型带入得到：
$$\begin{gathered} \overline{e_{x_k}}\approx A(x_{k-1}-{\hat{x}}_{k-1})+{\epsilon }_k \\ \begin{array}{rl}\overline{e_{z_k}}& \approx H(x_k-\overline{{\hat{x}}_k})+{\eta }_k\\ & \approx H\overline{e_{x_k}}+{\eta }_k\end{array} \end{gathered}$$
上面提到的 $\epsilon$ 和 $\eta$ 表示新的随机变量，分别服从：
$$\begin{gathered} p({\epsilon }_k)\sim N(0,WQ{W}^T) \\ p({\eta }_k)\sim N(0,VR{V}^T) \end{gathered}$$
现在是不是感觉上面的公式特别想线性卡尔曼滤波里面的基本模型，只不过在这里是估计误差 $\overline{e_{x_k}}$ 而不是状态 $x_k$ 。假设估计的结果是 ${\hat{e}}_k$ ，那么可以获得后验状态估计：
$${\hat{x}}_k=\overline{{\hat{x}}_k}+{\hat{e}}_k$$
然后又有：
$$p(\overline{e_{x_k}})\sim N(0,E[\overline{e_{x_k}}{\bar{e}}_{x_k}^T])$$
可以设置初始的$\overline{e_{x_k}}=0$ ，利用基本的卡尔曼滤波方法去推导，得到：
$${\hat{e}}_k=K_k\overline{e_{z_k}}$$
这个求解过程就转换为了线性卡尔曼滤波算法了，那么最后的状态的估计（带入上面的公式）：
$$\begin{array}{rl}{\hat{x}}_k& =\overline{{\hat{x}}_k}+{\hat{e}}_k\\ & =\overline{{\hat{x}}_k}+K_k\overline{e_{z_k}}\\ & =\overline{{\hat{x}}_k}+K_k(z_k-{\bar{z}}_k)\\ & =\overline{{\hat{x}}_k}+K_k(z_k-h(\overline{{\hat{x}}_k},0))\end{array}$$
其他三个公式很自然的就是上面利用基本卡尔曼滤波算法求解 $\overline{e_{x_k}}$ 的基本公式。

最后得到了上面五个方程式，是经典的扩展卡尔曼滤波的五个基本公式，利用这五个公式就可以完成卡尔曼滤波的求解过程。根据迭代过程分成两个方程集合，一个是time update方程，一个是measurement update方程，如下：

Time Update	Measurement Update
$\overline{{\hat{x}}_k}=f({\hat{x}}_{k-1},0,0)$	$K_k=\overline{P_k}{H}_k^T(H_k\overline{P_k}{H}_k^T+V_{k}R{V}_k^T{)}^{-1}$
$\overline{P_k}=A_k{P}_{k-1}{A}_k^T+W_{k}Q{W}_k^T$	${\hat{x}}_k=\overline{{\hat{x}}_k}+K_k(z_k-h(\overline{{\hat{x}}_k},0))$
	$P_k=(I-K_k{H}_k)\overline{P_k}$

迭代过程

得到上面的基本公式以后，就可以计算计算后面的状态，但是公式中有一些参数是未知的，需要定义，如下：

$u$ 变量一般来说是被忽略的；

$f$ 和 $h$ 函数是根据实际提供的；

$R$ 一般来说是相对简单可以求到的，就是仪器的噪声协方差；

$Q$ 就很难直接的获得，也没有特定的方法，一般是系统过程比较稳定，也就是没有过程噪声；

$A,W,H,V$ 等雅克比矩阵是根据函数求得的，是随时变化的；

$z$ 所有状态的的测量值都是由仪器测量得到的是已知的；

对于$R$ 和 $Q$ 的取值来说不是特别重要，虽然可能这两个参数估计的不是很准确（不可能符合纯粹的高斯或者正太分布），但是实际情况中卡尔曼滤波算法也可以得到整的比较准确态估计，但是需要记住的是：

The better you estimate the noise parameters, the better estimates you get.

那么这样只需要传入基本的 ${\hat{x}}_0$ 和 $P_0$ 就可以了，一般来说需要进行指定。对于 ${\hat{x}}_0$ 来说不需要什么技巧，因为随着卡尔曼滤波算法的运行，$x$ 会逐渐的收敛。但是对于 $P_0$ ，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的 ${\hat{x}}_0$ 是系统最优的，从而使算法不能收敛。

算法迭代过程和基本卡尔曼滤波类似。