从贝叶斯滤波到扩展卡尔曼滤波

0 前言

扩展卡尔曼滤波（Extended Kalman Filter，EKF）是标准卡尔曼滤波在非线性情形下的一种扩展形式，它是一种高效率的递归滤波器（自回归滤波器）。

EKF 的基本思想是利用泰勒级数展开将非线性系统的状态转移函数 $f(x)$ 和（或）观测函数 $h(x)$ 线性化，然后采用卡尔曼滤波框架对信号进行滤波，因此它是一种次优滤波。

1 贝叶斯滤波的三大概率密度函数

在此前的文章《从概率到贝叶斯滤波》和《从贝叶斯滤波到卡尔曼滤波》中，已经讲到贝叶斯滤波的先验概率密度函数、似然概率密度函数和后验概率密度函数：

(1) 先验概率密度函数
$$f_{X_k}^{-}(x)={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v$$

(2) 似然概率密度函数
$$f_{Y_k|X_k}(y_k|x)=f_{R_k}[y_k-h(x)]$$

(3) 后验概率密度函数
$$f_{X_k}^{+}(x)={\eta }_k\cdot f_{R_k}[y_k-h(x)]\cdot f_{X_k}^{-}(x)$$

其中，后验概率密度函数中的归一化常数 ${\eta }_k$ 为：
$${\eta }_k={\left\{{\int }_{-\infty }^{+\infty }{f}_{R_k}[y_k-h(x)]f_{X_k}^{-}(x)\mathrm{d}x\right\}}^{-1}$$

2 扩展卡尔曼滤波的假设

扩展卡尔曼滤波以贝叶斯滤波为理论基础，并作了五个前提假设。

2.1 与卡尔曼滤波相同的假设

(1) 假设一：状态量服从正态分布
$$X\sim \mathcal{N}({\mu }_X,{\sigma }_X^2)$$

(2) 假设二：观测量服从正态分布
$$Y\sim \mathcal{N}({\mu }_Y,{\sigma }_Y^2)$$

(3) 假设三：过程噪声服从均值为 0 的正态分布
$$Q\sim \mathcal{N}(0,{\sigma }_Q^2)$$

(4) 假设四：观测噪声服从均值为 0 的正态分布
$$R\sim \mathcal{N}(0,{\sigma }_R^2)$$

2.2 与卡尔曼滤波不同的假设

(5) 假设五：状态转移函数和（或）观测函数为非线性函数

在卡尔曼滤波的前提假设中，认为状态方程中的状态转移函数 $f(x)$ 以及观测方程中的观测函数 $h(x)$ 均为线性函数。基于这种线性假设，存在常数或常矩阵 $F$，使得 $f(x)$ 可以写成卡尔曼滤波中的线性形式，存在常数或常矩阵 $H$，使得 $h(x)$ 也可以写成卡尔曼滤波中的线性形式。

不同于标准卡尔曼滤波，扩展卡尔曼滤波处理的是非线性系统，假设系统的状态转移函数和（或）观测函数为非线性函数。

3 扩展卡尔曼滤波的公式推导

3.1 预测步的两个公式

当状态转移函数为线性函数时，扩展卡尔曼滤波的预测步与标准卡尔曼滤波相同；当状态转移函数为非线性函数时，扩展卡尔曼滤波的预测步采用下面的推导过程。

根据假设一，$k-1$ 时刻状态量 $X_{k-1}$ 服从均值为 ${\mu }_{k-1}^{+}$，方差为 ${{\sigma }_{k-1}^{+}}^2$ 的正态分布：
$$X_{k-1}\sim \mathcal{N}({\mu }_{k-1}^{+},{{\sigma }_{k-1}^{+}}^2)$$

$X_{k-1}$ 的后验概率密度函数为：
$$f_{X_{k-1}}^{+}=\frac{1}{{\sigma }_{k-1}^{+}\sqrt{2\pi }}{e}^{-\frac{(x-{\mu }_{k-1}^{+}{)}^2}{2{{\sigma }_{k-1}^{+}}^2}}$$

对状态转移函数 $f(x)$ 在 $X_{k-1}$ 的后验估计处 ${\hat{x}}_{k-1}^{+}$（即 ${\mu }_{k-1}^{+}$）进行一阶泰勒级数展开：
$$f(X_{k-1})=f({\hat{x}}_{k-1}^{+})+f^{\prime }({\hat{x}}_{k-1}^{+})(X_{k-1}-{\hat{x}}_{k-1}^{+})+o(X_{k-1}-{\hat{x}}_{k-1}^{+})$$

其中，$o(X_{k-1}-{\hat{x}}_{k-1}^{+})$ 为高阶无穷小量，对其进行舍弃，便可得到状态转移函数 $f(x)$ 的近似形式：
$$\begin{array}{rl}f(X_{k-1})& \approx f({\hat{x}}_{k-1}^{+})+f^{\prime }({\hat{x}}_{k-1}^{+})(X_{k-1}-{\hat{x}}_{k-1}^{+})\\ & \approx f^{\prime }({\hat{x}}_{k-1}^{+})(X_{k-1})+[f({\hat{x}}_{k-1}^{+})-f^{\prime }({\hat{x}}_{k-1}^{+}){\hat{x}}_{k-1}^{+}]\end{array}$$

令 $F_j=f^{\prime }({\hat{x}}_{k-1}^{+}),C_F=f({\hat{x}}_{k-1}^{+})-f^{\prime }({\hat{x}}_{k-1}^{+}){\hat{x}}_{k-1}^{+}$，则近似有：
$$f(X_{k-1})\approx F_j\ast X_{k-1}+C_F$$

结合假设三与上式，$k$ 时刻状态量 $X_k$ 的先验概率密度函数为：
$$\begin{array}{rl}{f}_{X_k}^{-}(x)& ={\int }_{-\infty }^{+\infty }{f}_{Q_k}[x-f(v)]f_{X_{k-1}}^{+}(v)\mathrm{d}v\\ & ={\int }_{-\infty }^{+\infty }{\sigma }_{Q_k}\end{array}$$