在一个 IMU 系统里，我们考虑它的五个变量：旋转 R、平移 p、角速度 ω、线速度 v 与加速度 a。根据第 2 章介绍的运动学，这些变量的运动学关系可以写成如下运动学方程：在到时间内对运动学方程进行欧拉积分得：IMU 测量方程（其中为 IMU 测量的高斯噪声 ）如下：IMU 测量方程带入积分后的运动学方程（其中为离散化后的 IMU 测量噪声 ）如下：其中噪声项满足：以上过程与我们在 IMU 测

本章，我们来关注实时的激光雷达定位系统。在点云地图基础之上，我们可以把当前激光扫描数据与地图进行匹配，从而获得车辆自身的位置，再与 IMU 等传感器进行滤波器融合。然而，。因此，点云定位在实际使用时，会遇到一些特有逻辑问题。本章将使用第 9 章构建的点云地图，展示点云定位的使用方法，并演示一个基于 ESKF 的实时定位方案。

完整的点云建图可以看成是一个的综合优化问题。大部分 L4 级别的自动驾驶任务都需要一张来进行地图标注、高精定位等任务。

处理同步之后的点云和 IMU。初始化完毕后，先使用 IMU 数据进行预测，再用预测数据对点云去畸变，最后对去畸变的点云做配准。前文已经得到了去畸变的点云，这里只需将其传递给增量 NDT 里程计，并使用滤波器预测得到的先验位姿作为增量 NDT 里程计的初始位姿，经过迭代计算后得到优化后的位姿后再返回给滤波器，滤波器进行观测过程。），其存储上一 IMU 时刻 ESKF 的名义转态变量和当前 Measu

上式是始终成立的，不论是相对于当前坐标系的连续旋转的叠加还是相对于固定坐标系的连续旋转的叠加。实际上上式扩展到多个连续坐标系的变换依然成立。方便，这里只考虑两次旋转的叠加（或者说三个坐标系之间的关系）。假设有三个坐标系{0}，{1}，{2}，坐标系{1}相对于{0}的姿态为。直接上例题，坐标系1和坐标系2的关系如上，求得坐标系1中的点（1，1，1）在坐标系2下的坐标。首先明确，左乘和右乘是关于多个