概率机器人3.1 卡尔曼滤波扩展卡尔曼滤波和无迹卡尔曼滤波
Posted 子龙丨风
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了概率机器人3.1 卡尔曼滤波扩展卡尔曼滤波和无迹卡尔曼滤波相关的知识，希望对你有一定的参考价值。
这一章将介绍卡尔曼滤波、扩展卡尔曼滤波以及无迹卡尔曼滤波，并从贝叶斯滤波的角度来进行分析并完成数学推导。如果您对贝叶斯滤波不了解，可以查阅相关书籍或阅读 【概率机器人 2 递归状态估计】。
这三种滤波方式都假设状态变量 $\\mathbf{x}_t$ 的置信度 $\\mathrm{bel}(\\mathbf{x}_t)$ 为正态分布，这样在计算 $\\mathbf{x}_t$ 的置信度时，只需要计算出其分布的均值与方差即可。下面将分别介绍这三种滤波算法。
1.卡尔曼滤波
首先，回顾一下贝叶斯滤波。其目标是求解状态变量 $\\mathbf{x}_t$ 的置信度 $ \\mathrm{bel}(\\mathbf{x}_t)$ ，求解思想是以递归的方式（根据 $ \\mathrm{bel}(\\mathbf{x}_{t-1})$ 计算 $ \\mathrm{bel}(\\mathbf{x}_t)$ ），具体实现分为两个步骤：
(a) 预测：利用 $ \\mathrm{bel}(\\mathbf{x}_{t-1})$ 计算 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t)$ ，即
$$ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) = \\int p(\\mathbf{x}_t | \\mathbf{x}_{t-1}, \\mathbf{u}_t) \\mathrm{bel}(\\mathbf{x}_{t-1}) \\mathrm{d}\\mathbf{x}_{t-1} \\tag{3.1} $$
(b) 测量更新：利用测量值 $\\mathbf{z}_t$ 和 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t)$ 计算 $ \\mathrm{bel}(\\mathbf{x}_{t})$，即
$$ \\mathrm{bel}(\\mathbf{x}_t) = \\eta p(\\mathbf{z}_t | \\mathbf{x}_t) \\overline{\\mathrm{bel}}(\\mathbf{x}_t) \\tag{3.2} $$
卡尔曼滤波是通过合理的假设以方便实现以上的计算，具体而言就是假设 $p(\\mathbf{x}_t | \\mathbf{x}_{t-1}, \\mathbf{u}_t) \\mathrm{bel}(\\mathbf{x}_{t-1})$ 和 $p(\\mathbf{z}_t | \\mathbf{x}_t)$ 为高斯分布，这样就可以用均值 $\\boldsymbol{\\mu}_t$ 和方差 $\\mathbf{\\Sigma}_t$ 来表示置信度 $ \\mathrm{bel}(\\mathbf{x}_t)$。具体假设如下：
(1) 状态转移概率 $p(\\mathbf{x}_t | \\mathbf{x}_{t-1}, \\mathbf{u}_t) $ 是带有随机高斯噪声的线性函数，可由下式表示：
$$ \\mathbf{x}_t = \\mathbf{A}_t \\mathbf{x}_{t-1} + \\mathbf{B}_t\\mathbf{u}_t + \\boldsymbol{\\varepsilon}_t \\tag{3.3} $$
其中，$\\boldsymbol{\\varepsilon}_t$ 表示高斯随机噪声，均值为0，方差为 $\\mathbf{R}_t$。
这样，状态转移概率 $p(\\mathbf{x}_t | \\mathbf{x}_{t-1}, \\mathbf{u}_t) $ 可由式(3.3)得出：
$$p(\\mathbf{x}_t | \\mathbf{x}_{t-1}, \\mathbf{u}_t) = \\mathrm{det} (2\\pi \\mathbf{R}_t)^{-\\frac{1}{2}} \\exp \\left \\{ -\\frac{1}{2} ( \\mathbf{x}_t - \\mathbf{A}_t\\mathbf{x}_{t-1} - \\mathbf{B}_t\\mathbf{u}_t )^T \\mathbf{R}_t^{-1} ( \\mathbf{x}_t - \\mathbf{A}_t\\mathbf{x}_{t-1} - \\mathbf{B}_t\\mathbf{u}_t ) \\right \\} \\tag{3.4} $$
(2) 测量概率 $p(\\mathbf{z}_t | \\mathbf{x}_t )$ 也是与带有高斯噪声的自变量呈线性关系：
$$ \\mathbf{z}_t = \\mathbf{C}_t \\mathbf{x}_t + \\boldsymbol{\\delta}_t \\tag{3.5} $$
其中，$\\boldsymbol{\\delta}_t$ 为测量噪声，是均值为0，方差为 $\\mathbf{Q}_t$ 的高斯随机分布。
因此，测量概率可由式(3.5)得出：
$$ p(\\mathbf{z}_t | \\mathbf{x}_t ) = \\mathrm{det} (2\\pi \\mathbf{Q}_t)^{-\\frac{1}{2}} \\exp \\left \\{ -\\frac{1}{2} ( \\mathbf{x}_t - \\mathbf{C}_t\\mathbf{x}_t )^T \\mathbf{Q}_t^{-1}( \\mathbf{x}_t - \\mathbf{C}_t\\mathbf{x}_t ) \\right \\} \\tag{3.6} $$
(3) 初始置信度 $ \\mathrm{bel}(\\mathbf{x}_0)$ 是正态分布，其均值为 $\\boldsymbol{\\mu}_0$ ，方差为 $\\mathbf{\\Sigma}_0$：
$$ \\mathrm{bel}(\\mathbf{x}_t) = p(\\mathbf{x}_0) = \\mathrm{det}(2\\pi \\mathbf{\\Sigma}_0)^{-\\frac{1}{2}} \\exp \\left\\{ -\\frac{1}{2} (\\mathbf{x}_0 - \\boldsymbol{\\mu}_0)^T \\mathbf{\\Sigma}_0^{-1} (\\mathbf{x}_0 - \\boldsymbol{\\mu}_0) \\right\\} \\tag{3.7} $$
以上三个假设保证了 $ \\mathrm{bel}(\\mathbf{x}_t)$ 在任何时刻 $t$ 总符合高斯分布，这样我们求解其均值与方差即可。
1.1 卡尔曼滤波算法
程序3.1描述了KF算法（Kalman filter algorithm），时刻 $t$ 的置信度 $ \\mathrm{bel}(\\mathbf{x}_t)$ 用 均值 $\\boldsymbol{\\mu}_t$ 、方差 $\\mathbf{\\Sigma}_t$ 表示。
| 程序3.1 卡尔曼滤波算法 | |
| 1: | Algorithm Kalman_filter( $\\boldsymbol{\\mu}_{t-1}$, $\\mathbf{\\Sigma}_{t-1}$, $\\mathbf{u}_t$, $\\mathbf{z}_t$ ): |
| 2: | $ \\overline{\\boldsymbol{\\mu}}_t = \\mathbf{A}_t \\boldsymbol{\\mu}_{t-1} + \\mathbf{B}_t\\mathbf{u}_t $ |
| 3: | $ \\overline{\\mathbf{\\Sigma}}_t = \\mathbf{A}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{A}_t^T + \\mathbf{R}_t $ |
| 4: | $ \\mathbf{K}_t = \\overline{\\mathbf{\\Sigma}}_t \\mathbf{C}_t^T (\\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_t \\mathbf{C}_t^T + \\mathbf{Q}_t)^{-1} $ |
| 5: | $ \\boldsymbol{\\mu}_t = \\overline{\\boldsymbol{\\mu}}_t + \\mathbf{K}_t (\\mathbf{z}_t - \\mathbf{C}_t \\overline{\\boldsymbol{\\mu}}_t ) $ |
| 6: | $ \\mathbf{\\Sigma}_t = (\\mathbf{I} - \\mathbf{K}_t \\mathbf{C}_t) \\overline{\\mathbf{\\Sigma}}_t $ |
| 7: | return $\\boldsymbol{\\mu}_t$, $\\mathbf{\\Sigma}_t$ |
程序3.1中的第2、3行为预测阶段，第4、5、6行为测量更新阶段。其中第4行计算的变量矩阵 $\\mathbf{K}_t $ 叫做卡尔曼增益矩阵，它明确了测量综合到新的状态估计的程度。
1.2 卡尔曼滤波的数学推导
(1) 预测阶段：
程序3.1中的第2、3行为预测阶段，其目标是根据时刻 $t-1$ 的置信度 $ \\mathrm{bel}(\\mathbf{x}_{t-1})$ 来计算 时刻 $t$ 的 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t)$。
在这里， 置信度 $ \\mathrm{bel}(\\mathbf{x}_{t-1})$ 由均值 $\\boldsymbol{\\mu}_{t-1}$ 和方差 $\\mathbf{\\Sigma}_{t-1}$ 表达，而 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t)$ 由 均值 $\\overline{\\boldsymbol{\\mu}}_{t-1}$ 和方差 $\\overline{\\mathbf{\\Sigma}}_{t-1}$ 表达。
根据 式(3.1)(3.4)，可得
$$ \\begin{eqnarray*} \\overline{\\mathrm{bel}}(\\mathbf{x}_t) &=& \\int \\underbrace{p(\\mathbf{x}_t | \\mathbf{x}_{t-1}, \\mathbf{u}_t)}_{N(\\mathbf{x}_t;\\, \\mathbf{A}_t \\mathbf{x}_{t-1}\\, +\\mathbf{B}_t\\mathbf{u}_t, \\mathbf{R}_t)} \\underbrace{ \\mathrm{bel}(\\mathbf{x}_{t-1}) }_{\\; N(\\mathbf{x}_{t-1}\\,; \\boldsymbol{\\mu}_{t-1} \\, , \\mathbf{\\Sigma}_{t-1} ) } \\mathrm{d}\\mathbf{x}_{t-1} \\\\ &=& \\eta \\int \\exp \\left\\{ -\\frac{1}{2} ( \\mathbf{x}_t - \\mathbf{A}_t\\mathbf{x}_{t-1} - \\mathbf{B}_t\\mathbf{u}_t )^T \\mathbf{R}_t^{-1} ( \\mathbf{x}_t - \\mathbf{A}_t\\mathbf{x}_{t-1} - \\mathbf{B}_t\\mathbf{u}_t ) \\right\\} \\\\ & & \\exp \\left\\{ -\\frac{1}{2} (\\mathbf{x}_{t-1} - \\boldsymbol{\\mu}_{t-1})^T \\mathbf{\\Sigma}_{t-1}^{-1} (\\mathbf{x}_{t-1} - \\boldsymbol{\\mu}_{t-1}) \\right\\} \\mathrm{d}\\mathbf{x}_{t-1} \\end{eqnarray*} \\tag{3.8} $$
式(3.8)包含了一个对 $\\mathbf{x}_{t-1}$ 的积分，我们首先尝试来消除这个积分。定义
$$\\begin{eqnarray*} L_t =& \\frac{1}{2}( \\mathbf{x}_t - \\mathbf{A}_t \\mathbf{x}_{t-1} -\\mathbf{B}_t\\mathbf{u}_t )^T \\mathbf{R}_t^{-1} ( \\mathbf{x}_t - \\mathbf{A}_t \\mathbf{x}_{t-1} -\\mathbf{B}_t\\mathbf{u}_t ) + \\\\ & \\frac{1}{2} (\\mathbf{x}_{t-1} - \\boldsymbol{\\mu}_{t-1})^T \\mathbf{\\Sigma}_{t-1}^{-1} (\\mathbf{x}_{t-1} - \\boldsymbol{\\mu}_{t-1}) \\end{eqnarray*} \\tag{3.9}$$
这样，式(3.8)简化为
$$ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) = \\eta \\int \\exp \\{ -L_t \\} \\mathrm{d}\\mathbf{x}_{t-1} \\tag{3.10} $$
为了能够消除式(3.8)中的积分，我们从 $L_t$ 的形式入手。由于 $L_t$ 是$\\mathbf{x}_{t-1}$的二次型，也是$\\mathbf{x}_{t}$的二次型，因此我们首先从 $L_t$ 分解出 $\\mathbf{x}_{t-1}$ 的二次型项。即 $L_t$ 可分解为：
$$ L_t = L_t(\\mathbf{x}_t) + \\frac{1}{2}(\\mathbf{x}_{t-1} - \\Delta)^T \\mathbf{\\Psi} ^{-1}(\\mathbf{x}_{t-1} -\\Delta) \\tag{3.11} $$
这样，式(3.11)中的第一项 $L_t(\\mathbf{x}_t)$ 可以从积分中提取出来，而第二项可以利用“正态分布的积分为1”来消去。
现在我们来求出式(3.11)中的 $\\Delta$ 和 $\\mathbf{\\Psi}$，根据式(3.9)计算 $L_t$ 对 $\\mathbf{x}_{t-1}$ 的一二阶导数：
$$ \\frac{\\partial L_t}{\\partial \\mathbf{x}_{t-1}} = - \\mathbf{A}_t^T \\mathbf{R}_t^{-1} ( \\mathbf{x}_t - \\mathbf{A}_t\\mathbf{x}_{t-1} - \\mathbf{B}_t\\mathbf{u}_t) + \\mathbf{\\Sigma}_{t-1}^{-1} (\\mathbf{x}_{t-1}-\\boldsymbol{\\mu}_{t-1}) \\tag{3.12} $$
$$ \\frac{\\partial^2 L_t}{\\partial \\mathbf{x}_{t-1}^2} = \\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} \\tag{3.13} $$
令 $L_t$ 的一阶导为 0 ，得 $L_t$ 关于 $\\mathbf{x}_{t-1}$ 的极值点为
$$ \\mathbf{x}_{t-1} = (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} ) ^{-1}[ \\mathbf{A}_t^T \\mathbf{R}_t^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) +\\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} ] \\tag{3.14} $$
观察式(3.11)，可知 $\\Delta$ 即为 $L_t$ 对 $\\mathbf{x}_{t-1}$ 的极小值点， $\\mathbf{\\Psi}^{-1}$即为 $L_t$对$\\mathbf{x}_{t-1}$的二阶导。因此可取
$$ \\Delta = \\mathbf{\\Psi} [ \\mathbf{A}_t^T \\mathbf{R}_t^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) +\\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} ] \\tag{3.15} $$
$$ \\mathbf{\\Psi}^{-1} = \\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} \\tag{3.16} $$
因此，$L_t(\\mathbf{x}_t) $ 为
$$ \\begin{eqnarray*} L_t(\\mathbf{x}_t) &=& L_t - \\frac{1}{2}(\\mathbf{x}_{t-1} - \\Delta)^T \\mathbf{\\Psi} ^{-1}(\\mathbf{x}_{t-1} -\\Delta) \\\\ &=& +\\frac{1}{2} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t)^T \\mathbf{R}_t^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) + \\frac{1}{2} \\boldsymbol{\\mu}_{t-1}^T \\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} - \\frac{1}{2} [ \\mathbf{A}_t^T \\mathbf{R}_t^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) \\\\ & & +\\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} ]^T (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1})^{-1} [ \\mathbf{A}_t^T \\mathbf{R}_t^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) +\\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} ] \\end{eqnarray*} \\tag{3.17} $$
式(3.17)的详细推导过程就不给出了。此时，式(3.10)可以进一步化简为
$$ \\begin{eqnarray*} \\overline{\\mathrm{bel}}(\\mathbf{x}_t) &=& \\eta \\int \\exp \\left\\{ -L_t(\\mathbf{x}_t) - \\frac{1}{2}(\\mathbf{x}_{t-1} - \\Delta)^T \\mathbf{\\Psi} ^{-1}(\\mathbf{x}_{t-1} -\\Delta) \\right\\} \\mathrm{d}\\mathbf{x}_{t-1} \\\\ &=& \\eta \\exp \\{ -L_t(\\mathbf{x}_t) \\} \\int \\exp \\left\\{ - \\frac{1}{2}(\\mathbf{x}_{t-1} - \\Delta)^T \\mathbf{\\Psi} ^{-1}(\\mathbf{x}_{t-1} -\\Delta) \\right\\} \\mathrm{d}\\mathbf{x}_{t-1} \\end{eqnarray*} \\tag{3.18} $$
根据“正态分布的的积分为1”，可知 $\\int \\exp \\left\\{ - \\frac{1}{2}(\\mathbf{x}_{t-1} - \\Delta)^T \\mathbf{\\Psi} ^{-1}(\\mathbf{x}_{t-1} -\\Delta) \\right\\} \\mathrm{d}\\mathbf{x}_{t-1} = \\det(2\\pi \\mathbf{\\Psi})^{ \\frac{1}{2}}$，则
$$ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) = \\eta \\exp \\{ -L_t(\\mathbf{x}_t) \\} \\tag{3.19} $$
式(3.19)将积分值 $\\det(2\\pi \\mathbf{\\Psi})^{ \\frac{1}{2}}$ 归入到归一化因子 $\\eta$ 中。这里，由于 $L_t(\\mathbf{x}_t)$ 是 $mathbf{x}_t$ 的二次型，故 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) $ 服从正态分布，这也正好符合我们前面的假设。
根据式(3.19)，计算 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) $ 对 $\\mathbf{x}_t$ 的一二阶导数，即可求出其分布的均值 $\\overline{\\boldsymbol{\\mu}}_{t}$ 和方差 $\\overline{\\mathbf{\\Sigma}}_{t}$。即
$$ \\begin{eqnarray*} \\frac{\\partial L_t(\\mathbf{x}_t)}{\\partial \\mathbf{x}_{t}} &=& \\mathbf{R}_t^{-1}(\\mathbf{x}_t - \\mathbf{B}_t \\mathbf{u}_t) - \\mathbf{R}_t^{-1}\\mathbf{A}_t (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} )^{-1}[ \\mathbf{A}_t^T \\mathbf{R}_t^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) +\\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} ] \\\\ &=& \\left[ \\mathbf{R}_t^{-1} - \\mathbf{R}_t^{-1}\\mathbf{A}_t (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} )^{-1} \\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\right ] (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) - \\\\ & & \\mathbf{R}_t^{-1}\\mathbf{A}_t (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} )^{-1} \\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} \\end{eqnarray*} \\tag{3.20}$$
利用求逆引理，有
$$ \\mathbf{R}_t^{-1} - \\mathbf{R}_t^{-1}\\mathbf{A}_t (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} )^{-1} \\mathbf{A}_t^T \\mathbf{R}_t^{-1} = ( \\mathbf{R}_t + \\mathbf{A}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{A}_t^T )^{-1} \\tag{3.21} $$
将(3.21)代入到(3.20)，可得
$$ \\frac{\\partial L_t(\\mathbf{x}_t)}{\\partial \\mathbf{x}_{t}} = ( \\mathbf{R}_t + \\mathbf{A}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{A}_t^T )^{-1} (\\mathbf{x}_t - \\mathbf{B}_t\\mathbf{u}_t) - \\mathbf{R}_t^{-1}\\mathbf{A}_t (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} )^{-1} \\mathbf{\\Sigma}_{t-1}^{-1}\\boldsymbol{\\mu}_{t-1} \\tag{3.22} $$
当一阶导为零时得到 $L_t(\\mathbf{x}_t) $ 的极小值点
$$ \\begin{eqnarray*} \\mathbf{x}_t &=& \\mathbf{B}_t\\mathbf{u}_t + \\underbrace{ ( \\mathbf{R}_t + \\mathbf{A}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{A}_t^T )^{-1} \\mathbf{R}_t^{-1}\\mathbf{A}_t }_{ \\mathbf{A}_t + \\mathbf{A}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{A}_t^T \\mathbf{R}_t^{-1}\\mathbf{A}_t } \\underbrace{ (\\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{\\Sigma}_{t-1}^{-1} )^{-1} \\mathbf{\\Sigma}_{t-1}^{-1} }_{ (\\mathbf{\\Sigma}_{t-1}^{-1} \\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{I} )^{-1} } \\boldsymbol{\\mu}_{t-1} \\\\ &=& \\mathbf{B}_t\\mathbf{u}_t + \\mathbf{A}_t ( \\mathbf{I} + \\mathbf{\\Sigma}_{t-1}^{-1} \\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} ) (\\mathbf{\\Sigma}_{t-1}^{-1} \\mathbf{A}_t^T \\mathbf{R}_t^{-1} \\mathbf{A} + \\mathbf{I} )^{-1} \\boldsymbol{\\mu}_{t-1} \\\\ &=& \\mathbf{B}_t\\mathbf{u}_t + \\mathbf{A}_t \\boldsymbol{\\mu}_{t-1} \\end{eqnarray*} \\tag{3.23}$$
故 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) $ 的均值 $\\overline{\\boldsymbol{\\mu}}_{t}$ 为 $ \\mathbf{B}_t\\mathbf{u}_t + \\mathbf{A}_t \\boldsymbol{\\mu}_{t-1} $。这证明了程序3.1卡尔曼滤波算法中第2行的正确性。
根据式(3.22)，得到
$$ \\frac{\\partial ^2 L_t(\\mathbf{x}_t)}{\\partial \\mathbf{x}_{t}^2} = ( \\mathbf{R}_t + \\mathbf{A}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{A}_t^T )^{-1} \\tag{3.24} $$
其逆矩阵即为 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) $ 的协方差 $\\overline{\\mathbf{\\Sigma}}_{t}$ 。这证明了程序3.1中第3行的正确性。
(2) 测量更新阶段
程序3.1第4~6行为测量更新阶段，其目标是利用测量值 $\\mathbf{z}_t$ 和 $ \\overline{\\mathrm{bel}}(\\mathbf{x}_t)$ 计算 $ \\mathrm{bel}(\\mathbf{x}_{t})$， 即求解 $\\boldsymbol{\\mu}_t$、$\\mathbf{\\Sigma}_t$。
根据式(3.2)，得
$$ \\mathrm{bel}(\\mathbf{x}_{t}) = \\eta \\underbrace{ p(\\mathbf{z}_t | \\mathbf{x}_t)}_{N(\\mathbf{z}_t ; \\mathbf{C}_t \\mathbf{x}_t , \\mathbf{Q}_t )} \\underbrace{ \\overline{\\mathrm{bel}}(\\mathbf{x}_t) }_{ \\;\\;\\; N(\\mathbf{x}_t; \\overline{\\boldsymbol{\\mu}}_{t} , \\overline{\\mathbf{\\Sigma}}_{t} )} \\tag{3.25} $$
为方便分析，定义
$$ J_t = \\frac{1}{2} (\\mathbf{z}_t - \\mathbf{C}_t \\mathbf{x}_t )^T \\mathbf{Q}_t ^{-1} (\\mathbf{z}_t - \\mathbf{C}_t \\mathbf{x}_t ) + \\frac{1}{2} ( \\mathbf{x}_t - \\overline{\\boldsymbol{\\mu}}_{t} )^T \\overline{\\mathbf{\\Sigma}}\\,\\!_{t}^{-1} ( \\mathbf{x}_t - \\overline{\\boldsymbol{\\mu}}_{t} ) \\tag{3.26} $$
则式(3.27)简化为
$$ \\mathrm{bel}(\\mathbf{x}_{t}) = \\eta \\exp \\{ - J_t\\} \\tag{3.27}$$
要求解 $\\boldsymbol{\\mu}_t$、$\\mathbf{\\Sigma}_t$，需计算 $J_t$ 关于 $ \\mathbf{x}_{t}$ 的一二节导数：
$$ \\frac{\\partial J_t}{\\partial \\mathbf{x}_{t}} = - \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} (\\mathbf{z}_t - \\mathbf{C}_t \\mathbf{x}_t ) + \\overline{\\mathbf{\\Sigma}}\\,\\!_{t}^{-1} ( \\mathbf{x}_t - \\overline{\\boldsymbol{\\mu}}_{t} ) \\tag{3.28} $$
$$ \\frac{\\partial^2 J_t}{\\partial \\mathbf{x}_{t}^2} = \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1}\\mathbf{C}_t + \\overline{\\mathbf{\\Sigma}}\\,\\!_{t}^{-1} \\tag{3.29} $$
根据，式(3.29)，可得 $ \\mathrm{bel}(\\mathbf{x}_{t})$的协方差为
$$ \\mathbf{\\Sigma}_t = (\\mathbf{C}_t^T \\mathbf{Q}_t ^{-1}\\mathbf{C}_t + \\overline{\\mathbf{\\Sigma}}\\,_{t}^{-1} )^{-1} \\tag{3.30}$$
当 $J_t$ 的一阶导为零时得到极小值点为
$$ \\begin{eqnarray*} \\mathbf{x}_t &=& \\overline{\\boldsymbol{\\mu}}_{t} + \\underbrace{ ( \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\mathbf{C}_t + \\overline{\\mathbf{\\Sigma}}\\,\\!_{t}^{-1} )^{-1} }_{ \\mathbf{\\Sigma}_t } \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} ( \\mathbf{z}_t - \\mathbf{C}_t \\overline{\\boldsymbol{\\mu}}_{t} ) \\\\ &=& \\overline{\\boldsymbol{\\mu}}_{t} + \\mathbf{\\Sigma}_t \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} ( \\mathbf{z}_t - \\mathbf{C}_t \\overline{\\boldsymbol{\\mu}}_{t} ) \\end{eqnarray*} \\tag{3.31}$$
上式的值即为 $\\boldsymbol{\\mu}_t$，现在定义卡尔曼增益为
$$ \\mathbf{K}_t = \\mathbf{\\Sigma}_t \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\tag{3.32} $$
则得到
$$ \\boldsymbol{\\mu}_t = \\overline{\\boldsymbol{\\mu}}_{t} + \\mathbf{K}_t ( \\mathbf{z}_t - \\mathbf{C}_t \\overline{\\boldsymbol{\\mu}}_{t} ) \\tag{3.33} $$
由于式(3.32)等式右边包含 $\\mathbf{\\Sigma}_t$ ，故对其变形
$$ \\begin{eqnarray*} \\mathbf{K}_t &=& \\mathbf{\\Sigma}_t \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\\\ &=& \\mathbf{\\Sigma}_t \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\underbrace{ ( \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T +\\mathbf{Q}_t )( \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T +\\mathbf{Q}_t)^{-1} }_{\\mathbf{I}} \\\\ &=& \\mathbf{\\Sigma}_t ( \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T + \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\mathbf{Q}_t ) ( \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T +\\mathbf{Q}_t)^{-1} \\\\ &=& \\mathbf{\\Sigma}_t ( \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T + \\overline{\\mathbf{\\Sigma}}\\,_t^{-1} \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T ) ( \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T +\\mathbf{Q}_t)^{-1} \\\\ &=& \\mathbf{\\Sigma}_t \\underbrace{ ( \\mathbf{C}_t^T \\mathbf{Q}_t ^{-1} \\mathbf{C}_t + \\overline{\\mathbf{\\Sigma}}\\,_t^{-1} ) }_{ \\mathbf{\\Sigma}_t^{-1} } \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T ( \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T +\\mathbf{Q}_t)^{-1} \\\\ &=& \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T ( \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_{t} \\mathbf{C}_t^T +\\mathbf{Q}_t)^{-1} \\end{eqnarray*} \\tag{3.34} $$
利用求逆引理，式(3.30)可以化简为
$$ \\begin{eqnarray*} \\mathbf{\\Sigma}_t &=& (\\mathbf{C}_t^T \\mathbf{Q}_t ^{-1}\\mathbf{C}_t + \\overline{\\mathbf{\\Sigma}}\\,_{t}^{-1} )^{-1} \\\\ &=& \\overline{\\mathbf{\\Sigma}}_t - \\overline{\\mathbf{\\Sigma}}_t \\mathbf{C}_t ^T ( \\mathbf{Q}_t + \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_t \\mathbf{Q}_t^T )^{-1} \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_t \\\\ &=& [\\, \\mathbf{I} - \\underbrace{ \\overline{\\mathbf{\\Sigma}}_t \\mathbf{C}_t ^T ( \\mathbf{Q}_t + \\mathbf{C}_t \\overline{\\mathbf{\\Sigma}}_t \\mathbf{Q}_t^T )^{-1} }_{ \\mathbf{K}_t } \\mathbf{C}_t \\,] \\overline{\\mathbf{\\Sigma}}_t \\\\ &=& (\\mathbf{I} - \\mathbf{K}_t \\mathbf{C}_t ) \\overline{\\mathbf{\\Sigma}}_t \\end{eqnarray*} \\tag{3.35} $$
式(3.34)(3.33)(3.35)与程序3.1的3~5行相对应，即证毕。$\\square$
2.扩展卡尔曼滤波（EKF）
扩展卡尔曼滤波是卡尔曼滤波在非线性情形下的近似推广，具体而言就是 放宽了关于式(3.3)(3.5)的假设，这里假设状态转移概率和测量概率分别由非线性函数 $g(\\cdot)$ 和 $h(\\cdot)$ 控制：
$$ \\mathbf{x}_t = g(\\mathbf{u}_t, \\mathbf{x}_{t-1}) + \\boldsymbol{\\varepsilon}_t \\tag{3.36} $$
$$ \\mathbf{z}_t = h(\\mathbf{x}_t) + \\boldsymbol{\\delta}_t \\tag{3.37} $$
如果直接按照前面的数学推导来计算，在计算式(3.12)时将会出现问题，因为需要确定 $\\frac{ \\partial g(\\mathbf{u}_t, \\mathbf{x}_{t-1}) }{ \\partial \\mathbf{x}_{t-1} }$ ，这也将导致式(3.13)的二阶导变得十分复杂。
为了方便推导，EKF利用一阶泰勒展开的方式将非线性函数线性化，即
$$ \\begin{eqnarray*} g(\\mathbf{u}_t, \\mathbf{x}_{t-1}) & \\approx & g(\\mathbf{u}_t, \\boldsymbol{\\mu}_{t-1}) + g\'( \\mathbf{u}_t, \\boldsymbol{\\mu}_{t-1} ) ( \\mathbf{x}_{t-1} - \\boldsymbol{\\mu}_{t-1} ) \\\\ & = & g(\\mathbf{u}_t, \\boldsymbol{\\mu}_{t-1}) + \\mathbf{G}_t ( \\mathbf{x}_{t-1} - \\boldsymbol{\\mu}_{t-1} ) \\end{eqnarray*} \\tag{3.38} $$
$$ \\begin{eqnarray*} h( \\mathbf{x}_t ) & \\approx & h( \\overline{ \\boldsymbol{\\mu} }_t ) + h\'( \\overline{ \\boldsymbol{\\mu} }_t ) ( \\mathbf{x}_t - \\overline{ \\boldsymbol{\\mu} }_t ) \\\\ & = & h( \\overline{ \\boldsymbol{\\mu} }_t ) + \\mathbf{H}_t ( \\mathbf{x}_t - \\overline{ \\boldsymbol{\\mu} }_t ) \\end{eqnarray*} \\tag{3.39} $$
其中， $ \\mathbf{G}_t = \\frac{ \\partial g(\\mathbf{u}_t, \\mathbf{x}_{t-1} \\,) }{ \\partial \\mathbf{x}_{t-1} } |_{ \\mathbf{x}_{t-1} \\,=\\, \\boldsymbol{\\mu}_{t-1} } $、$ \\mathbf{H}_t = \\frac{ \\partial h(\\mathbf{x}_{t}) }{ \\partial \\mathbf{x}_{t} } |_{ \\mathbf{x}_{t} = \\overline{ \\boldsymbol{\\mu} }_t } $
由此得到扩展卡尔曼滤波算法，如程序3.2。
| 程序3.2 扩展卡尔曼(EKF)滤波算法 | |
| 1: | Algorithm Extended_Kalman_filter($\\boldsymbol{\\mu}_{t-1}$, $\\mathbf{\\Sigma}_{t-1}$, $\\mathbf{u}_t$, $\\mathbf{z}_t$) |
| 2: | $ \\overline{\\boldsymbol{\\mu}}_t = g(\\mathbf{u}_t, \\boldsymbol{\\mu}_{t-1}) $ |
| 3: | $ \\overline{\\mathbf{\\Sigma}}_t = \\mathbf{G}_t \\mathbf{\\Sigma}_{t-1} \\mathbf{G}_t^T + \\mathbf{R}_t $ |
| 4: | $ \\mathbf{K}_t = \\overline{\\mathbf{\\Sigma}}_t \\mathbf{H}_t^T (\\mathbf{H}_t \\overline{\\mathbf{\\Sigma}}_t \\mathbf{H}_t^T + \\mathbf{Q}_t)^{-1} $ |
| 5: | $ \\boldsymbol{\\mu}_t = \\overline{\\boldsymbol{\\mu}}_t + \\mathbf{K}_t (\\mathbf{z}_t - h(\\overline{\\boldsymbol{\\mu}}_t ) ) $ |
| 6: | $ \\mathbf{\\Sigma}_t = (\\mathbf{I} - \\mathbf{K}_t \\mathbf{H}_t) \\overline{\\mathbf{\\Sigma}}_t $ |
| 7: | return $\\boldsymbol{\\mu}_t$, $\\mathbf{\\Sigma}_t$ |
可以发现，程序3.2与程序3.1非常类似，不同的是 1）在求 $\\overline{\\boldsymbol{\\mu}}_t$、$\\boldsymbol{\\mu}_t$ 是直接使用非线性函数$g(\\cdot)$、$h(\\cdot)$计算；2）其余部分只是做了简单替换$ \\mathbf{A}_t \\rightarrow \\mathbf{G}_t $、$ \\mathbf{C}_t \\rightarrow \\mathbf{H}_t $。在按照式(3.38)(3.39)的近似后，扩展卡尔曼的数学推导与卡尔曼的推导相同。
3.无迹卡尔曼滤波（UKF）
无迹卡尔曼滤波的思想是挑选几个关键点，通过加权统计来计算均值与方差，以实现滤波过程。
| 程序3.3 无迹卡尔曼(UKF)滤波算法 | |
| 1: | Algorithm Unscented_Kalman_filter($\\boldsymbol{\\mu}_{t-1}$, $\\mathbf{\\Sigma}_{t-1}$, $\\mathbf{u}_t$, $\\mathbf{z}_t$) |
| 2: | $ \\boldsymbol{\\chi }_{t-1} = ( \\boldsymbol{\\mu}_{t-1} \\;\\;\\;\\; \\boldsymbol{\\mu}_{t-1}+\\gamma \\sqrt{ \\mathbf{\\Sigma}_{t-1} } \\;\\;\\;\\; \\boldsymbol{\\mu}_{t-1} - \\gamma \\sqrt{ \\mathbf{\\Sigma}_{t-1} } ) $ |
| 3: | $ \\bar{\\boldsymbol{\\chi }}_{t}^{\\ast } = g( \\mathbf{u}_t , \\boldsymbol{\\chi }_{t-1} ) $ |
| 4: | $ \\bar{\\boldsymbol{\\mu}}_t = \\sum_{i=0}^{2n} w_m^{[i]} \\bar{\\boldsymbol{\\chi }}\\,\\!_{t}^{\\ast [i] } $ |
| 5: | $ \\overline{\\mathbf{\\Sigma}}_t = \\sum_{i=0}^{2n} w_c^{[i]} ( \\bar{\\boldsymbol{\\chi }}\\,\\!_{t}^{\\ast [i] } - \\bar{\\boldsymbol{\\mu}}_t ) ( \\bar{\\boldsymbol{\\chi }}\\,\\!_{t}^{\\ast [i] } - \\bar{\\boldsymbol{\\mu}}_t ) ^T + \\mathbf{R}_t $ |
| 6: | $ \\overline{\\boldsymbol{\\chi }}_t = ( \\bar{\\boldsymbol{\\mu}}_t \\;\\;\\;\\; \\bar{\\boldsymbol{\\mu}}_t + \\gamma \\sqrt{ \\bar{\\mathbf{\\Sigma}}_t } \\;\\;\\;\\; \\bar{\\boldsymbol{\\mu}}_t - \\gamma \\sqrt{ \\bar{\\mathbf{\\Sigma}}_t } ) $ |
| 7: | $ \\overline{\\mathbf{Z}}_t = h (\\bar{\\boldsymbol{\\chi }}_t) $ |
| 8: | $ \\hat{\\mathbf{z}}_t = \\sum_{i=0}^{2n} w_m^{[i]} \\overline{\\mathbf{Z}}\\,\\!_t^{[i]} $ |
| 9: | $ \\mathbf{S}_t = \\sum_{i=0}^{2n} w_c^{[i]} ( \\overline{\\mathbf{Z}}\\,\\!_t^{[i]} - \\hat{\\mathbf{z}}_t) ( \\overline{\\mathbf{Z}}\\,\\!_t^{[i]} - \\hat{\\mathbf{z}}_t)^T + \\mathbf{Q}_t $ |
| 10: | $ \\overline{\\mathbf{\\Sigma}}\\,\\!_t^{x,z} = \\sum_{i=0}^{2n} w_c^{[i]} ( \\bar{\\boldsymbol{\\chi }}\\,\\!_t^{[i]} - \\bar{\\boldsymbol{\\mu}}_t ) ( \\overline{\\mathbf{Z}}\\,\\!_t^{[i]} - \\hat{\\mathbf{z}}_t)^T $ |
| 11: | $ \\mathbf{K}_t = \\overline{\\mathbf{\\Sigma}}\\,\\!_t^{x,z} \\mathbf{S}_t^{-1} $ |
| 12: | $ \\boldsymbol{\\mu}_t = \\bar{\\boldsymbol{\\mu}}_t + \\mathbf{K}_t ( \\mathbf{z}_t - \\hat{\\mathbf{z}}_t)$ |
| 13: | $ \\mathbf{\\Sigma}_t = \\overline{\\mathbf{\\Sigma}}_t - \\mathbf{K}_t \\mathbf{S}_t \\mathbf{K}_t^T $ |
| 14: | return $ \\boldsymbol{\\mu}_t $, $\\mathbf{\\Sigma}_t$ |
以上是关于概率机器人3.1 卡尔曼滤波扩展卡尔曼滤波和无迹卡尔曼滤波的主要内容，如果未能解决你的问题，请参考以下文章
机动目标跟踪—当前统计模型（CS模型）扩展卡尔曼滤波/无迹卡尔曼滤波 matlab实现
机动目标跟踪—当前统计模型（CS模型）扩展卡尔曼滤波/无迹卡尔曼滤波 matlab实现