【扩展卡尔曼滤波理论推导与实践】【理论】【2/3 公式推导】

本节默认你能够完整推导标准卡尔曼滤波，将会简化一些推导的描述。如果你还不会完整推导标准卡尔曼滤波，请先从 【卡尔曼滤波理论推导与实践】系列开始看起。

---

非线性系统

扩展卡尔曼滤波解决的是非线性系统，其非线性状态空间方程为：
$$\{\begin{array}{l}\overrightarrow{z_{k模}}=\mathrm{f}(\overrightarrow{z_{k-1模}},F_{k-1})\\ \overrightarrow{y_{k测}}=\mathrm{h}(\overrightarrow{z_{k测}})\end{array}$$
$\mathrm{f},\mathrm{h}$函数是向量函数，有多个输出$\left[\begin{array}{c}{f}_1\\ ...\\ f_n\end{array}\right]$，$n$是$\vec{z}$的维数，比如追踪平面图像物体坐标和速度，$\vec{z}=\left[\begin{array}{c}x\\ y\\ v_x\\ v_y\end{array}\right]$。
其非线性真实系统模型为：
$$\{\begin{array}{l}\overrightarrow{z_{k真}}=\mathrm{f}(\overrightarrow{z_{k-1真}},F_{k-1},\overrightarrow{w_{k-1}})\\ \overrightarrow{y_{k测}}=\mathrm{h}(\overrightarrow{z_{k真}},\overrightarrow{v_k})\end{array}$$

因为非线性函数的期望不能用协方差矩阵描述，无法用卡尔曼滤波的套路进行推导，所以要进行线性化近似，这就是扩展卡尔曼滤波与标准卡尔曼滤波的唯一区别。

泰勒展开

扩展卡尔曼滤波使用泰勒展开进行线性化近似，扩展卡尔曼滤波是一个二元向量输入多输出系统，比如输入为$[\overrightarrow{z_{k-1模}},\overrightarrow{w_{k-1}}]$，这有两列向量；输出为$\overrightarrow{z_{k真}}$这个向量。
这里有两个公式要提前知道：

• 单输出的二元函数在$(x_0,y_0)$点的泰勒展开公式：
  $$f(x,y)=f(x_0,x_0)+\frac{df}{dx}{|}_{x_0,y_0}\ast (x-x_0)++\frac{df}{dy}{|}_{x_0,y_0}\ast (y-y_0)$$
• 多输出函数对输入向量求导公式：
  $$\frac{d\vec{f}(\vec{x})}{d\vec{x}}=\left[\begin{array}{cccc}\frac{d{f}_1}{d{x}_1}& \frac{d{f}_1}{d{x}_2}& \cdots & \frac{d{f}_1}{d{x}_m}\\ \frac{d{f}_2}{d{x}_1}& \frac{d{f}_2}{d{x}_2}& \cdots & \frac{d{f}_2}{d{x}_m}\\ ⋮& ⋮& \ddots & ⋮\\ \frac{d{f}_n}{d{x}_1}& \frac{d{f}_n}{d{x}_2}& \cdots & \frac{d{f}_n}{d{x}_m}\end{array}\right]=雅可比矩阵$$

接下来进行泰勒展开：
记$\mathrm{f}$对$\vec{z}$求导的雅可比矩阵为$\mathrm{A}$，$\mathrm{f}$对$\vec{w}$求导的雅可比矩阵为$\mathrm{W}$，$\mathrm{h}$对$\vec{z}$求导的雅可比矩阵为$\mathrm{H}$，$\mathrm{h}$对$\vec{v}$求导的雅可比矩阵为$\mathrm{V}$。
注意：

• $F_{k-1}$是给定值而不是变量，不参与泰勒展开
• 噪声的期望（平均值）是零，所以泰勒展开点的噪声值为零

可以在任意已知点（比如模型值和上轮估计值都是已知的）展开，在哪个点展开都是为了方便推导。
状态转移方程分别在$\overrightarrow{z_{k-1估}},\overrightarrow{z_{k模}}$处泰勒展开：
$$\{\begin{array}{l}\overrightarrow{z_{k模}}=f(\overrightarrow{z_{k-1估}},F_{k-1})+\mathrm{A}\ast (\overrightarrow{z_{k-1模}}-\overrightarrow{z_{k-1估}})\\ \overrightarrow{y_{k测}}=h(\overrightarrow{z_{k模}})+\mathrm{H}\ast (\overrightarrow{z_{k测}}-\overrightarrow{z_{k模}})\end{array}$$
真实系统模型分别在$(\overrightarrow{z_{k-1估}},\vec{0}),(\overrightarrow{z_{k模}},\vec{0})$泰勒展开：
$$\{\begin{array}{l}\overrightarrow{z_{k真}}=f(\overrightarrow{z_{k-1估}},F_{k-1},\vec{0})+\mathrm{A}\ast (\overrightarrow{z_{k-1真}}-\overrightarrow{z_{k-1估}})+\mathrm{W}\ast (\overrightarrow{w_{k-1}}-\vec{0})\\ \overrightarrow{y_{k测}}=h(\overrightarrow{z_{k模}},\vec{0})+\mathrm{H}\ast (\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}})+\mathrm{V}\ast (\overrightarrow{v_k}-\vec{0})\end{array}$$

卡尔曼滤波

线性化后，接下来的推导过程和标准卡尔曼滤波不能说一摸一样，只能说九分甚至十分地相似。
这里再放一遍卡尔曼滤波核心思想：

用测量值修正系统模型值，得到当前轮次的最优估计值。再将该最优估计值当作下一轮的系统模型输入，继续进行下一轮的最优估计，如此迭代进行。

要最优化估计值，首先把估计值表达式写出来：
$$\begin{array}{rlr}\overrightarrow{z_{k估}}& =\overrightarrow{z_{k模}}+{\mathrm{G}}_{\mathrm{k}}\ast (\overrightarrow{z_{k测}}-\overrightarrow{z_{k模}})& \end{array}$$
对$\mathrm{G}$矩阵进行一个变换，变出一个$\overrightarrow{y_{k测}}$出来，方便推导，令$\mathrm{G}=\mathrm{K}\ast \mathrm{H}$，于是估计值表达式变为：
$$\begin{array}{rl}\overrightarrow{z_{k估}}& =\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}\ast \mathrm{H}\ast (\overrightarrow{z_{k测}}-\overrightarrow{z_{k模}})\\ & =\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}\ast (\overrightarrow{y_{k测}}-\mathrm{H}\ast \overrightarrow{z_{k模}})\end{array}$$

卡尔曼增益

$\overrightarrow{z_{k估}}$表达式右侧除了${\mathrm{K}}_{\mathrm{k}}$不知道，其他都是已知数，继续推导${\mathrm{K}}_{\mathrm{k}}$吧。那么就是推导最优估计下的${\mathrm{K}}_{\mathrm{k}}$，就是使得估计误差协方差矩阵的迹最小，先把估计误差协方差矩阵写出来：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[\overrightarrow{e_{k估}}{\overrightarrow{e_{k估}}}^T]\\ & =E[(\overrightarrow{z_{k真}}-\overrightarrow{z_{k估}})\ast (\overrightarrow{z_{k真}}-\overrightarrow{z_{k估}}{)}^T]\end{array}$$

展开一下$\overrightarrow{z_{k真}}-\overrightarrow{z_{k估}}$:
$$\begin{array}{rl}\overrightarrow{z_{k真}}-\overrightarrow{z_{k估}}& =\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}-{\mathrm{K}}_{\mathrm{k}}\ast (\overrightarrow{y_{k测}}-\mathrm{h}(\overrightarrow{z_{k模}}))\\ & =\overrightarrow{e_{k模}}-{\mathrm{K}}_{\mathrm{k}}\ast (h(\overrightarrow{z_{k模}},0)+\mathrm{H}\ast (\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}})+\mathrm{V}\ast \overrightarrow{v_k}-h(\overrightarrow{z_{k模}}))\\ & =\overrightarrow{e_{k模}}-{\mathrm{K}}_{\mathrm{k}}\ast (\mathrm{H}\ast \overrightarrow{e_{k模}}+\mathrm{V}\ast \overrightarrow{v_k})\\ & =(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}-{\mathrm{K}}_{\mathrm{k}}\mathrm{V}\ast \overrightarrow{v_k}\end{array}$$

${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$可以继续展开了：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[((\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}-{\mathrm{K}}_{\mathrm{k}}\mathrm{V}\ast \overrightarrow{v_k})\ast ((\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}-{\mathrm{K}}_{\mathrm{k}}\mathrm{V}\ast \overrightarrow{v_k}{)}^T]\\ & =E[((\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}-{\mathrm{K}}_{\mathrm{k}}\mathrm{V}\ast \overrightarrow{v_k})\ast ({\overrightarrow{e_{k模}}}^T(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}{)}^T-{\overrightarrow{v_k}}^T{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}}}^T)]\end{array}$$
接下来将括号乘开。这里有两点有助于化简：噪声$\overrightarrow{v_k}$的期望是零，又与$\overrightarrow{e_{k模}}$相互独立，所以$\overrightarrow{v_k}$和$\overrightarrow{e_{k模}}$搭边的项都是零。
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{估}}& =E[(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H})\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}{)}^T+{\mathrm{K}}_{\mathrm{k}}\mathrm{V}\ast \overrightarrow{v_k}{\overrightarrow{v_k}}^T{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}}}^T]\\ & =(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}{)}^T+{\mathrm{K}}_{\mathrm{k}}\mathrm{VR}{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}}}^T\end{array}$$
${\mathrm{P}}_{\mathrm{k}\mathrm{估}}$的表达式也出来了，${\mathrm{K}}_{\mathrm{k}}$是自变量，想让$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$最小，即最优估计。由于$tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})$是二次函数，开口向上，极值点必定是极小值点，于是接下来准备$\frac{tr({\mathrm{P}}_{\mathrm{k}\mathrm{估}})}{d{\mathrm{K}}_{\mathrm{k}}}=0$。在算极值点前，有个矩阵求导公式需要提前知道：
$$\frac{dtr(AB{A}^T)}{d\mathrm{A}}=2AB$$
这个求导公式结合链式求导法则，就可以进行求极值点了，注意$\mathrm{VR}{\mathrm{V}}^T$看作一个整体：
$$\begin{array}{rl}\frac{d{\mathrm{P}}_{\mathrm{k}\mathrm{估}}}{d{\mathrm{K}}_{\mathrm{k}}}& =2(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}(-{\mathrm{H}}^T)+2{\mathrm{K}}_{\mathrm{k}}\mathrm{VR}{\mathrm{V}}^T\\ & =0\end{array}$$

${\mathrm{K}}_{\mathrm{k}}$这不就出来了，和标准卡尔曼滤波里的${\mathrm{K}}_{\mathrm{k}}$挺像的：
$${\mathrm{K}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T}$$

模型误差协方差矩阵

和标准卡尔曼滤波的推导套路一样，继续推导${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$。
先把表达式搞出来：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[\overrightarrow{e_{k模}}{\overrightarrow{e_{k模}}}^T]\\ & =E[(\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}})\ast (\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}{)}^T]\end{array}$$

展开一下$\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}$：
$$\begin{array}{rl}\overrightarrow{z_{k真}}-\overrightarrow{z_{k模}}& =f(\overrightarrow{z_{k-1估}},F_{k-1},0)+\mathrm{A}\ast (\overrightarrow{z_{k-1真}}-\overrightarrow{z_{k-1估}})+\mathrm{W}\ast \overrightarrow{w_{k-1}}-(f(\overrightarrow{z_{k-1估}},F_{k-1})+\mathrm{A}\ast (\overrightarrow{z_{k-1模}}-\overrightarrow{z_{k-1估}}))\\ & =\mathrm{A}(\overrightarrow{z_{k-1真}}-\overrightarrow{z_{k-1估}})+\mathrm{W}\ast \overrightarrow{w_{k-1}}\\ & =\mathrm{A}\overrightarrow{e_{k估}}+\mathrm{W}\overrightarrow{w_{k-1}}\end{array}$$

${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$继续展开，展开后的表达式和标准卡尔曼滤波里的${\mathrm{P}}_{\mathrm{k}\mathrm{模}}$也挺像的：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}& =E[(\mathrm{A}\overrightarrow{e_{k-1估}}+\mathrm{W}\overrightarrow{w_{k-1}})\ast (\mathrm{A}\overrightarrow{e_{k-1估}}+\mathrm{W}\overrightarrow{w_{k-1}}{)}^T]\\ & =E[(\mathrm{A}\overrightarrow{e_{k-1估}}+\mathrm{W}\overrightarrow{w_{k-1}})\ast ({\overrightarrow{e_{k-1估}}}^T{\mathrm{A}}^T+{\overrightarrow{w_{k-1}}}^T{\mathrm{W}}^T)]\\ & =E[\mathrm{A}\overrightarrow{e_{k-1估}}{\overrightarrow{e_{k-1估}}}^T{\mathrm{A}}^T+\mathrm{W}\overrightarrow{w_{k-1}}{\overrightarrow{w_{k-1}}}^T{\mathrm{W}}^T]\\ & =\mathrm{A}E[\overrightarrow{e_{k-1估}}{\overrightarrow{e_{k-1估}}}^T]{\mathrm{A}}^T+\mathrm{W}E[\overrightarrow{w_{k-1}}{\overrightarrow{w_{k-1}}}^T]{\mathrm{W}}^T\\ & =\mathrm{A}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}{\mathrm{A}}^T+\mathrm{WQ}{\mathrm{W}}^T\end{array}$$

${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$表达式在上面推导过了，只不过$k$换成$k-1$：
$$表达式：{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}=(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}){\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}(\mathrm{I}-{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T)+{\mathrm{K}}_{\mathrm{k}-1}\mathrm{VR}{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T，{\mathrm{K}}_{\mathrm{k}-1}=\frac{{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T}$$
把${\mathrm{K}}_{\mathrm{k}-1}$代到${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$表达式里就得到${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$的具体值，你完全可以将${\mathrm{K}}_{\mathrm{k}-1}$直接无脑代入${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$的表达式，毕竟${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$等式右边全是上轮迭代得到的已知数，但是${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$可以进行化简减少计算量：
$$\begin{array}{rl}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}& =({\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T+{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T)+{\mathrm{K}}_{\mathrm{k}-1}\mathrm{VR}{\mathrm{V}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T\\ & =({\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T)+{\mathrm{K}}_{\mathrm{k}-1}(\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T){{\mathrm{K}}_{\mathrm{k}-1}}^T\\ & =({\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T)+{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}{\mathrm{H}}^T{{\mathrm{K}}_{\mathrm{k}-1}}^T\\ & ={\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}-{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}{\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}\\ & =(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}-1}\mathrm{H}){\mathrm{P}}_{\mathrm{k}-1\mathrm{模}}\end{array}$$
又是和标准卡尔曼滤波里的${\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}$长得挺像的。
这样扩展卡尔曼滤波的公式全推导完了。

公式总结

1. $\overrightarrow{z_{k模}}=\mathrm{f}(\overrightarrow{z_{k-1模}},F_{k-1})$
2. ${\mathrm{P}}_{\mathrm{k}\mathrm{模}}=\mathrm{A}{\mathrm{P}}_{\mathrm{k}-1\mathrm{估}}{\mathrm{A}}^T+\mathrm{WQ}{\mathrm{W}}^T$
3. ${\mathrm{K}}_{\mathrm{k}}=\frac{{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T}{\mathrm{H}{\mathrm{P}}_{\mathrm{k}\mathrm{模}}{\mathrm{H}}^T+\mathrm{VR}{\mathrm{V}}^T}$
4. $\overrightarrow{z_{k估}}=\overrightarrow{z_{k模}}+{\mathrm{K}}_{\mathrm{k}}\ast (\overrightarrow{y_{k测}}-\mathrm{h}(\overrightarrow{z_{k模}}))$
5. ${\mathrm{P}}_{\mathrm{k}\mathrm{估}}=(\mathrm{I}-{\mathrm{K}}_{\mathrm{k}}\mathrm{H}){\mathrm{P}}_{\mathrm{k}\mathrm{模}}$

大致迭代过程和标准卡尔曼滤波一样，这五个公式按照顺序遍历计算。要注意的是标准卡尔曼滤波的$\mathrm{A},\mathrm{V},\mathrm{W}$都是固定的矩阵，而扩展卡尔曼滤波中的$\mathrm{A},\mathrm{V},\mathrm{W}$是雅可比矩阵，每次迭代过程中都需要更新计算一次。