五. 激光雷达建图和定位方案-算法工具

前面内容：

一. 器件选型心得（系统设计）--1_goldqiu的博客-CSDN博客

一. 器件选型心得（系统设计）--2_goldqiu的博客-CSDN博客

二. 多传感器时间同步方案（时序闭环）--1

三. 多传感器标定方案（空间同步）--1_goldqiu的博客-CSDN博客

三. 多传感器标定方案（空间同步）--2

三. 多传感器标定方案（空间同步）--3

四. 点云着色（真彩点云）方案

五. 激光雷达建图和定位方案-引言-CSDN博客

五. 激光雷达建图和定位方案-基础概念-CSDN博客

五. 激光雷达建图和定位方案-算法工具

滤波器理论回顾

在此之前我先讲讲对多传感器融合的理解：

在运动感知和环境感知上，即定位和环境感知上都需要多传感器融合技术；

至于定位上，我们主要是要估计出机器人或车辆在环境中的位姿，那么它作为状态变量的其中一大部分，需要在多传感器融合技术参与下，更高精度和更具有鲁棒性地估计出来；多传感器中有作为状态预测也有作为状态观测的，通过设计一个状态估计模型（滤波或者优化），将预测和观测信息结合起来通过批量优化或滤波递归的方式不断更新机器人的状态，主要就是融合多传感器预测和观测信息的不确定性或协方差或置信度，对状态进行一个评估、融合，估计得到一个更加高确定性、协方差更小、置信度更高的一个状态。

SLAM通常由运动方程和观测方程组成：

非线性形式

添加图片注释，不超过 140 字（可选）

线性形式

添加图片注释，不超过 140 字（可选）

线性形式的最优估计由卡尔曼滤波器给出。

对于一个SLAM问题来说，有运动方程和观测方程，运动方程为k时刻的运动状态，由上一时刻的运动状态以及状态转移矩阵，加上输入和噪声组成；观测方程为k时刻的观测，由k时刻的观测矩阵以及k时刻的运动状态，加上观测噪声组成。

状态由高斯分布描述：

预测：

添加图片注释，不超过 140 字（可选）

更新：

添加图片注释，不超过 140 字（可选）

卡尔曼滤波器的五个公式:

1. 预测的状态= 状态转移矩阵乘上一时刻的状态（最优）+输入

2. 预测的状态协方差 = 状态转移矩阵乘上一时刻的状态协方差（最优）+输入噪声 （因为有噪声的存在，所以在IMU预测过程中，状态协方差是不断增大的，即预测的状态信息是越来越不可信的）

P为状态协方差矩阵，R为输入噪声的协方差矩阵

3. 更新是更新卡尔曼增益K，K中包含观测矩阵和观测噪声协方差矩阵，以及预测的状态协方差矩阵

卡尔曼增益K，K中包含观测矩阵和观测噪声协方差矩阵，以及预测的下一时刻的状态协方差矩阵；即预测和观测的加权组合。

4. 下一时刻的状态包含观测和预测的加权组合。

下一时刻的状态X，包括卡尔曼增益、观测、观测矩阵、预测的下一时刻的状态矩阵，是观测和预测的加权组合（观测减去预测乘卡尔曼增益）。

5. 协方差矩阵P(也是下一时刻的),包括预测的协方差矩阵P，卡尔曼增益，观测矩阵（也是观测和预测的加权组合）

扩展卡尔曼滤波：

对非线性的运动方程或观测方程进行线性化，一阶泰勒展开。

迭代扩展卡尔曼滤波：

当非线性化程度比较强的时候，误差就会比较大；但是由于线性化的工作点离真值越近，线性化的误差就越小，就可以通过迭代来找到准确的线性化工作点，从而提高精度。

在这个情况下，后验协方差矩阵应该放到最后一步更新。

迭代的方法是重新做线性化；终止的条件是:两次的后验状态均值之间的差异小于一个阈值。

最优化和图优化

最简单的方法是梯度下降，即找一个初始值，沿着负梯度（非线性函数对求解参数进行求导）的方向逐步迭代，当迭代步长足够小时，即完成求解。

我们定义成数学问题，对于：

添加图片注释，不超过 140 字（可选）

我们的思路：

添加图片注释，不超过 140 字（可选）

ps：刚开始的时候，x1=x0 + ∆x0，k步后，xk + ∆xk

非线性优化问题的关键是如何确定∆x。

方法如下：

添加图片注释，不超过 140 字（可选）

方法1（最速下降法）：步长