卡尔曼滤波器算法

本文购买自 https://gitchat.csdn.net/activity/5d37aeb1b5590f6f299a9cdc，修改了公式乱码，仅供学习，请支持原版购买

文章正文

首先声明一下，此文是预备给初学者阅读的，旨在帮助初学者更好地理解卡尔曼滤波器算法，降低公式推导的难度，所以我尽量避免一些太过于学术化的讲解。

我在学习卡尔曼滤波器算法时，也花了不少时间。

有时候觉得自己弄懂了，遇到新的问题时，还是不知道灵活运用，然后就发现自己还是没有弄懂，自己只是因为照着别人的博客顺着别人的思路顺利阅读完毕而已，脱离了博客中的示例场景，切换到新的场景，仍然不能很好地运用，反反复复之后，某天闭眼也能手推卡尔曼滤波器涉及到的 5 个公式时，仿佛有大彻大悟的感觉，然后，我才认为自己再遇到新的问题时，我不会再像之前那样不知从何入手了。

好了，正文开始。
先看看为什么叫“卡尔曼”。跟其他著名的理论（例如傅立叶变换，泰勒级数等等）一样，卡尔曼也是一个人的名字，而跟他们不同的是，他是个现代人！

卡尔曼全名 Rudolf Emil Kalman

匈牙利数学家，1930 年出生于匈牙利首都布达佩斯。1953，1954 年于麻省理工学院分别获得电机工程学士及硕士学位。1957 年于哥伦比亚大学获得博士学位。我们现在要学习的卡尔曼滤波器，正是源于他的博士论文和 1960 年发表的论文《A New Approach to Linear Filtering and Prediction Problems》（线性滤波与预测问题的新方法）。

简单来说，卡尔曼滤波器是一个“optimal recursive data processing algorithm（最优化自回归数据处理算法）”。对于解决很大部分的问题，他是最优，效率最高甚至是最有用的。他的广泛应用已经超过 30 年，包括机器人导航，控制，传感器数据融合甚至在军事方面的雷达系统以及导弹追踪等等。近年来更被应用于计算机图像处理，例如头脸识别，图像分割，图像边缘检测等等。

1. 什么是卡尔曼滤波器？

卡尔曼滤波器是一种估计算法，在上个世纪六十年代就大显神威了，NASA 将它应用在阿波罗飞船的轨迹预测上面，所以它称得上是一个能上天的算法。

卡尔曼滤波器能够对未来数据做预测，到今天，它在许多行业都发挥了重大的作用，如导航、自动驾驶、电池电量预测、雷达目标追踪、机器视觉等等。

你可以将卡尔曼滤波器算法应用在你想要预测的领域，例如市场上猪肉的价格、房间里面的温度、太阳底下你影子的长度变化、你个人网站或者是博客或者是 QQ 空间的访问量等，这个是灵活的，只要你愿意尝试，反正它是一个优秀的估计算法，耗费的硬件资源也少，灵活方便，每一个程序员都应该了解一下。

2. 如何通俗解释卡尔曼滤波器？

我们已经知道，卡尔曼滤波器算法可以做估算。这个小节，我进一步举例阐述，让大家对它的逻辑有一个进一步的感性认知提神。

首先，我们要明确地记得卡尔曼是用来估算的，这是我们使用的目的，很多人被复杂的公式推导搞不清头脑，最终忘记了卡尔曼滤波器算法的本来目的，从而让自己特别纠结踌躇。

其次，如果我们能够有办法确定每时每刻的事件，那么就不需要运用卡尔曼滤波器。卡尔曼滤波器算法适用于不能保证能够准确确认的事件。

简单来讲，卡尔曼滤波就是加权，给不同的数据分配不同的权重，然后产生新的数据，也是数据融合的意思。

比如，公司叫你和小李同时评价新员工的表现。

你说这人太不靠谱了，满分为 10 分的情况下最多打 6 分，小李却说人不错，可以给 8 分。

那上司怎么办呢？一般情况下最简单的方法是求平均值，也就是 6+8 等于 14，平均分是 7。

但一般靠谱的领导会有类似的逻辑处理事情。

在领导眼中，你和小李说话的可信度是不一样的。所以他可能信你少一些，也可能信你多一点，所以，最终的结果就不会是加权平均。可能是 6*0.3+8*0.7=7.4。

这等效于这样的公式：

最优数据 = 你的数据 + K(小李的数据 − 你的数据)
卡尔曼滤波器也正式类似的逻辑，不过做了些许巧妙的处理，首先它会计算你们的误差，在这里是 8 分减去 6 分，为 2 分，接着用一个比例 K，取值 0~1，最终结果是两者都有考虑，但是权重不一致。

$6+K∗2$

如果 K 为 0，则上司完全信任你，你说多少分就是多少分，如果 K 为 1 的话，那么就是 8 分，代表上司完全信任小李的评价。其它的情况，K 在 0 和 1 之间，最终的结果是你们的分数综合，但是权重不一样。

K 等于 0.5 时，两者权重一样。

K 小于 0.5 时，偏向你，反之偏向小李。

在卡尔曼滤波器中 K 被称为卡尔曼增益。

到这里，我们基本上知道了卡尔曼滤波器大概是怎么一回事了，下面我们还需要理解 3 个概念才算更进一步的理解。

预测（predict）
测量（mesasure）
估算（estimate）

我打个比方。

王老板是做肉类批发生意的，他手下有员工小 A 和小 B。

小 A 负责每天早上给王老板预测当天肉类的价格。

小 B 每周一上午 9 点到专门的行政机构去摘抄数据，然后汇报给王老板。

所以，王老板有 2 个数据源，王老板懂得卡尔曼滤波器算法，所以，他能够搞定价格估算这件事情。

小 A 和小 B 的数据来源不一样，误差也不一样。

小 A 的数据是靠经验拍脑袋决定的，被称为预测（predict）。

小 B 的数据来源于机构的统计，准确度比较高，被称为测量（measure），也被称为校正（correct）。

有同学可能会有疑问，小 B 的数据很准确，那么我们为什么不只用小 B 的数据呢？

关键是，条件不允许啊，现实生活中，很多的东西都是被制约的，比如，小 B 只能在周一上午能够得到数据，但是其它 6 天，王老板也得做生意啊，因此，王老板必须依赖于小 A 的经验预测。

但是为了防止这种拍脑袋的数据结果偏差太大，所以王老板才会在适当的时候引入小 B 的数据，来校正小 A 的预测，最终王老板综合两者，得到了自己的估算数据，并且将它作为每天的结果。

这个例子，就是典型的卡尔曼滤波器算法所要涉及的基本概念示例。

到这一步，我们已经很清楚知道了这个算法的概念，包括预测、测量、估算，也知道需要用一个卡尔曼增益来加权调和 2 个数据形成最终的估算数据。

如果是文科生，或者其他理工人员，了解了这么多后，已经可以和人家谈笑风生讲这个神奇的算法了。

但是，作为一个相关的软件工程师，这个还不够的。

你还需要知道，如何套用公式去求解：

预测的数据值 ,公式中用 $\hat{}$ 表示
测量的数据值，公式中用 $\bar{}$ 表示
卡尔曼增益，公式中用 K 表示
最终估算的数据值，公式中用 $\tilde{}$ 表示

如果，你是一个算法工程师，光知道用还是不够的，你得清楚知道公式怎么来的，透彻理解它的公式后，遇到新的应用场景，你才能从容不迫。

不然，总是反反复复陷入一种似懂非懂的困境。

3. 卡尔曼滤波器相关的公式

卡尔曼滤波器算法是时域上的线性变换估算。

在一个系统中，卡尔曼滤波器用状态(state）来描述某个时刻的系统，它是一个向量。

比如，我要用一个向量来描述我每天的健康状态，我选用了最低心率、最低血压、疲劳度 3 个指标。

$x=(68,88,20)^T$

这个数据怎么来的呢？

我可以每周去医院测量一次，可以准确获得。

其它时间，是我自己的预测，虽然和真实值有出入，但大致还过的过去。

最关键的是，我今天的预测和昨天相关，明天的和今天的相关，因为状态是向量，所以很容易用线性代数的方式描述。

$x_{t} = F*x_{t-1} + \omega_{t}$

t 表示当前时刻，t-1 表示上一时刻，ω 表示此次预测的噪声，假定 ω 符合高斯分布。
F 是状态转移矩阵，因为在线性代数中，矩阵表示变换，所以一个向量与一个矩阵相乘，一般会得到一个新的向量。
F 的值与具体的问题相关，实际解决问题时，要分析状态到状态变换时的关系。

比如，我认为我每天的状态相差不大。所以，我的 F 可以是单位矩阵。

$\left\{ \begin{matrix} 1 & 0 & 0 \\ 0& 1 & 0 \\ 0 & 0 & 1 \\ \end{matrix} \right\}$
状态的误差可以设置为

$ω=(1,1,4)^T$

实际上，完整的状态变换公式应该是：

$x_{t} = F*x_{t-1} +B*\vec{u_{t}}+ \omega_{t}$

B 是控制矩阵，u 是控制变量，它们的乘积代表外力干预。
- 比如，如果让我加班，我的疲劳度会加 20，如果多放一天假，我的疲劳度会减少 15。
- B 和 U 的取值同样需要具体问题具体分析。

用 $P_{t}$ 表示 $x_{t}$ 的协方差，有下面的公式。

$P_{t} = F*P_{t-1}*F^{T} +Q_{t}$

$Q_t$ 是噪声的协方差。

这是预测相关的两个公式，下面讲测量。

用 $Z_t$ 表示测量的数据。

$Z_{t} = H*S_{t} + \vec{u_{t}}$

H 是状态变换矩阵，有时候测量的数据和预测的数据单位不统一，这个时候就需要转换。
- 举个例子，假设让你预测今天的苹果价格，你说 10 元/斤，然后，网络上的报价是 3.2$/kg，这就是单位不一致，需要用 H 这样的矩阵变换过来，才能有效处理。

测量的数据也需要记录协方差信息，用 $\bar{P_t}$ 表示，有下面的公式：

$\bar{P_{t}} = H*\hat{P_{t}}*H^{T}+R_{t}.$

Rt 是测量噪声 $\vec{v}$ 的协方差。

有了预测和测量的公式，我们还需要估算的公式，在这之前我们知道，我们要求 K,也就是卡尔曼增益。

$K = P_{t}*H^{T}(H_{k}P_{t}H^{T}+R_{t})^{-1}$

最终，我们用 $\tilde{x_t}$ 表示经卡尔曼滤波计算后的估计状态。

$\tilde{x_{t}} = \hat{x_{t}} + K * (z_{t} - H*\hat{lx_{t}})$

它也有协方差，用 $\hat{P_t}$ 表示：

$\tilde{P_{t}} = \hat{P_{t}}-K*H*\hat{P_{t}}$

与卡尔曼滤波器相关的公式就全部罗列出来了，一字排开如下：

$x_{t} = F*x_{t-1} +B*\vec{u_{t}}+ \omega_{t}$ ............ (公式1)

$\hat{P_{t}} = F*\hat{P_{t-1}}*F^{T} +Q_{t}$ ................（公式 2）

$Z_{t} = H*x_{t} + \vec{\nu_{t}}$ ....................（公式 3）

$\bar{P_{t}} = H*\hat{P_{t}}*H^{T}+R_{t}$ ...........（公式 4）

$\tilde{x_{t}} = \hat{x_{t}} + K * (z_{t} - H*\hat{x_{t}})$ .......（公式 5）

$\tilde{P_{t}} = \hat{P_{t}}-K*H*\hat{P_{t}}$ ............（公式 6）

$K = \hat{P_{t}}*H^{T}(H_{k}\hat{P_{t}}H^{T}+R_{t})^{-1}$ ....（公式 7）

有了上面的公式，只要我们好好调试预测误差和测量误差，我们就能套用卡尔曼滤波器算法来取得比较好的效果，对于一般工程师而言，这已经足够了。

但是，如果你是算法岗位的话，这个还不够的，你需要确定下面一些问题：

公式 2 怎么由公式 1 推导出来？
公式 4 怎么由公式 3 推导出来？
公式 5 怎么推导出来的？

4. 公式推导

本节的目的是便于理解为主，所以整个推导可能不会显得那么正式与严肃。

首先，定义好符号。

因为涉及到预测值、测量值、估计值，为了避免混淆，用不同的符号表示。

我们预测的状态用 $\hat{x}$ 表示，其实也可以理解为先验状态。

用 $x$ 表示理论上的绝对真值。

用 $\tilde{x}$ 表示经卡尔曼滤波求出来的估算值，也称为最优值，也可以理解为后验状态。

在这里插入图片描述

无论是预测值，还是估算值，与真实值之间还是存在误差的。

在这里插入图片描述

如上图虚线表示，分别用 $\hat{e}$ 和 $\tilde{e}$ 表示。

接下来，我们用一个协方差矩阵 $P_t$ 来表示预测值偏离真实值的状态。

根据协方差的公式可得，当前 t 时刻的状态协方差为：

$P_{t} = E[(\hat{e_{t}})(\hat{e_{t}})^T] = E[(x_{t}-\hat{x_{t}})(x_{t}-\hat{x_{t}})^T]$

因为卡尔曼滤波器是依照时间递推的，所以 t-1 时刻的状态协方差为 $P_{t−1}$ 。

$P_{t-1} = E[(\hat{e_{t-1}})(\hat{e_{t-1}})^T] = E[(x_{t-1}-\hat{x_{t-1}})(x_{t-1}-\hat{x_{t-1}})^T]$

我们前面已经知道，t 时刻的状态可以由 t-1 时刻的状态转移过来,也就是如公式 1 所示。

$x_{t} = F*x_{t-1} +B*\vec{u_{t}}+ \omega_{t}$ .........(公式1)

那么按照刚刚协方差的公式：

$P_{t} = E[(\hat{e_{t}})(\hat{e_{t}})^T] =$
$E[(F*x_{t-1}+B\vec{u_{t}}+\omega_{t}-F*\hat{x_{t-1}}-B\vec{u_{t}})(F*x_{t-1}+B\vec{u_{t}}+\omega_{t}-F*\hat{x_{t-1}}-B\vec{u_{t}})^T]$
$= E[(F(x_{t-1}-\hat{x_{t-1}})+\omega_{t})(F(x_{t-1}-\hat{x_{t-1}})+\omega_{t})^T]$

因为 x 和 ω 没有什么相关性，根据概率论知识，两个独立的变量，协方差为 0，所以上面的公式可以化简为：

$=F*E[(x_{t-1}-\hat{x_{t-1}})(x_{t-1}-\hat{x_{t-1}})^T]*F^T+E[\omega_{t}\omega_{t}^T]$

仔细观察公式，可以看出来，公式实际上就是：

$P_{t}=F*P_{t-1}*F^T+Q_{t}.$

这实际上就是公式 2，到这里，公式 2 已经就可以证明了。

神奇的是， $B\vec{u}$ 的协方差没有参与到迭代当中。

实际上，概率论中有这么一个公式。

$Cov(Ax)=E(Ax−μ)(Ax−μ)^T)=A∗Cov(x)∗A$
那么通过这个公式也能直接又公式 1 推导出公式 2。

现在看公式 3。

$Z_{t} = H*x_{t} + \vec{\nu_{t}}$ ...........（公式 3）

我们依照同样的方式可以推出公式 4，我们有 $\tilde{P_t}$ 表示测量值的协方差，很容易得到：

$\tilde{P_{t}}=H*\tilde{P_{t}}*H^T+R_{t}$

公式 4 也证明了， $R_t$ 是测量误差 $v$ 的协方差。

最难的是公式 5 的证明。

文章前面的小节我们已经有了卡尔曼增益的概率，也大概知道了怎么基于 2 个数据求最优的估计。

现在，预测值是 $\hat{x}$ ，测量值是 $z$ ，我们用 $\tilde{x}$ 来表示经卡尔曼滤波估算的值。

在这里插入图片描述

K 代表一种比例，因为有 K 的存在，所以 $\tilde{x}$ 会在 $\hat{x}$ 和 $z$ 之间取值摆动：

$\tilde{x_{t}}=\hat{x_{t}}+K(z_{t}-\hat{x_{t}})$

根据前面测量的公式，可得：

$\tilde{x_{t}}=\hat{x_{t}}+K(H*x_{t}+v_{t}-H*\hat{x_{t}})$

展开这个公式，可得：

$\tilde{x_{t}}=\hat{x_{t}}+kH*x_{t}+Kv_{t}-KH*\hat{x_{t}}$

接下来，就有一个巧妙的变换：

$\tilde{x_{t}} - x =\hat{x_{t}}-x+kH*x_{t}-KH*\hat{x_{t}} + Kv_{t}$

实际上就是：

$\tilde{e_{t}}=(I-KH)\hat{e_{t}}-Kv_{t}$

巧妙的是到这一步，估计值的误差和预测值的误差还有测量的噪声有了联系。

我们用 $\tilde{P_t}$ 表示估计值的协方差，根据协方差定义可得。

$\tilde{P_{t}} = E[\tilde{e_{t}}\tilde{e_{t}}^T]=(I-KH)\hat{P_{t}}(I-KH)^T+KR_{t}K^T$

现在，只剩下 K 的值没有确定了。

协方差代表偏离真实值的程度，卡尔曼滤波器的算法就是以不断减少估计值对应的协方差，从而让估计值渐渐偏向于真实值。

所以，我们以 $\tilde{P_t}$ 建立一个目标函数，目的就是求它最小值时，K 的取值。

我们可以求 K 相对于 $\tilde{P}$ 的偏导数,当结果为 0 时，表示 K 是最佳的取值。

可以得到：

$-2(\hat{P_{t}}H^T)+2K(H\hat{P_{t}}H^T+R_{t}) = 0$

可以得到：

$K = \hat{P_{t}}H^T(H\hat{P_{t}}H^T+R_{t})^{-1}$

K 求出来后，把它的值套入前面求出来的公式当中， $\tilde{x_{t}}$ 和 $\tilde{P_{t}}$ 也就出来了。

$\tilde{P_{t}} = \hat{P_{t}}-K*H*\hat{P_{t}}$ ........... (公式6)

5. 卡尔曼滤波器算法预测鼠标指针坐标

这个是比较经典的一个卡尔曼滤波的示例。创建一个黑色背景的窗口，鼠标移动时实时显示它的位置，这样就形成了轨迹。

另外还将经过卡尔曼滤波器算法估计的位置也显示出来。

源代码是 C++ 编写的，软件环境为 ubuntu16.04、OpenCV3.3.0、CMake。

我假设读者都有 OpenCV 和 C++ 的基础。

#include <opencv2/opencv.hpp>
#include <iostream>

cv::KalmanFilter kalmanfilter(2,2);
cv::Mat last_measurement(2,1,CV_32FC1);
cv::Mat current_measurement(2,1,CV_32FC1);

cv::Mat last_prediction(2,1,CV_32FC1);
cv::Mat current_prediction(2,1,CV_32FC1);

cv::Mat frame(800,800,CV_8UC3);

void onMouseMove(int event,int x,int y,int flag,void* data)
{

    last_prediction = current_prediction;
    last_measurement = current_measurement;

    current_measurement.at<float>(0) = x;
    current_measurement.at<float>(1) = y;

    std::cout << current_measurement << std::endl;

    kalmanfilter.correct(current_measurement);

    current_prediction = kalmanfilter.predict();
    //kalmanfilter.predict();

    cv::line(frame,cv::Point(last_measurement.at<float>(0),last_measurement.at<float>(1)),cv::Point(current_measurement.at<float>(0),current_measurement.at<float>(1)),cv::Scalar(0,255,0),2);
    cv::line(frame,cv::Point(last_prediction.at<float>(0),last_prediction.at<float>(1)),cv::Point(current_prediction.at<float>(0),current_prediction.at<float>(1)),cv::Scalar(0,0,255),2);

    std::cout << "on mouse move:" << current_prediction <<std::endl;

}

int main(int argc,char** argv)
{
    //cv::Mat frame(800,800,CV_8UC3);

    cv::namedWindow("test");
    cv::setMouseCallback("test",onMouseMove,NULL);

    cv::Mat F(2,2,CV_32F,cv::Scalar(0));
    //m.at<float>(0,0) = 1;
    //m.at<float>(1,1) = 1;

    cv::setIdentity(F,cv::Scalar(1));

    cv::Mat H(2,2,CV_32F);
    cv::setIdentity(H,cv::Scalar(1));

    cv::Mat p(2,2,CV_32F);
    cv::setIdentity(p,cv::Scalar(0.1));

    kalmanfilter.measurementMatrix = H;
    kalmanfilter.transitionMatrix = F;
    kalmanfilter.processNoiseCov = p;

    while (true)
    {
        cv::imshow("test",frame);

        if (cv::waitKey(30) == 113)
            break;
    }

    cv::destroyAllWindows();

    return 0;
}

源码采用 CMake 编译。

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(test)

find_package(OpenCV)

include_directories(${OpenCV_INCLUDE_DIRS})
add_executable(test main.cpp)
target_link_libraries(test ${OpenCV_LIBS})

在当前目录创建一个 build 目录。然后编译执行。

mkdir build

cd build

cmake ..

make

./test

最终结果，如下图：

kalman_mouse

红色的轨迹是预测的。

绿色的轨迹是测量得到的。

为了示例的简单化，我将测量矩阵和过程矩阵都设置为了单位矩阵，过程噪声也设置得比较少，测量噪声就干脆没有设置。大家可以自行调整参数。

大家注意看动画中鼠标急剧转弯时，绿色的轨迹马上能够反映出来，而红色的轨迹则比较圆润，这也体现了卡尔曼滤波的平滑效果。

6. 卡尔曼滤波器算法在自动驾驶当中的应用示例

前面说过，卡尔曼滤波器的算法在很多行业都有广泛的应用，只要你熟练掌握了它，那么它就能够为你所用。

现在，很多高配的车上都配备了 ADAS 功能，能够有效地检测道路线和其他车辆，从而潜在危险发生时，车辆会预警。

那么，检测车辆就成为一项必须完成的硬指标。

能够识别车辆的算法有很多，有传统的机器视觉算法，也有神经网络。

目前，威力最大的还是 SSD 和 YOLO v3，尤其是 YOLO v3，检测速度非常快。

但是，有一个问题是，在汽车上软件都要运行在嵌入式平台，硬件资源有限，所以车辆检测耗时要比 PC 上要久。

这涉及到一个实时性的问题，安装在车辆上的摄像头，每秒要处理 24 帧才算实时，也就是要在 50 毫秒内完成所有的图像处理代码，很可惜的是这很难办到。

所以，需要卡尔曼滤波器这样的算法来估计。

假设算法检测到一辆车需要 200ms，卡尔曼滤波器预估算法只要 3ms，那么我们可以每隔 4 帧用算法检测一次车辆位置，而其它时间我们用卡尔曼滤波器算法估计车辆位置。

算法检测出来的车辆可以当做是测量数据，其余的就是卡尔曼滤波器的预估，最终平均下来每一帧的处理时间不超过 50ms，也就达到了实时性的要求。

在这里插入图片描述

这个是 YOLO 算法检测出来的，不一定每一帧都可以检测的到，我们可以看到 bbox 在闪烁，抖动比较大。

在这里插入图片描述

这个是 YOLO 结合卡尔曼滤波器做出来的效果，可以看到白色的框抖动比较小，起到了平滑作用。

7. 总结

总体上来讲，卡尔曼滤波器要理解起来并不难。

但是要灵活运用去解决新的问题，而不仅仅停留在别人的示例 Demo 中的话，需要自己下一番功夫，要好好理解它。

现在，将容易混淆的地方再罗列一下。

预测值（predict），这是先验概率，是靠系统的转移矩阵得到的值，误差比较大。
测量值（measure），误差较小，但也存在噪声。
估计值（estimate），经测量值修正的的一个值，也就是后验概率。

观察各自的协方差，可以发现每次预测的时候，过去的误差会累计到当前，按照这个趋势，预测值会离真实值越来越远。

在这里插入图片描述

而测量值的介入，经过卡尔曼增益的比例调和，新的估计值会往真实值回归，协方差公式也说明了，它是递减的。

在这里插入图片描述

上图中，圆圈代表测量阶段的测量值，它的介入会产生一个估计值，也就是蓝色的向量，这让原本的轨迹向理论上的真实值靠拢，经过不停的迭代，整个系统预测的轨迹也会越来越准，这就是卡尔曼滤波器的美妙之处。

卡尔曼公式求证的手段和角度有很多，但本质上它和最小二乘法相差不大，不同之处是它是迭代递归的。

如果，有同学对向量的协方差不熟悉，可以推荐《程序员的数学 2 概率统计》这本书，如果对于矩阵求导不熟悉，网络上有大量的资料可以查阅。

最后，本文介绍的是最基本的卡尔曼滤波，它适合线性的系统，但现实生活中，很多系统是非线性的，解决这类问题可以用 EKF 和 UKF 两种，有兴趣的同学可以自行深入了解。

本文首发于 GitChat，https://gitchat.csdn.net/activity/5d37aeb1b5590f6f299a9cdc