扩展卡尔曼滤波器

扩展卡尔曼滤波器

---

---

前言

卡尔曼滤波器作为状态估计的一种重要方法，有着其使用范围的局限性。即，卡尔曼滤波器仅适用于线性系统中。
这是因为卡尔曼滤波器使用的前提是先验分布是高斯似然分布的共轭先验。这就要求先验分布也是高斯的，并且可以确保后验分布与先验分布是属于同一个分布族的，即同属于高斯分布。
根据高斯分布的性质，高斯分布在经过一个线性系统后，仍为高斯分布。但是高斯分布在经过一个非线性系统后，则无法确保其仍为高斯分布。直接使用线性卡尔曼滤波器的公式便不再可行。
因此，需要找到一种方法，使得在非线性系统下仍可以使用卡尔曼滤波器公式。

---

一、泰勒展开

扩展卡尔曼滤波器的主要思想是对非线性系统做线性化处理，转变为线性系统，并在此基础上实现预测和滤波。其使用的线性化处理方式是泰勒展开。
先回顾以下泰勒展开的公式。假设有一函数为$f(x)$，在$x_0$处做泰勒展开后，可以得到
$$f(x)=f(x_0)+\frac{\partial f}{\partial x}(x-x_0)+...+o(x-x_0)$$
其中，$o(x-x_0)$为高阶项。
对于多元函数，泰勒展开公式类似与一元函数的公式。假设有一个多元函数$f(x,y)$，在$x_0$,$y_0$做泰勒展开后，可以得到
$$f(x,y)=f(x_0,y_0)+\frac{\partial f}{\partial x}(x-x_0)+\frac{\partial f}{\partial x}(y-y_0)+...+o(x-x_0)+o(y-y_0)$$

二、扩展卡尔曼滤波器

1.推导

首先处理状态转移函数的线性化。
假设$k+1$时刻的状态向量表示为
$$x_{k+1}=f(x_k,u_k,v_k)$$
其中，$f(\cdot )$为状态转移方程，$x_k$为$k$时刻的状态向量，$u_k$为k时刻的机动项，$v_k$为k时刻的过程噪声。假设过程噪声服从均值为0的高斯分布，$v_k\sim N(0,Q)$。由于在实际情况下，我们无法获取$k$时刻的状态向量，因此使用$k$时刻的状态向量估计值${\hat{x}}_{k|k}$代替。同时，为了推导方便，暂时不考虑机动项$u_k$，并令过程噪声$v_k$为0。由此得到下式
$$\begin{array}{rl}{x}_{k+1}& =f(x_k,v_k)\\ & =f({\hat{x}}_{k|k},0)+\frac{\partial f}{\partial x_k{|}_{x_k={\hat{x}}_{k|k}}}(x_k-{\hat{x}}_{k|k})+\frac{\partial f}{\partial v_k{|}_{v_k=0}}(v_k-0)+...+o(x_k-{\hat{x}}_{k|k})+o(v_k-0)\end{array}$$
令$\frac{\partial f}{\partial x_k{|}_{x_k={\hat{x}}_{k|k}}}=F$,$\frac{\partial f}{\partial v_k{|}_{v_k=0}}=V$,并忽略高阶项，仅保留一阶项，则上式可简写为
$$x_{k+1}\approx f({\hat{x}}_{k|k},0)+F(x_k-{\hat{x}}_{k|k})+V{v}_k$$
由上式可推导出预测状态向量的表达式
$${\hat{x}}_{k+1|k}\approx E\left\{f({\hat{x}}_{k|k},0)+F(x_k-{\hat{x}}_{k|k})+V{v}_k\right\}=f({\hat{x}}_{k|k},0)$$
预测误差协方差矩阵的表达式为
$$\begin{array}{rl}{P}_{k+1|k}& =E\left\{(x_{k+1}-{\hat{x}}_{k+1|k})(x_{k+1}-{\hat{x}}_{k+1|k}{)}^T\right\}\\ & =E\left\{(F(x_k-{\hat{x}}_{k|k})+V{v}_k)(F(x_k-{\hat{x}}_{k|k})+V{v}_k{)}^T\right\}\\ & =F{P}_{k|k}{F}^T+VQ{V}^T\end{array}$$
同理，接下来再对观测函数做线性化。
假设$k+1$时刻的观测向量表示为
$$z_{k+1}=h(x_{k+1},w_{k+1})$$
其中，$h(\cdot )$为观测函数，$x_k$为$k+1$时刻的状态向量，$w_k$为k+1时刻的观测噪声。假设观测噪声服从均值为0的高斯分布，$w_k\sim N(0,R)$。使用$k+1$时刻的状态向量预测值${\hat{x}}_{k+1|k}$代替$x_{k+1}$，同时令$w_k$为0。由此得到下式
$$\begin{array}{rl}{z}_{k+1}& =h(x_{k+1},w_{k+1})\\ & =h({\hat{x}}_{k+1|k},0)+\frac{\partial h}{\partial x_{k+1}{|}_{x_{k+1}={\hat{x}}_{k+1|k}}}(x_{k+1}-{\hat{x}}_{k+1|k})+\frac{\partial h}{\partial w_{k+1}{|}_{w_{k+1}=0}}(w_{k+1}-0)+...+o(x_{k+1}-{\hat{x}}_{k+1|k})+o(w_{k+1}-0)\end{array}$$
令$\frac{\partial h}{\partial x_{k+1}{|}_{x_{k+1}={\hat{x}}_{k+1|k}}}=H$,$\frac{\partial f}{\partial w_k{|}_{w_k=0}}=W$,并忽略高阶项，仅保留一阶项，则上式可简写为
$$z_{k+1}\approx h({\hat{x}}_{k+1|k},0)+H(x_{k+1}-{\hat{x}}_{k+1|k})+W{w}_{k+1}$$
由上式可推导出预测观测向量的表达式
$${\hat{z}}_{k+1}\approx E\left\{h({\hat{x}}_{k+1|k},0)+H(x_{k+1}-{\hat{x}}_{k+1|k})+W{w}_{k+1}\right\}=h({\hat{x}}_{k+1|k},0)$$
则
$$\begin{array}{rl}{P}_{xz,k+1|k}& =E\left\{(x_{k+1}-{\hat{x}}_{k+1})(z_{k+1}-{\hat{z}}_{k+1|k}{)}^T\right\}\\ & =E\left\{({\hat{x}}_{k+1|k}-z_{k+1})(H(x_{k+1}-{\hat{x}}_{k+1|k})+W{w}_{k+1}{)}^T\right\}\\ & =P_{k+1|k}H\end{array}$$

$$\begin{array}{rl}{P}_{zz,k+1|k}& =E\left\{(z_{k+1}-{\hat{z}}_{k+1|k})(z_{k+1}-{\hat{z}}_{k+1|k}{)}^T\right\}\\ & =E\left\{(H(x_{k+1}-{\hat{x}}_{k+1|k})+W{w}_{k+1})(H(x_{k+1}-{\hat{x}}_{k+1|k})+W{w}_{k+1}{)}^T\right\}\\ & =H{P}_{k+1|k}{H}^T+W{R}_{k+1}{W}^T\end{array}$$
可以得到卡尔曼增益为
$$K=P_{xz}{P}_{zz}^{-1}=P_{k+1|k}H(H{P}_{k+1|k}{H}^T+W{R}_{k+1}{W}^T{)}^{-1}$$

最后通过滤波步骤，得到后验的状态空间均值与方差
$${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+K(z_{k+1}-{\hat{z}}_{k+1|k})$$
$$P_{k+1|k+1}=(I-KH)P_{k+1|k}(I-KH{)}^T+K{R}_{k+1}{K}^T$$

2.总结

扩展卡尔曼滤波器也是由五个公式构成的

预测步：

$${\hat{x}}_{k+1|k}=f({\hat{x}}_{k|k},0)$$
$$P_{k+1|k}=F{P}_{k|k}{F}^T+VQ{V}^T$$

卡尔曼增益计算：

$$K=P_{xz}{P}_{zz}^{-1}=P_{k+1|k}H(H{P}_{k+1|k}{H}^T+W{R}_{k+1}{W}^T{)}^{-1}$$

滤波步：

$${\hat{x}}_{k+1|k+1}={\hat{x}}_{k+1|k}+K(z_{k+1}-{\hat{z}}_{k+1|k})$$
$$P_{k+1|k+1}=(I-KH)P_{k+1|k}(I-KH{)}^T+K{R}_{k+1}{K}^T$$