扩展卡尔曼滤波原理

摘要

本文在线性最优滤波的基础上结合正交投影详细介绍扩展卡尔曼滤波的数学原理并给出了数学推导过程。

基础背景

扩展卡尔曼滤波[1]作为卡尔曼滤波的扩展，在非线性系统的应用中取得了相当的成功。阅读本文前需要读者先行阅读卡尔曼滤波原理[2]并理解其中的正交投影的数学原理。本文数学推导部分采用的标记与卡尔曼论文的标记类似。

符号约定

本文主要处理离散系统，即信号将会在相同的时间间隔内被采样获取得到，故本文中涉及时间的符号$t$为具备整数型意义。向量作为$n \times 1$维的矩阵，为了更好的与矩阵区分开，将使用小写粗体字母表示，如$\pmb{a}, \pmb{b},…, \pmb{z}$；矩阵采用大写粗体字母表示，如$\pmb{A},\pmb{B},…,\pmb{Z}$。

动态系统的数学模型

一个非线性离散系统可以用如下数学模型来表示：

$$\pmb{x}(t)=\pmb{f}(\pmb{x}(t-1))+\pmb{u}(t-1)$$ $$\pmb{y}(t)=\pmb{m}(\pmb{x}(t))$$

其中$\pmb{x}(t)$为$n \times 1$维的随机过程向量，$\pmb{u}(t)$为均值为零的高斯随机过程。

非线性问题的解

问题: 考虑如下动态系统

$$\pmb{x}(t)=\pmb{f}(\pmb{x}(t-1))+\pmb{u}(t-1)$$ $$\pmb{y}(t)=\pmb{m}(\pmb{x}(t))$$

其中$\pmb{x}(t)$为$n \times 1$维的随机过程向量，$\pmb{u}(t)$为均值为零的高斯随机过程。给定观测结果$\pmb{y}(t_0),…, \pmb{y}(t)$的值，找到$t$时刻$\pmb{x}(t)$的最优估计值

$$\pmb{x}^*(t|t)$$

使得估计误差的最小均方值达到最小。

状态预测方程

根据定义

$$\pmb{x}^*(t|t-1) = \hat{E}\left[\pmb{x}(t)|\mathcal{Y}(t-1)\right]$$

将式$(1)$代入上述式子得到

$$\begin{align*} &\pmb{x}^*(t|t-1) = \hat{E}[\pmb{f}(\pmb{x}(t-1))\\ &+\pmb{u}(t-1)|\mathcal{Y}(t-1)] \tag{2} \end{align*}$$

现对$f(\pmb{x}(t-1))$进行泰勒展开有

$$\begin{align*} &\pmb{f}(\pmb{x}(t-1)) = \pmb{f}(\pmb{x}^*(t-1|t-1)) \\ &+\pmb{J}_f(\pmb{x}^*(t-1|t-1))(\pmb{x}(t-1)-\pmb{x}^*(t|t-1)) \end{align*}$$

代入式$(2)$得到

$$\begin{align} &\pmb{x}^*(t|t-1) = \pmb{f}(\pmb{x}^*(t-1|t-1)) \\ &+ \pmb{J}_f(\pmb{x}^*(t-1|t-1)\\ &\times E\left[ \widetilde{\pmb{x}}(t-1|t-1)|\mathcal{Y}(t-1) \right] \\ &=\pmb{f}(\pmb{x}^*(t-1|t-1)) \tag{3} \end{align}$$

式子$(3)$称为状态预测方差。

滤波增益系数

假定$\pmb{x}^*(t|t-1)$已知，于是有

$$\begin{align} \pmb{x}^*(t|t) &= \hat{E}[\pmb{x}(t)|\mathcal{Y}(t)] \\ &=\hat{E}[\pmb{x}(t)|\mathcal{Y}(t-1)]+\hat{E}[\pmb{x}|\mathcal{Z}(t)] \\ &=\pmb{x}^*(t|t-1)+\hat{E}[\pmb{x}(t)|\mathcal{Z}(t)] \tag{4} \end{align}$$

注意到

$$\hat{E}[\pmb{x}(t)|\mathcal{Z}(t)] = \pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1) \tag{5}$$

上式右边

$$\begin{align*} \widetilde{\pmb{y}}(t|t-1) &= \pmb{y}(t)-\bar{\pmb{y}}(t|t-1) \\ &= \pmb{y}(t)-\pmb{m}(\pmb{x}^*(t|t-1)) \tag{6} \end{align*}$$

将式$(5)$与式$(6)$代入式$(4)$得到

$$\begin{align*} &\pmb{x}^*(t|t) = \pmb{x}^*(t|t-1) \\ &+ \pmb{\Delta}^*(t)\left[\pmb{y}(t)-\pmb{m}(\pmb{x}^*(t|t-1))\right] \tag{7} \end{align*}$$

注意到

$$\widetilde{\pmb{x}}(t|\mathcal{Z}(t)) = \pmb{x}(t)-\hat{E}[\pmb{x}(t)|\mathcal{Z}(t)]$$

与

$$\widetilde{\pmb{y}}(t|t-1)$$

是正交的，两者进行正交乘积有

$$\begin{align} 0 &= E\left[\left[\pmb{x}(t)-\pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1)\right]\widetilde{\pmb{y}}^T(t|t-1)\right] \\ &= E[\pmb{x}(t)\widetilde{\pmb{y}}^T(t|t-1)]\\ &\;\;\;\;-\pmb{\Delta}^*(t)E[\widetilde{\pmb{y}}(t|t-1)\widetilde{\pmb{y}}^T(t|t-1)] \tag{8} \end{align}$$

注意到

$$\pmb{x}(t)=\bar{\pmb{x}}(t|t-1)+\widetilde{\pmb{x}}(t|t-1)$$

同时

$$\bar{\pmb{x}}(t|t-1)$$

与

$$\mathcal{Z}(t)$$

是正交的，式$(8)$可以化简为

$$\begin{align*} 0=&E[\widetilde{\pmb{x}}(t|t-1)\widetilde{\pmb{y}}^T(t|t-1)] \\ &-\pmb{\Delta}^*(t)E[\widetilde{\pmb{y}}(t|t-1)\widetilde{\pmb{y}}^T(t|t-1)] \tag{9} \end{align*}$$

对$\pmb{y}(t)$在

$$\widetilde{\pmb{x}}(t|t-1)$$

进行泰勒展开得到

$$\begin{align*} \pmb{y}(t) = &\pmb{m}(\pmb{x}^*(t|t-1))\\ &+\pmb{J}_m(\pmb{x}^*(t|t-1))(\pmb{x}(t)-\pmb{x}^*(t|t-1))) \end{align*}$$

代入式子$(6)$得到

$$\begin{align} &\widetilde{\pmb{y}}(t|t-1) = \pmb{J}_m(\pmb{x}^*(t|t-1))(\pmb{x}(t) \\ &\;\;\;\;\;\;\;\;\;\;\;\;\;-\pmb{x}^*(t|t-1))) \\ &\;\;\;\;\;=\pmb{J}_m(\pmb{x}^*(t|t-1))\widetilde{\pmb{x}}(t|t-1) \tag{10} \end{align}$$

将式子$(10)$代入式子$(9)$有

$$\begin{align} &0=E\left[\widetilde{\pmb{x}}(t|t-1)\widetilde{\pmb{x}}^T(t|t-1)\right]\times \\ &\pmb{J}^T_m(\pmb{x}^*(t|t-1))-\pmb{\Delta}^*(t)\pmb{J}_m(\pmb{x}^*(t|t-1))\\ &\times E\left[ \widetilde{\pmb{x}}(t|t-1)\widetilde{\pmb{x}}^T(t|t-1) \right]\\ &\;\;\;\;\;\;\;\;\;\;\;\times \pmb{J}^T_m(\pmb{x}^*(t|t-1)) \\ &= \pmb{P}_{(-)}(t)\pmb{J}^T_m(\pmb{x}^*(t|t-1))- \\ &\;\;\;\;\;\;\pmb{\Delta}^*(t)\pmb{J}_m(\pmb{x}^*(t|t-1))\pmb{P}_{(-)}(t)\\ &\;\;\;\times \pmb{J}^T_m(\pmb{x}^*(t|t-1)) \tag{11} \end{align}$$

记

$$\pmb{J}_m(\pmb{x}^*(t|t-1)) = \pmb{J}_{m(-)}$$

式$(11)$可以简写为

$$0 =\pmb{P}_{(-)}(t)\pmb{J}^T_{m(-)}-\pmb{\Delta}^*(t)\pmb{J}_{m(-)}\pmb{P}_{(-)}(t)\pmb{J}^T_{m(-)}$$

解得非线性系统的卡尔曼增益

$$\pmb{\Delta}^* = \pmb{P}_{(-)}\pmb{J}^T_{m(-)}\left[ \pmb{J}_{m(-)}\pmb{P}_{(-)}\pmb{J}^T_{m(-)} \right]^{-1} \tag{12}$$

状态更新方程

注意到式子$(4)$有

$$\begin{align*} \pmb{x}^*(t|t) &=\pmb{x}^*(t|t-1)+\hat{E}[\pmb{x}(t)|\mathcal{Z}(t)] \\ &= \hat{E}[\pmb{x}(t)|\mathcal{Y}(t-1)]\\ &\;\;\;\;+\pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1) \\ &= \hat{E}\left[\pmb{f}(\pmb{x}(t-1))|\mathcal{Y}(t-1)\right]\\ &\;\;\;\;\;+\pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1) \tag{13} \end{align*}$$

现在对$\pmb{f}(\pmb{x}(t-1))$进行泰勒展开，有

$$\begin{align*} \pmb{f}(\pmb{x}(t-1)) & = \pmb{f}(\pmb{x}^*(t-1|t-1))\\ &+\pmb{J}_f(\pmb{x}^*(t-1|t-1))\widetilde{\pmb{x}}(t-1|t-1) \end{align*}$$

代入式$(13)$有

$$\begin{align*} \pmb{x}^*(t|t) &= \pmb{f}(\pmb{x}^*(t-1|t-1)) \\ &+ \pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1) \tag{14} \end{align*}$$

上述式子为状态更新方程。

先验估计误差协方差矩阵

记$\pmb{J}_{f(+)}(t-1)=\pmb{J}_f(\pmb{x}^*(t-1|t-1))$根据定义

$$\begin{align*} \widetilde{\pmb{x}}(t|t-1) &= \pmb{x}(t)-\pmb{x}^*(t|t-1) \\ &=\pmb{f}(\pmb{x}(t-1))-\pmb{x}^*(t|t-1)+ \pmb{u}(t-1)\\ &=\pmb{J}_{f(+)}(t-1)(\widetilde{\pmb{x}}(t-1|t-1))\\ &\;\;\;\;\;\;\;\;+\pmb{u}(t-1) \end{align*}$$

注意到$\pmb{Q}(t-1)=E[\pmb{u}(t-1)\pmb{u}^T(t-1)]$由协方差的定义得到

$$\begin{align*} \pmb{P}^*_{(-)}(t) &= E\left[\widetilde{\pmb{x}}(t|t-1)\widetilde{\pmb{x}}^T(t|t-1)\right] \\ &=\pmb{J}_{f(+)}(t-1)\pmb{P}^*_{(+)}(t-1)\\ &\times\pmb{J}_{f(+)}(t-1) + \pmb{Q}(t-1) \tag{15} \end{align*}$$

后验估计误差协方差矩阵

记

$$\begin{align*} &\pmb{J}_{f(+)}(t-1)=\pmb{J}_f(\pmb{x}^*(t-1|t-1)) \\ &\pmb{J}_{m(-)}(t-1)=\pmb{J}_m(\pmb{x}^*(t|t-1)) \end{align*}$$

根据定义

$$\begin{align*} \widetilde{\pmb{x}}(t|t)=\pmb{x}(t)-\pmb{x}^*(t|t) \end{align*}$$

将式$(14)$代入上述式子并对$\pmb{x}(t)$利用泰勒展开得到

$$\begin{align*} \widetilde{\pmb{x}}(t|t)=&\pmb{J}_{f(+)}(t-1)\widetilde{\pmb{x}}(t-1|t-1)\\ &-\pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1)+\pmb{u}(t-1) \end{align*}$$

将式子$(3)$与式$(10)$代入上述式子得到

$$\begin{align*} \widetilde{\pmb{x}}(t|t)&=\pmb{J}_{f(+)}(t-1)\widetilde{\pmb{x}}(t-1|t-1)\\ &-\pmb{\Delta}^*(t) \pmb{J}_{m(-)}(t)[ \pmb{J}_{f(+)}(t-1)\widetilde{\pmb{x}}(t-1|t-1)\\ &\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;\;+\pmb{u}(t-1) ]+\pmb{u}(t-1) \end{align*}$$

提取公因式并整理得到

$$\begin{align*} \widetilde{\pmb{x}}(t|t) &=\left[ \pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t) \right]\\ &\times \pmb{J}_{f(+)}(t-1)\widetilde{\pmb{x}}(t-1|t-1)\\ &+[\pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t)]\pmb{u}(t-1) \end{align*}$$

由协方差的定义得到

$$\begin{align*} &\pmb{P}^*_{(+)}(t) = E\left[\widetilde{\pmb{x}}(t|t)\widetilde{\pmb{x}}^T(t|t)\right] \\ &=\left[ \pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t) \right]\pmb{J}_{f(+)}(t-1)\\ &\;\;\;\;\;\;\;\;\;\;\times \pmb{P}^*_{(+)}(t-1)\\ &\times\pmb{J}^T_{f(+)}(t-1)\left[ \pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t) \right]^T\\ &+[\pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t)]\pmb{Q}(t-1)[\pmb{I} - \\ &\;\;\;\pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t)]^T\\ &=\left[\pmb{I}-\pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t)\right]\pmb{P}^*_{(-)}(t) \tag{16} \end{align*}$$

离散非线性卡尔曼滤波公式汇总

定理: 考虑问题1，在给定非线性系统的观测值$\pmb{y}(t_0),…\pmb{y}(t)$的条件下，$t$时刻状态变量$\pmb{x}(t)$的最优估计$\pmb{x}^*(t|t)$由如下公式给出

$$\begin{align*} \pmb{x}^*(t|t) = \pmb{f}(\pmb{x}^*(t-1|t-1)) + \pmb{\Delta}^*(t)\widetilde{\pmb{y}}(t|t-1) \end{align*}$$

其中$\pmb{\Delta}^*(t)$由如下公式给出

$$\begin{align*} \pmb{\Delta}^*(t) = \pmb{P}^*_{(-)}(t)\pmb{J}^T_{h(-)}\left[ \pmb{J}_{h(-)}\pmb{P}^*_{(-)}(t)\pmb{J}^T_{h(-)} \right]^{-1} \end{align*}$$

后验估计误差为

$$\begin{align*} \widetilde{\pmb{x}}(t|t) &=\left[ \pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t) \right]\pmb{J}_{f(+)}(t-1)\\ &\;\;\;\;\;\;\;\times \widetilde{\pmb{x}}(t-1|t-1)\\ &+[\pmb{I} - \pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t)]\pmb{u}(t-1) \end{align*}$$

先验误差协方差矩阵与后验误差协方差矩阵分别由下列公式给出

$$\begin{align*} \pmb{P}^*_{(-)}(t)=\pmb{J}_{f(+)}(t-1)\pmb{P}^*_{(+)}(t-1)&\pmb{J}_{f(+)}(t-1) \\ &+ \pmb{Q}(t-1) \end{align*}$$ $$\pmb{P}^*_{(+)}(t)=\left[\pmb{I}-\pmb{\Delta}^*(t)\pmb{J}_{m(-)}(t)\right]\pmb{P}^*_{(-)}(t)$$

参考文献

[1] A. H. Jazwinski, Stochastic Processes and Filtering Theory, Academic Press, Inc, vol. 64, 1970. [2] Kalman R E. A new approach to linear filtering and prediction problems[J]. 1960.

---

• Next
  矩阵迹的性质