滤波算法_扩展卡尔曼滤波(EKF, Extended Kalman filter)_全网最详细的数学推导_Part2

江湖上都叫我秋博

已于 2022-09-09 16:10:26 修改

阅读量1.5k

点赞数 8

分类专栏：滤波算法

于 2022-09-09 15:57:09 首次发布

本文链接：https://blog.csdn.net/heqiunong/article/details/126784006

版权

EKF 扩展卡尔曼滤波全文最详细数学推导

滤波算法专栏收录该内容

6 篇文章

订阅专栏

本文是滤波算法_扩展卡尔曼滤波(EKF, Extended Kalman filter)_全网最详细的数学推导_Part1的延续。

3、卡尔曼增益推导

回顾标准卡尔曼滤波中模型估计和测量估计进行数据融合的方程（后验估计方程）： ${\mathbf{\hat x}}_k^{} = {\mathbf{\hat x}}_k^ - + {K_k}\left( {{{\mathbf{z}}_k} - {\mathbf{H\hat x}}_k^ - } \right)$
类似地：扩展卡尔曼滤波的后验估计方程定义如下：
${\mathbf{\hat x}}_k^{} \triangleq {\mathbf{\hat x}}_k^ - + {K_k}\left( {{{\mathbf{z}}_k} - h\left( {{\mathbf{\hat x}}_k^ - ,{\mathbf{0}}} \right)} \right) = {\mathbf{\hat x}}_k^ - + {K_k}\left( {{{\mathbf{z}}_k} - {\mathbf{\hat z}}_k^ - } \right)$
在上述的定义中， $h\left( {{\mathbf{\hat x}}_k^ - ,{\mathbf{0}}} \right)$ 与标准卡尔曼滤波后验估计方程中的 ${\mathbf{H\hat x}}_k^ -$ 代表相同的含义。

与标准卡尔曼滤波类似，现在的问题也是 ${K_k}$ 应该如何设计？

与标准卡尔曼滤波的推导过程类似，首先定义估计误差向量为： ${{\mathbf{e}}_k} = {{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^{}$ ， $p\left( {{{\mathbf{e}}_k}} \right) \sim \left( {{\mathbf{0}},{{\mathbf{P}}_k}} \right)$ ，要想 ${\mathbf{\hat x}}_k^{} \to {{\mathbf{x}}_k}$ ，就需要令误差 ${{\mathbf{e}}_k} = {{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^{}$ 的方差最小，方差越小，就越接近于期望值，而我们当然期望估计误差向量为0了，即： $E\left( {{{\mathbf{e}}_k}} \right) = 0$ 。同样地，与标准卡尔曼滤波的推导类似，估计误差向量的方差最小，即是估计误差协方差矩阵 ${{{\mathbf{P}}_k}}$ 的迹 $tr\left( {{{\mathbf{P}}_k}} \right) = \sigma _{{e_1}}^2 + \sigma _{{e_2}}^2 + \cdots \sigma _{{e_n}}^2$ 最小。
${{\mathbf{P}}_k} = E\left( {{{\mathbf{e}}_k}{\mathbf{e}}_k^T} \right) = E\left[ {\left( {{{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^{}} \right){{\left( {{{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^{}} \right)}^T}} \right]$

在标准卡尔曼滤波的那篇博客我已经推导过上式的结论了。

明确目标后，下面正式开始扩展卡尔曼滤波的Kalman Gain ${K_k}$ 求解。
首先化简 ${{\mathbf{x}}_k} - {\mathbf{\hat x}}_k^{}$ ，将 ${{\mathbf{x}}_k}{\mathbf{ = \hat x}}_k^ - + {{\mathbf{A}}_k}\left( {{{\mathbf{x}}_{k - 1}} - {{{\mathbf{\hat x}}}_{k - 1}}} \right) + {{\mathbf{W}}_k}{{\mathbf{w}}_{k - 1}}$ 与 ${\mathbf{\hat x}}_k^{} = {\mathbf{\hat x}}_k^ - + {K_k}\left( {{{\mathbf{z}}_k} - {\mathbf{\hat z}}_k^ - } \right)$ 代入：
$\begin{aligned} {{\bf{x}}_k} - {\bf{\hat x}}_k^{} &= \left[ {{\bf{\hat x}}_k^ - + {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}}} \right] - \left[ {{\bf{\hat x}}_k^ - + {K_k}\left( {{{\bf{z}}_k} - {\bf{\hat z}}_k^ - } \right)} \right]\\\\ &= {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}\left( {{{\bf{z}}_k} - {\bf{\hat z}}_k^ - } \right) \end{aligned}$

进一步将 ${{\bf{z}}_k} = {\bf{\hat z}}_k^ - + {{\bf{H}}_k} \cdot \left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right) + {{\bf{V}}_k}{{\bf{v}}_k}$ 代入：

不必惊讶，下面进一步再将 ${{\mathbf{x}}_k}{\mathbf{ = \hat x}}_k^ - + {{\mathbf{A}}_k}\left( {{{\mathbf{x}}_{k - 1}} - {{{\mathbf{\hat x}}}_{k - 1}}} \right) + {{\mathbf{W}}_k}{{\mathbf{w}}_{k - 1}}$ 代入：

$\begin{aligned} {{\bf{x}}_k} - {\bf{\hat x}}_k^{} &= {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}\left\{ {{{\bf{H}}_k}\left[ {{\bf{\hat x}}_k^ - + {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {\bf{\hat x}}_k^ - } \right] + {{\bf{V}}_k}{{\bf{v}}_k}} \right\}\\\\ &= {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - \left( {{K_k}{{\bf{H}}_k}{{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {K_k}{{\bf{H}}_k}{{\bf{W}}_k}{{\bf{w}}_{k - 1}} + {K_k}{{\bf{V}}_k}{{\bf{v}}_k}} \right)\\\\ & = {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) - {K_k}{{\bf{H}}_k}{{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{V}}_k}{{\bf{v}}_k} \end{aligned}$

由于前面定义了 ${{\bf{e}}_k} = {{\bf{x}}_k} - {\bf{\hat x}}_k^{}$ ，所以 ${{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}}$ 可以用 ${e_{k - 1}}$ 来表示， ${{\bf{x}}_k} - {\bf{\hat x}}_k^{}$ 进一步化简为：
${{\bf{x}}_k} - {\bf{\hat x}}_k^{} = {{\bf{A}}_k}{e_{k - 1}} + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{A}}_k}{e_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{V}}_k}{{\bf{v}}_k}$

${\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^{}} \right)^T} = e_{k - 1}^T{\bf{A}}_k^T + {\bf{w}}_{k - 1}^T{\bf{W}}_k^T - e_{k - 1}^T{\bf{A}}_k^T{\bf{H}}_k^TK_k^T - {\bf{w}}_{k - 1}^T{\bf{W}}_k^T{\bf{H}}_k^TK_k^T - {\bf{v}}_k^T{\bf{V}}_k^TK_k^T$

$\begin{aligned} {{\bf{P}}_k} &= E\left( {{{\bf{e}}_k}{\bf{e}}_k^T} \right) = E\left[ {\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^{}} \right){{\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^{}} \right)}^T}} \right]\\\\ & = E\left[ \begin{array}{l} \left( {{{\bf{A}}_k}{e_{k - 1}} + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{A}}_k}{e_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{V}}_k}{{\bf{v}}_k}} \right)\\\\ \times \left( {e_{k - 1}^T{\bf{A}}_k^T + {\bf{w}}_{k - 1}^T{\bf{W}}_k^T - e_{k - 1}^T{\bf{A}}_k^T{\bf{H}}_k^TK_k^T - {\bf{w}}_{k - 1}^T{\bf{W}}_k^T{\bf{H}}_k^TK_k^T - {\bf{v}}_k^T{\bf{V}}_k^TK_k^T} \right) \end{array} \right] \end{aligned}$

上式公式如果真的展开的话，公式会特别长，因此下面拿一项来说明化简的办法，在上式第一个括号的第一项与第二个括号的第二项相乘时，会产生一项是: $E\left( {{{\bf{A}}_k}{e_{k - 1}}{\bf{w}}_{k - 1}^T{\bf{W}}_k^T} \right) = {{\bf{A}}_k}E\left( {{e_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T$ 因为 ${e_{k - 1}},{\bf{w}}_{k - 1}^{}$ 互相独立的随机变量，因此： ${{\bf{A}}_k}E\left( {{e_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T = {{\bf{A}}_k}E\left( {{e_{k - 1}}} \right)E\left( {{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T = {{\bf{A}}_k} \cdot {\bf{0}} \cdot {\bf{0}} \cdot {\bf{W}}_k^T = 0$

因此在化简上式的时候，就可以把存在两个互相独立的随机变量的项全部约掉。 ${e_{k - 1}},{\bf{w}}_{k - 1}^{},{{\bf{v}}_k}$ 它们三个都互相独立的。

${\bf{P}}_k$ 的表达式可进一步化为：

$\begin{aligned} {{\bf{P}}_k} &= E\left[ \begin{array}{l} \left( {{{\bf{A}}_k}{e_{k - 1}} + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{A}}_k}{e_{k - 1}} - {K_k}{{\bf{H}}_k}{{\bf{W}}_k}{{\bf{w}}_{k - 1}} - {K_k}{{\bf{V}}_k}{{\bf{v}}_k}} \right)\\\\ \times \left( {e_{k - 1}^T{\bf{A}}_k^T + {\bf{w}}_{k - 1}^T{\bf{W}}_k^T - e_{k - 1}^T{\bf{A}}_k^T{\bf{H}}_k^TK_k^T - {\bf{w}}_{k - 1}^T{\bf{W}}_k^T{\bf{H}}_k^TK_k^T - {\bf{v}}_k^T{\bf{V}}_k^TK_k^T} \right) \end{array} \right]\\\\ & = {{\bf{A}}_k}E\left( {{e_{k - 1}}e_{k - 1}^T} \right){\bf{A}}_k^T - {{\bf{A}}_k}E\left( {{e_{k - 1}}e_{k - 1}^T} \right){\bf{A}}_k^T{\bf{H}}_k^TK_k^T + {{\bf{W}}_k}E\left( {{{\bf{w}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T\\\\ &-{{\bf{W}}_k}E\left( {{{\bf{w}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T{\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{{\bf{A}}_k}E\left( {{e_{k - 1}}e_{k - 1}^T} \right){\bf{A}}_k^T + {K_k}{{\bf{H}}_k}{{\bf{A}}_k}E\left( {{e_{k - 1}}e_{k - 1}^T} \right){\bf{A}}_k^T{\bf{H}}_k^TK_k^T\\\\ &-{K_k}{{\bf{H}}_k}{{\bf{W}}_k}E\left( {{{\bf{w}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T + {K_k}{{\bf{H}}_k}{{\bf{W}}_k}E\left( {{{\bf{w}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T{\bf{H}}_k^TK_k^T + {K_k}{{\bf{V}}_k}E\left( {{{\bf{v}}_k}{\bf{v}}_k^T} \right){\bf{V}}_k^TK_k^T \end{aligned}$

这里引用标准卡尔曼滤波里面的结论。
$E\left( {{e_{k - 1}}e_{k - 1}^T} \right) = {P_{k - 1}},E\left( {{{\bf{w}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right) = Q,E\left( {{{\bf{v}}_k}{\bf{v}}_k^T} \right) = R$
将其代入可得：
$\begin{aligned} {{\bf{P}}_k} &= {{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T - {{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T{\bf{H}}_k^TK_k^T + {{\bf{W}}_k}Q{\bf{W}}_k^T - {{\bf{W}}_k}Q{\bf{W}}_k^T{\bf{H}}_k^TK_k^T\\\\ &-{K_k}{{\bf{H}}_k}{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T + {K_k}{{\bf{H}}_k}{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T{\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{{\bf{W}}_k}Q{\bf{W}}_k^T\\\\ &+{K_k}{{\bf{H}}_k}{{\bf{W}}_k}Q{\bf{W}}_k^T{\bf{H}}_k^TK_k^T + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T \end{aligned}$

进一步提取公共项，把 ${{\bf{P}}_k}$ 的表达式化简一番
$\begin{aligned} {{\bf{P}}_k} &= {{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T - {{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T{\bf{H}}_k^TK_k^T + {{\bf{W}}_k}Q{\bf{W}}_k^T - {{\bf{W}}_k}Q{\bf{W}}_k^T{\bf{H}}_k^TK_k^T\\\\ &-{K_k}{{\bf{H}}_k}{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T + {K_k}{{\bf{H}}_k}{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T{\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{{\bf{W}}_k}Q{\bf{W}}_k^T\\\\ &+{K_k}{{\bf{H}}_k}{{\bf{W}}_k}Q{\bf{W}}_k^T{\bf{H}}_k^TK_k^T + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T\\\\ &={{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {{\bf{W}}_k}Q{\bf{W}}_k^T\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) - {K_k}{{\bf{H}}_k}{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right)\\\\ &-{K_k}{{\bf{H}}_k}{{\bf{W}}_k}Q{\bf{W}}_k^T\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T\\\\ &=\left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right){{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right)\\\\ &+\left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right){{\bf{W}}_k}Q{\bf{W}}_k^T\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T\\\\ &= \left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right)\left( {{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T + {{\bf{W}}_k}Q{\bf{W}}_k^T} \right)\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T \end{aligned}$

大道至简，越简洁的式子越美。 ${{\bf{P}}_k}$ 的化简式暂时先放一放。

参考估计误差向量的定义: ${{\bf{e}}_k} = {{\bf{x}}_k} - {\bf{\hat x}}_k^{}$ 定义先验估计误差向量为：
${\bf{e}}_k^ - = {{\bf{x}}_k} - {\bf{\hat x}}_k^ -$
那么，
${\bf{P}}_k^ - = E\left( {{\bf{e}}_k^ - {\bf{e}}_k^{ - T}} \right) = E\left[ {\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right){{\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right)}^T}} \right]$
同样地，先计算 ${{\bf{x}}_k} - {\bf{\hat x}}_k^ -$ 。
$\begin{aligned} {{\bf{x}}_k} - {\bf{\hat x}}_k^ - &= \left[ {{\bf{\hat x}}_k^ - + {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}}} \right] - {\bf{\hat x}}_k^ - \\\\ &= {{\bf{A}}_k}\left( {{{\bf{x}}_{k - 1}} - {{{\bf{\hat x}}}_{k - 1}}} \right) + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} = {{\bf{A}}_k}{{\bf{e}}_{k - 1}} + {{\bf{W}}_k}{{\bf{w}}_{k - 1}} \end{aligned}$
${\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right)^T}$ 也先求出来方便后续计算。
${\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right)^T} = {\bf{e}}_{k - 1}^T{\bf{A}}_k^T + {\bf{w}}_{k - 1}^T{\bf{W}}_k^T$

将上面两个式子代入到 ${\bf{P}}_k^ -$ 的表达式中：
$\begin{aligned} {\bf{P}}_k^ - &= E\left[ {\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right){{\left( {{{\bf{x}}_k} - {\bf{\hat x}}_k^ - } \right)}^T}} \right] = E\left[ {\left( {{{\bf{A}}_k}{{\bf{e}}_{k - 1}} + {{\bf{W}}_k}{{\bf{w}}_{k - 1}}} \right)\left( {{\bf{e}}_{k - 1}^T{\bf{A}}_k^T + {\bf{w}}_{k - 1}^T{\bf{W}}_k^T} \right)} \right]\\\\ &= {{\bf{A}}_k}E\left( {{{\bf{e}}_{k - 1}}{\bf{e}}_{k - 1}^T} \right){\bf{A}}_k^T + {{\bf{A}}_k}E\left( {{{\bf{e}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T + {{\bf{W}}_k}E\left( {{{\bf{w}}_{k - 1}}{\bf{e}}_{k - 1}^T} \right){\bf{A}}_k^T + {{\bf{W}}_k}E\left( {{{\bf{w}}_{k - 1}}{\bf{w}}_{k - 1}^T} \right){\bf{W}}_k^T\\\\ &= {{\bf{A}}_k}{\bf{P}}_{k - 1}^{}{\bf{A}}_k^T + 0 + 0 + {{\bf{W}}_k}{\bf{QW}}_k^T\\\\ &= {{\bf{A}}_k}{\bf{P}}_{k - 1}^{}{\bf{A}}_k^T + {{\bf{W}}_k}{\bf{QW}}_k^T \end{aligned}$

将 ${\bf{P}}_k^ - = {{\bf{A}}_k}{\bf{P}}_{k - 1}^{}{\bf{A}}_k^T + {{\bf{W}}_k}{\bf{QW}}_k^T$ ，带入到 ${{\bf{P}}_k}$ 的公式当中，可得

$\begin{aligned} {{\bf{P}}_k} &= \left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right)\left( {{{\bf{A}}_k}{P_{k - 1}}{\bf{A}}_k^T + {{\bf{W}}_k}Q{\bf{W}}_k^T} \right)\left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T\\\\ & = \left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right){\bf{P}}_k^ - \left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T \end{aligned}$

把 ${{\bf{P}}_k}$ 的公式再进行展开：
$\begin{aligned} {{\bf{P}}_k} &= \left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right){\bf{P}}_k^ - \left( {{\bf{I}} - {\bf{H}}_k^TK_k^T} \right) + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T\\\\ &= {\bf{P}}_k^ - - {\bf{P}}_k^ - {\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - + {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^TK_k^T + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T \end{aligned}$

Don’t Forget，我们的目的是寻找一个合适的 ${{\bf{K}}_k}$ 使得 $tr\left( {{\bf{P}}_k^{}} \right)$ 最小。等式两边分别取迹可得：
$tr\left( {{\bf{P}}_k^{}} \right) = tr\left( {{\bf{P}}_k^ - } \right) - tr\left( {{\bf{P}}_k^ - {\bf{H}}_k^T{\bf{K}}_k^T} \right) - tr\left( {{{\bf{K}}_k}{{\bf{H}}_k}{\bf{P}}_k^ - } \right) + tr\left( {{{\bf{K}}_k}{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T{\bf{K}}_k^T} \right) + tr\left( {{K_k}{{\bf{V}}_k}{\bf{RV}}_k^TK_k^T} \right)$

注意上式的第二项和第三项，取第二项 ${\bf{P}}_k^-{{\bf{H}}^T}{\bf{K}}_k^T$ 的转置 ${\left( {{\bf{P}}_k^- {{\bf{H}}^T}{\bf{K}}_k^T} \right)^T} = {{\bf{K}}_k}{\left( {{\bf{P}}_k^ - {{\bf{H}}^T}} \right)^T} = {{\bf{K}}_k}{\bf{HP}}{_k^{-T}}= {{\bf{K}}_k}{\bf{HP}}_k^-$

第二项的转置正好是第三项。

数学基础知识补充：
如果 ${\bf{A}} = {{\bf{B}}^T},tr\left( {\bf{A}} \right) = tr\left( {\bf{B}} \right)$ ，因为转置之后，对角线的不变的。

因此：
$tr\left( {{\bf{P}}_k^{}} \right) = tr\left( {{\bf{P}}_k^ - } \right) - 2tr\left( {{{\bf{K}}_k}{{\bf{H}}_k}{\bf{P}}_k^ - } \right) + tr\left( {{{\bf{K}}_k}{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T{\bf{K}}_k^T} \right) + tr\left( {{K_k}{{\bf{V}}_k}{\bf{RV}}_k^TK_k^T} \right)$

要找到 ${{\bf{K}}_k}$ 使得 $tr\left( {{\bf{P}}_k^{}} \right)$ 最小，即对 $tr\left( {{\bf{P}}_k^{}} \right)$ 求 ${{\bf{K}}_k}$ 的导数，并令导数为0。即：
$\frac{{d\left[ {tr\left( {{\bf{P}}_k^{}} \right)} \right]}}{{d\left( {{\bf{K}}_k^{}} \right)}} = 0$

数学基础知识补充：
${{d\left[ {tr\left( {{\bf{AB}}} \right)} \right]} \over {d\left( {\bf{A}} \right)}} = {{\bf{B}}^T},{{d\left[ {tr\left( {{\bf{AB}}{{\bf{A}}^T}} \right)} \right]} \over {d\left( {\bf{A}} \right)}} = 2{\bf{AB}}$

因此：
$\begin{aligned} \frac{{d\left[ {tr\left( {{\bf{P}}_k^{}} \right)} \right]}}{{d\left( {{\bf{K}}_k^{}} \right)}} &= {\bf{0}} - 2{\left( {{{\bf{H}}_k}{\bf{P}}_k^ - } \right)^T} + 2{{\bf{K}}_k}{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + 2{{\bf{K}}_k}{\bf{R}}\\\\ &= - 2{\bf{P}}_k^ - {\bf{H}}_k^T + 2{{\bf{K}}_k}{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + 2{{\bf{K}}_k}{{\bf{V}}_k}{\bf{RV}}_k^T\\\\ &= 2\left[ { - {\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{K}}_k}\left( {{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{V}}_k}{\bf{RV}}_k^T} \right)} \right] = 0 \end{aligned}$

最终，终于得到扩展卡尔曼滤波器的增益。
${{\bf{K}}_k} = {\bf{P}}_k^ - {\bf{H}}_k^T{\left( {{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{V}}_k}{\bf{RV}}_k^T} \right)^{ - 1}} = \frac{{{\bf{P}}_k^ - {\bf{H}}_k^T}}{{{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{V}}_k}{\bf{RV}}_k^T}}$

4、估计误差协方差矩阵化简

下面对估计误差协方差矩阵 ${{\bf{P}}_k}$ 进行进一步化简，将 ${{\bf{K}}_k}$ 的表达式代入：
$\begin{aligned} {{\bf{P}}_k} &= {\bf{P}}_k^ - - {\bf{P}}_k^ - {\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - + {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^TK_k^T + {K_k}{{\bf{V}}_k}R{\bf{V}}_k^TK_k^T\\\\ &= {\bf{P}}_k^ - - {\bf{P}}_k^ - {\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - + {K_k}\left( {{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{V}}_k}R{\bf{V}}_k^T} \right)K_k^T\\\\ &= {\bf{P}}_k^ - - {\bf{P}}_k^ - {\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - + \frac{{{\bf{P}}_k^ - {\bf{H}}_k^T}}{{{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{V}}_k}{\bf{RV}}_k^T}}\left( {{{\bf{H}}_k}{\bf{P}}_k^ - {\bf{H}}_k^T + {{\bf{V}}_k}R{\bf{V}}_k^T} \right)K_k^T\\\\ &= {\bf{P}}_k^ - - {\bf{P}}_k^ - {\bf{H}}_k^TK_k^T - {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - + {\bf{P}}_k^ - {\bf{H}}_k^TK_k^T\\\\ &= {\bf{P}}_k^ - - {K_k}{{\bf{H}}_k}{\bf{P}}_k^ - \\\\ &= \left( {{\bf{I}} - {K_k}{{\bf{H}}_k}} \right){\bf{P}}_k^ - \end{aligned}$

5、全文总结

下面总结扩展卡尔曼滤波的5大公式：

先验估计公式：（ ${{\bf{u}}_k}$ 是已知量， ${{\bf{\hat x}}_{k - 1}}$ 是上一时刻的后验估计）
${\bf{\hat x}}_k^ - = f\left( {{{{\bf{\hat x}}}_{k - 1}},{\bf{0}},{{\bf{u}}_k}} \right)$