概率机器人总结——(扩展)卡尔曼滤波先实践再推导

概率机器人总结——(扩展)卡尔曼滤波先实践再推导

为什么要把扩展两个字加个括号呢，因为本文的实践过程是以扩展卡尔曼为例，但是推导过程主要是参考博客卡尔曼滤波 – 从推导到应用(一)，相较于《概率机器人》上的推导过程更容易理解，而《概率机器人》上对于扩展卡尔曼滤波的推到也是基于卡尔曼滤波进行了一个拓展，因此本文以卡尔曼滤波推导为学习样例，然后加入个人的一些理解分析

（1）卡尔曼、扩展卡尔曼、粒子滤波到底什么关系？

徐亦达老师的卡尔曼滤波的推导视频里强调了这几个滤波的关系，如下：

模型类型	$p\left(x_k\mid x_{k-1}\right)$	$p(y_t\mid x_t)$	$p(x_1)$	说明
离散状态动态模型	$A{x}_t+B{u}_t$	$H{x}_t$	$\pi$	隐马尔科夫模型，无滤波
线性高斯动态模型	$N(A{x}_t+B{u}_t+\delta ,Q)$	$N(H{x}_t+c,R)$	$N(\mu ,\sigma )$	卡尔曼滤波
非线性非高斯动态模型	$f(x_{t-1})$	$g(x_{t-1})$	$f(x_0)$	粒子滤波
首先什么是动态模型呢？
这个就是状态模型，在离散状态下就是大名鼎鼎的隐马尔科夫模型，在机器学习范畴内非常有效的一种模型，而当我们的状态转移方程和观测方程都变成线性高斯模型之后，这时候处理最有效的方式之一就是卡尔曼滤波，而扩展卡尔曼呢？当动态模型都为非线性高斯模型是就是用扩展卡尔曼滤波（其基本思路就是将非线性模型线性化），而最后最复杂的是非线性非高斯模型，怎么办呢？粒子滤波咯，粒子滤波我写了一个同款博客，欢迎参考概率机器人总结——粒子滤波先实践再推导，这就是他们之间的关系。

（2）实践——扩展卡尔曼滤波

首先给出线性高斯动态模型的状态转移方程和观测方程，如下：$$x_t=A_t{x}_{t-1}+B_t{u}_t+{\epsilon }_t$$$$z_t=C_t{x}_t+{\delta }_t$$
其中${\epsilon }_t$和${\delta }_t$分别为满足零均值高斯分布的噪声。
然后，我们从卡尔曼滤波的公式开始，如下：
$\begin{array}{rl}{\overline{\mu }}_t& =A_t{\mu }_{t-1}+B_t{u}_t\\ {\overline{\Sigma }}_t& =A_t{\Sigma }_{t-1}{A}_t^T+R_t\\ K_t& ={\overline{\Sigma }}_t{C}_t^T{\left(C_t{\overline{\Sigma }}_t{C}_t^T+Q_t\right)}^{-1}\\ {\mu }_t& ={\overline{\mu }}_t+K_t\left(z_t-C_t{\overline{\mu }}_t\right)\\ {\Sigma }_t& =\left(I-K_t{C}_t\right){\overline{\Sigma }}_t\end{array}$
如果没有接触过卡尔曼滤波，第一次看到这些公式会觉得怎么这么复杂，学明白之后发现其实还是蛮简单的，这里先简单说下对这个公式的理解
第一个公式和均值（期望）有关，指的是通过上一个状态的均值${\mu }_{t-1}$和控制量以及状态转移方程预测出下一个状态的均值${\overline{\mu }}_t$
第二个公式和方差有关，指的是根据上一个状态的方差${\Sigma }_{t-1}$和噪声的方差$R_t$预测出下一个状态的方差${\overline{\Sigma }}_t$

注意高斯分布的状态的变量其实就均值和方差两个东西，通过第一步和第二步我们就完成了【预测过程】

第三个公式和卡尔曼增益有关，指的是通过观测方程及其噪声的方差计算卡尔曼增益
第四个公式和均值有关，指的是通过卡尔曼增益对预测出来的均值进行更新
第五个公式和方差有关，和第四步一样，指的是通过卡尔曼增益对预测出来方差进行更新

我们先计算了卡尔曼增益然后分别跟新了均值和方差两个东西，通过后面三步我们就完成了【更新过程】

好了，卡尔曼滤波搞清楚了，我们再来看下扩展卡尔曼滤波，首先给出非线性高斯动态模型的方程，如下：$$x_t=g\left(u_t,x_{t-1}\right)+{\epsilon }_t$$$$z_t=h\left(x_t\right)+{\delta }_t$$
同理，其中${\epsilon }_t$和${\delta }_t$分别为满足零均值高斯分布的噪声。
$\begin{array}{rl}{\overline{\mu }}_t& =g\left(u_t,{\mu }_{t-1}\right)\\ {\overline{\Sigma }}_t& =G_t{\Sigma }_{t-1}{G}_t^T+R_t\\ K_t& ={\overline{\Sigma }}_t{H}_t^T{\left(H_t{\overline{\Sigma }}_t{H}_t^T+Q_t\right)}^{-1}\\ {\mu }_t& ={\overline{\mu }}_t+K_t\left(z_t-h\left({\overline{\mu }}_t\right)\right)\\ {\Sigma }_t& =\left(I-K_t{H}_t\right){\overline{\Sigma }}_t\end{array}$
和卡尔曼滤波一对比，发现简直一模一样，主要不同的是将状态转移方程中系数矩阵$A_t,C_t$换成了非线性方程的雅克比矩阵$G_t,H_t$，为什么是雅克比矩阵，这当然是和推导有关，我们可以先直观感受下，在《概率机器人》的推导过程中，在对状态转移的分布进行积分获状态的置信度时需要提取出来一个关键公式：$$\begin{array}{rl}{L}_t=& \frac{1}{2}{\left(x_t-A_t{x}_{t-1}-B_t{u}_t\right)}^{\mathrm{T}}{R}_t^{-1}\left(x_t-A_t{x}_{t-1}-B_t{u}_t\right)+\\ & \frac{1}{2}{\left(x_{t-1}-{\mu }_{t-1}\right)}^{\mathrm{T}}{\Sigma }_{t-1}^{-1}\left(x_{t-1}-{\mu }_{t-1}\right)\end{array}$$
而扩展卡尔曼滤波的这个对应的公式是$$\begin{array}{rl}{L}_t=& \frac{1}{2}{\left[x_t-g\left({\mathbf{u}}_t,{\mathbf{\mu }}_{t-1}\right)-G_t\left({\mathbf{x}}_{t-1}-{\mathbf{\mu }}_{t-1}\right)\right]}^{\mathrm{T}}{\mathbf{R}}_t^{-1}\left[{\mathbf{x}}_t-g\left({\mathbf{u}}_t,{\mathbf{\mu }}_{t-1}\right)-G_t\left({\mathbf{x}}_{t-1}-{\mathbf{\mu }}_{t-1}\right)\right]\\ & +\frac{1}{2}{\left({\mathbf{x}}_{t-1}-{\mathbf{\mu }}_{t-1}\right)}^{\mathrm{T}}{\mathbf{\Sigma }}_{t-1}^{-1}\left({\mathbf{x}}_{\mathbf{t}-1}-{\mathbf{\mu }}_{t-1}\right)\end{array}$$
我们发现状态$x_t-1$前的系数矩阵就是由$A_t$变为了$G_t$，但是这样理解可能还不够直观。

下面我们可以看下PythonRobotics中提供的拓展卡尔曼滤波的python代码是怎么写的

def ekf_estimation(xEst, PEst, z, u):

    #  Predict
    xPred = motion_model(xEst, u)
    jF = jacobF(xPred, u)
    PPred = jF.dot(PEst).dot(jF.T) + R

    #  Update
    jH = jacobH(xPred)
    zPred = observation_model(xPred)
    y = z.T - zPred
    S = jH.dot(PPred).dot(jH.T) + Q
    K = PPred.dot(jH.T).dot(np.linalg.inv(S))
    xEst = xPred + K.dot(y)
    PEst = (np.eye(len(xEst)) - K.dot(jH)).dot(PPred)

    return xEst, PEst

我这里都不要太多解释，对应着公式看就能都对应上，这里比较有意思的一个点是代码为了追求简单采样了线性高斯动态模型来代替的非线性高斯动态模型，就是用扩展卡尔曼滤波来解了一个线性高斯动态模型，我们可以看下这个雅克比矩阵是怎么处理的，以状态转移方程为例：

def jacobF(x, u):
    yaw = x[2, 0]
    v = u[0, 0]
    jF = np.array([
        [1.0, 0.0, -DT * v * math.sin(yaw), DT * math.cos(yaw)],
        [0.0, 1.0, DT * v * math.cos(yaw), DT * math.sin(yaw)],
        [0.0, 0.0, 1.0, 0.0],
        [0.0, 0.0, 0.0, 1.0]])

    return jF

代码中这个矩阵是怎么来的呢？
首先状态转移方程的模型是这样的：$${\mathbf{x}}_{t+1}=F{\mathbf{x}}_t+B{\mathbf{u}}_t$$
其中$F=\left[\begin{array}{llll}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 0\end{array}\right]$以及$B=\left[\begin{array}{cc}\cos (\varphi )dt& 0\\ \sin (\varphi )dt& 0\\ 0& dt\\ 1& 0\end{array}\right]$

然后雅克比矩阵的计算公式是$$J_F=\left[\begin{array}{cccc}\frac{dx}{dx}& \frac{dx}{dy}& \frac{dx}{d\varphi }& \frac{dx}{dv}\\ \frac{dy}{dx}& \frac{dy}{dy}& \frac{dy}{d\varphi }& \frac{dy}{dv}\\ \frac{d\varphi }{dx}& \frac{d\varphi }{dy}& \frac{d\varphi }{d\varphi }& \frac{d\varphi }{dv}\\ \frac{dv}{dx}& \frac{dv}{dy}& \frac{dv}{d\varphi }& \frac{dv}{dv}\end{array}\right]$$把所有变量都带进去就可以得到$$J_F=\left[\begin{array}{cccc}1& 0& -v\sin (\varphi )dt& \cos (\varphi )dt\\ 0& 1& v\cos (\varphi )dt& \sin (\varphi )dt\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$通过这个例子可以加深我们对扩展卡尔曼滤波的实际操作的理解。
然后代码运行的结果如下（建议读下源码，对理解很有帮助）：

（3）推导——卡尔曼滤波

卡尔曼滤波的推导各有千秋，《概率机器人》上是直接对高斯分布的形式进行积分，然后通过求积分结果（正态分布）的极小值和方差获得预测或者下一状态的均值和方差，而徐亦达老师的视频课程是通过高斯形式的联合分布来给出结论的，但是给我印象最深的还是白巧克力亦唯心博主（贺博，大佬大佬…）给出来的推导过程，下面我基于鉴于博客的推导过程给出自己的理解。

首先给出运动方程和观测方程，这应该没什么问题：
$$x_k=A{x}_{k-1}+B{u}_{k-1}+w_{k-1}（1）$$$$z_k=H{x}_k+v_k（2）$$其中，$p(w)\sim N(0,Q)$，$p(v)\sim N(0,R)$
我们首先明确，这两个方程我们都是已知的，我们能够通过运动方程得到理论预测（先验），然后我们又有观测数据，通过观测数据我们可以对理论预测进行修正，从而得到后验。

这里我们给出这样一个修正公式$${\hat{x}}_k={\hat{x}}_k^{\prime }+K_k\left(z_k-{\hat{z}}_k\right)={\hat{x}}_k^{\prime }+K_k\left(z_k-H{\hat{x}}_k^{\prime }\right)（3）$$其中，${\hat{x}}_k^{\prime }$是我们理论预测值（先验），${\hat{x}}_k$是我们修正后的估计值（后验），${\hat{z}}_k$是测量值的预测值（就是将我们的状态预测值，${\hat{x}}_k^{\prime }$带入观测方程得到的结果），$z_k$就是我们实际的测量值，$\left(z_k-H{\hat{x}}_k^{\prime }\right)$就是残差。

这里通过协方差矩阵往下推，首先给出估计值和真实值之前的协方差矩阵：$$P_k=E\left[e_k{e}_k^T\right]=E\left[\left(x_k-{\hat{x}}_k\right){\left(x_k-{\hat{x}}_k\right)}^T\right]（4）$$将上文的（3）式和（2）式代入得$$P_k=E[\left[\left(I-K_{k}H\right)\left(x_k-{\hat{x}}_k^{\prime }\right)-K_k{v}_k\right]{\left[\left(I-K_{k}H\right)\left(x_k-{\hat{x}}_k^{\prime }\right)-K_k{v}_k\right]}^T]（5）$$然后我们给出理论预测值和真实值之间的协方差矩阵$$P_k^{\prime }=E\left[e_k^{\prime }{e}_k^{\prime T}\right]=E\left[\left(x_k-{\hat{x}}_k^{\prime }\right){\left(x_k-{\hat{x}}_k^{\prime }\right)}^T\right]$$展开（5）式得$$P_k=\left(I-K_{k}H\right)E\left[\left(x_k-{\hat{x}}_k^{\prime }\right){\left(x_k-{\hat{x}}_k^{\prime }\right)}^T\right]\left(I-K_{k}H\right)+K_{k}E\left[v_k{v}_k^T\right]K_k^T$$得$$P_k=\left(I-K_{k}H\right)P_k^{\prime }{\left(I-K_{k}H\right)}^T+K_{k}R{K}_k^T$$得$$P_k=P_k^{\prime }-K_{k}H{P}_k^{\prime }-P_k^{\prime }{H}^T{K}_k^T+K_k\left(H{P}_k^{\prime }{H}^T+R\right)K_k^T$$协方差的矩阵的对角线元素就是方差，我们对上式所有对角线元素求和，求和结果满足下式$$T\left[P_k\right]=T\left[P_k^{\prime }\right]-2T\left[K_{k}H{P}_k^{\prime }\right]+T\left[K_k\left(H{P}_k^{\prime }{H}^T+R\right)K_k^T\right]$$我们求一个K使得最小均方误差最小，于是对上式求导有$$\frac{dT\left[P_k\right]}{d{K}_k}=-2{\left(H{P}_k^{\prime }\right)}^T+2K_k\left(H{P}_k^{\prime }{H}^T+R\right)$$$$K_k=P_k^{\prime }{H}^T{\left(H{P}_k^{\prime }{H}^T+R\right)}^{-1}$$当当当！这就是卡尔曼增益啦，卡尔曼增益里面包含了一个我们前文提到过的$P_k^{\prime }$（预测值与真实值之间的协方差矩阵），它是怎么求的呢？通过我们预测过程求解出来的：$$\begin{array}{rl}{P}_{k+1}^{\prime }& =E\left[e_{k+1}^T{e}_{k+1}^{T\prime }\right]\\ & =E\left[\left(x_{k+1}-{\hat{x}}_{k+1}^{\prime }\right){\left(x_{k+1}-{\hat{x}}_{k+1}^{\prime }\right)}^r\right]\\ & =E\left[\left(A\left(x_k-{\hat{x}}_k\right)+{\omega }_k\right){\left(A\left(x_k-{\hat{x}}_k\right)+{\omega }_{kt}\right)}^r\right]\\ & =E\left[\left(A{e}_k\right){\left(A{e}_k\right)}^r\right]+E\left[{\omega }_k{\omega }_k^T\right]\\ & =A{P}_k{A}^T+Q\end{array}$$倒数第三步到倒数第二步的原因是状态变量与噪声独立，到这里卡尔曼滤波中的所有公式都可解释了，是不是很爽快！

（4）顺便问一句，如何通过卡尔曼滤波进行多传感器融合？

扩展卡尔曼滤波EKF与多传感器融合这个博客简单地讲清楚了这个问题，如下两张PPT所示:

如果卡尔曼滤波搞得差不多清楚了的话这两张图就很好理解了，Predicition预测的方式是相同的，Update更新是他们的测量方程不同，例如这篇博客中举的例子，对于激光而言，观测矩阵如下：$$z=Hx+R=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\end{array}\right]\left[\begin{array}{c}{p}_x\\ p_y\\ v_x\\ v_y\end{array}\right]+\left[\begin{array}{cc}0.0225& 0\\ 0& 0.0225\end{array}\right]$$对于雷达而言$$z=f(x)+R=\left[\begin{array}{c}\rho \\ \varphi \\ \dot{\rho }\end{array}\right]=\left[\begin{array}{c}\sqrt{p_x^2+p_y^2}\\ \arctan \frac{p_y}{p_x}\\ \frac{p_x{v}_x+p_y{v}_y}{\sqrt{p_x^2+p_y^2}}\end{array}\right]+\left[\begin{array}{ccc}0.09& 0& 0\\ 0& 0.0009& 0\\ 0& 0& 0.09\end{array}\right]$$
因此在扩展卡尔曼滤波的更新过程中就会不一样，在工程实践方面肯定还存在这许多实践细节要注意，这里就不在细究，之后遇到实际项目时再进一步考虑。

就到这里啦，有问题欢迎交流～

此外，对SLAM算法感兴趣的同学可以看考我的博客SLAM算法总结——经典SLAM算法框架总结