综述：
Localizaiton的主要功能是根据车身姿态信息，Lidar数据，点云高精度地图，组合导航信息，在点云高精度地图覆盖的区域内（时速<30km/h）完成高精度的位姿输出。
在场景适应能力上，着重解决整体区域有部分RTK覆盖，但存在密集高楼GPS信号不好区域（半封闭或者全封闭场景）
需求与约束：
隐含约束及需求：
车辆上电后，定位初始化过程，暂定通过GPS来进行；如果长期没有GPS信号，初始化失败的情况下，可以提供人工初始化，实现给定相应的初始化站点和位姿信息方式，即车辆事先开到我们给定位姿区域，完成初始化工作。
出现定位异常时，定位模块若不能及时恢复，车辆则自动停下来。
总体结构和功能：
输入：主要包括车身姿态信息，lidar数据，点云高精地图，组合导航信息（视觉未加入）
输出：位姿以及位姿质量信息
内部子系统主要包含2部分：运动估计，定位信息融合
运动估计：输入车身，惯性器件的运动信息，上一个状态的位姿信息，对车辆运动进行建模，得到车辆运动状态（坐标变化，加速度变化）
定位信息融合：输入lidar数据，点云高精度地图，组合导航信息，运动估计的结果，得到输出位姿以及位姿质量信息
输入输出信号：
| 序号 | 传感器/模块 | 输入信号 | 输出信号 |
| 1 | 车身 | Chassis（主要使用轮速信号） | |
| 2 | 惯导 | Localization_ins | |
| 3 | 高精度地图 | PointCloud2 | |
| 4 | 4*补盲激光雷达 | PointCloud2 | |
| 5 | 运动估计 | Localization_ins，Chassis | Odometry |
| 6 | 定位信息融合 | Localization_ins, Odometry | LocalizationMsg |
| 7 | 融合定位结果 | LocalizationEstimate | |
| 8 | 融合定位模块状态 | LocalizationStatus |
运动估计
输入：车身姿态信息（Chassis），组合导航信息（LocalizaitonMsg）
输出：递推位姿信息
内部子系统主要包含俩块：1）通过轮速里程和惯导进行递推，2）记录历史多帧，算出运动的速度和角速度来基于运动模型递推
处理运动畸变校正及位姿优化
关于激光雷达点云处理中运动畸变校正与位姿优化的数学推导，这是 SLAM（同步定位与地图构建）算法中的核心环节
- 姿态表示：用平移向量 \(\tau\) 和旋转向量 \(\theta\)（罗德里格斯变换为矩阵 R）描述 6 自由度运动。
- 时间插值：通过线性插值计算每个点云时刻的姿态 \(T_{(k,i)}^L\)，消除运动畸变。
- 点云去畸变：将点云从采集时刻投影到扫描起始时刻，得到校正后的点云 \(\tilde{\epsilon}_k, \tilde{H}_k\)。
- 非线性优化：基于边缘点和平面点的几何距离误差，用 Levenberg-Marquardt 算法迭代优化姿态 \(T_K^L(t)\)
⑴ 标签匹配：由于角特征点和平面特征点中的每一个特征在分割后都用其标签进行编码，所以我们只需找到上一帧这两种特征点种具有相同标签的对应点。对于Fpt中的平面特征，只使用Ftp - 1中标记为地面点的点来寻找平面patch作为对应。对于Fet中的边缘特征，其对应的边缘线可以在分割后的簇的Ft e - 1中找到。通过这种方法找到匹配点，可以提高匹配精度。换句话说，相同对象的匹配对应更有可能是在两次扫描中发现。这个过程也缩小了通信的潜在候选范围。
⑵ 两步L-M优化:（阻尼高斯牛顿优化方法），将当前扫描到的边缘与平面特征点之间的距离及其与前一次扫描的对应关系的一系列非线性表达式编译成一个综合距离向量。采用Levenberg-Marquardt (L-M)方法求出两个连续扫描之间的最小距离变换。
⑶ GPS回环检测，通过每10秒提取一个GPS准确信息加入到位姿图中，相当于每隔10秒我们就获得一个准确的位置，把位姿的传播误差平均分配到这段时间内的每一个关键帧上面，做一个位姿的优化。
定位融合信息
输入：运动估计结果，组合导航信息，点云高精度地图，点云信息
输出：位姿信息以及位姿质量信息
内部子系统主要包含俩部分：融合结果估计和融合状态管理
融合结果估计包括：激光点云拼接，NDT匹配，点云地图加载，初始化，
融合状态管理：置信度涉及，错误发现恢复，定位错误侦测
GPS优先模式下，gps_level≤ndt_level，则使用GPS定位，否则使用NDT定位；
NDT优先模式下，ndt_level≤gps_level，则使用NDT定位，否则使用GPS定位。
当gps_level和ndt_level均为3时，发送Error故障码，车辆减速停车。车辆正常运行时，实时校核GPS位置和NDT位置，当两者偏差较大时，限速行驶
Kalman滤波器
用反馈控制方法估计过程状态：滤波器估计过程某一时刻的状态，然后以（含噪声的）测量变量的方式获得反馈。
时间更新方程负责及时向前推算当前状态变量和误差协方差估计的值，以便为下一个时间状态构造先验估计。
测量更新方程负责反馈——也就是说，它将先验估计和新的测量变量结合以构造改进的后验估计。时间更新方程也可视为预估方程，测量更新方程可视为校正方程。最后的估计算法成为一种具有数值解的预估－校正算法
NDT
先根据参考数据（reference scan）来构建多维变量的正态分布，如果变换参数能使得俩副激光数据匹配的很好，那么变换点在参考系中的概率密度会很大，需要用非线性优化方法使得概率密度之和最大的变换参数，此时俩副激光点云数据将匹配的最好。
LOAM建图
具体包括：点云分割与特征提取、点云Odometry、点云Mapping、局部地图生成、因子图优化
- 第一部分：Segmentation： 这一部分的主要操作是分离出地面点云；同时对剩下的点云进行聚类，滤除数量较少的点云簇。
- 第二部分：Feature Extraction： 对分割后的点云（已经分离出地面点云）进行边缘点和面点特征提取，这一步和LOAM里面的操作一样。
- 第三部分：Lidar 里程计： 在连续帧之间进行（边缘点和面点）特征匹配找到连续帧之间的位姿变换矩阵。
- 第四部分：Lidar Mapping： 对feature进一步处理，然后在全局的 point cloud map 中进行配准。
- 第五部分：Transform Integration： Transform Integration 融合了来自 Lidar Odometry 和 Lidar Mapping 的 pose estimation 进行输出最终的 pose estimate。
一文详解激光SLAM框架LeGO-LOAM - 知乎
注意GPS和NDT之间切换和约束，低速，偏差，以及可信度平衡
