专栏文章:

深蓝学院-多传感器融合定位课程-第1章-概述_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第2章-3D激光里程计I_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第3章-3D激光里程计II_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第4章-点云地图构建及基于地图的定位_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第5章-惯性导航原理及误差分析_goldqiu的博客-CSDN博客

深蓝学院-多传感器融合定位课程-第6章-惯性导航解算及误差模型_goldqiu的博客-CSDN博客

github保存了相关代码和学习笔记：

Shenlan-Course-Multi-Sensor-Fusion-for-Localization-Mapping/第7章：基于滤波的融合方法I at main · goldqiu/Shenlan-Course-Multi-Sensor-Fusion-for-Localization-Mapping

第7章：基于滤波的融合方法Ⅰ

课程内容

代码、PPT、视频见文件夹

课程笔记

滤波器作用

滤波器的本质：结合预测与观测，得到最“精确”的后验值。 实际中，预测与观测均从传感器而来，因此滤波器的作用便是结合各传感器得到一个最好的融合结果。

1) 实际中预测往往从IMU、编码器等传感器递推而来； 2) 观测往往从GPS、雷达、相机等传感器而来； 3) 后验为融合后的结果，即定位模块的输出。

概率基础知识

概率、概率密度

高斯概率密度函数

联合高斯概率密度函数

四元数微分方程

高斯随机变量的线性分布

滤波器基本原理

状态估计模型

贝叶斯滤波

经过以上化简，最终后验概率可以写为

根据以上结果，可以画出贝叶斯滤波的信息流图如下

1) 在高斯假设前提下，用贝叶斯滤波的原始形式推导比较复杂，可以利用高斯的特征得到简化形式，即广义高斯滤 波。后面KF、EKF、IEKF的推导均采用这种形式。 2) 实际中，UKF 和 PF 多应用再扫地机器人等2D小场景，与本课程目标场景不符，因此不做讲解。

卡尔曼滤波(KF)推导

扩展卡尔曼滤波(EKF)推导

迭代扩展卡尔曼滤波(IEKF)推导

迭代的方法是重新做线性化；

终止的条件是:两次的后验状态均值之间的差异小于一个阈值。

基于滤波器的融合

通过以上推导，滤波问题可以简单理解为“预测 + 观测 = 融合结果”。 结合实际点云地图中定位的例子，预测由IMU给出，观测即为激光雷达点云和地图匹配得到的姿态和位置。 融合流程用框图可以表示如下：

状态方程

观测方程

构建滤波器

Kalman 滤波实际使用流程

作业

在提供的工程框架中，补全代码，实现基于地图的融合定位，并与不加滤波时的定位结果做比较。 (注：由于kitti数据有部分问题，虽然课程提供了修复后的数据，但是仍会对结果有影响，因此在某些路段滤波效果会较差，可以忽略这部分路段。)

评价标准： 1)及格：补全代码，且滤波功能正常。 2)良好：补全代码，功能正常，且经过调试参数，滤波后性能比滤波前好。(这个没有定量标准，各位把详细的误差对比结果提供在作业中，供助教有足够依据评阅) 3) 优秀： 在前面的模型推导中 ， 考虑了器件误差中的随机游走 ， 请给出不考虑随机游走模型时的推导过程，并在工程框架中实现。对比这两种方法的性能差异(最好给出原因分析)。 另外，kalman滤波的性能对噪声的设置较为敏感，请在提供结果的同时，给出不同噪声设置情况下的结果对比(至少5组参数)。

参考资料：Quaternion kinematics for the error-state Kalman filter

环境安装

需要安装新版本的proto 3.15x，按照第四章的方式生成对应的文件。

出现Protobuf版本不匹配的错误：

protoc --version 查看Protobuf版本是否对应

不对应则更改环境：

sudo gedit ~/.bashrc

export PATH=/usr/local/bin/:$PATH

source ~/.bashrc

参考博客

多传感器融合定位理论基础（八）：基于滤波的融合方法

以误差作为状态量的滤波方法ESKF(Error state kalman filter)较为常用，“EKF尝试通过线性化来近似名义状态的变化，而EsKF则尝试通过线性化来近似误差状态的变化”。而使用位姿做更新的EKF方法也成为(名义方程)。在完成一次ESKF迭代后，有点需要主要的是：当有新的融合数据进来，要进行一次新的观测时，因为之前的状态量已经用来补偿了，因此需要清零，所以此时的 先验状态量 delta_x_k_hat = 0 。 变化的更新体现在观测上 delta_y = (预测值 - 观测值) 。