fast-lio 重点--滤波方法

迭代扩展卡尔曼滤波的紧耦合激光雷达和IMU

计算卡尔曼增益的新公式
新公式的计算负荷取决于状态维度而不是测量维度。

系统描述

1、
设M为n维的一种流形(比如说旋转群SO(3))。就是一种符号？？？？
“在微积分和几何学中，切空间（Tangent space）是描述流形（Manifold）中点局部的线性空间。 粗略地说，切空间可以被认为是流形在特定点处的切平面。 切空间中的向量可以被视为在该点处“接触”流形的矢量。考虑一个平面曲线，例如圆。在任何给定点，曲线上的切向量都是切线。 每个切向量都是在该点处的曲线的一个“接触点”，并且可以用一个向量来表示。 通常，流形上的点被描述为一组坐标，而切空间中的向量则由其对应的导数或方向导数表示。”
我的理解就是旋转群中某一个具体旋转其所在的空间叫做切空间
流的一个重要属性是对于Rn它是局部同胚（拓扑学中的概念–描述映射关系）的
也就是说在流和Rn之间可以建立一个双射映射–在不知道流形的内部结构的情况下，修改和比较流形状态
**
盒式加：李群与李代数的指数映射（罗德里格斯变换）运算
盒式减：李群运算后经过对数逆变换回李代数
**
1、从切空间映射到流


采用流形状态和在Rn中映射的局部邻域中表示的小变化，并将该变化应用于状态以产生新的修改状态。

当M是旋转群时:


罗德里格斯公式表示旋转R
当M是n为实数空间

2、从流映射到切空间


确定两个状态之间的映射差异。
当M为旋转群

当M为实数空间

上面就是介绍一下盒式加减，也就是说切空间的变化会映射到流形上，使用盒式加，流形与流形的变化会映射到实数空间，使用盒式减。

2、连续模型
激光雷达与IMU(并作为载体系)的外参为


李代数（旋转向量）来表示姿态，李群（旋转矩阵）表示姿态
是IMU测量量的白噪声
是由高斯噪声导致的IMU偏差。
3、离散模型
采样间隔：


整个流形包含了三个量：状态量x，控制量u，误差量n
状态量包含：旋转、平移、速度、偏差、重力
控制量：角速度、加速度
误差量：IMU测量的白噪声，获得测量随机游走的白噪声
<随机游走就是白噪声的积分>
角速度和加速度白噪声的积分被称为角度随机游走和速度随机游走
角速度随机游走和加速度随机游走是角加速度等白噪声积分的结果

比如说:t+1时刻的R=t时刻R(
*(
))
这里就是旋转群与旋转矢量的盒式加
在实际处理中,我们会把整个旋转矢量根据罗德里格斯公式(如下)转换为旋转矩阵(k可以看作
)整个过程就用盒式+表示

4 激光测量量的预处理

fast-lio通过一段时间的点云累积,然后一次性处理这些点
系统最小间隔20ms(50hz)–一次扫描
如系统框图
100khz-500khz以20ms采集为一帧（扫描）的话(每帧 2000个点到10000个点)，采集所用时间为tk
从原始点云中提取平面点和边点。其数量是m；都是在时间段 tk-1——tk内采集的
，写作左上标代表在时间处的激光雷达帧，在一帧扫描的时间段里，有很多次IMU测量，如下图。
，时间对应着状态。
需要注意的是，扫描的末尾是最后一个雷达特征点，
IMU并没有必要配准到扫描的开始和结束。

状态估计

使用迭代扩展卡尔曼滤波器
描述估计协方差在状态估计的切空间
如果是上一帧最后优化的状态是（作为tk-1到tk的先验状态和协方差）其协方差为
也就是随机误差状态向量（如下图）的协方差。
如下符号顶的波浪线代表着误差量

第一项是两个旋转（观测旋转，优化旋转）之间的差异如下。

但是这里需要它的旋转矢量形式，所以应用了一次对数映射将李群变为李代数如下：

为什么要变为旋转矢量的形式？因为这样就可以用误差的协方差来表示姿态的不确定性了。

前向传播

接收的一个imu数据就执行，前向传播量是（状态预测量），从前向传播公式看，，前向传播量的第一个数是上一个时间最后一个优化量，也就是下一帧IMU状态传播的开始。但是为什么要将噪声设置为0呢-参考EKF中预测均值的形式。

使用误差状态动态模型，如下去传播协方差。实际状态测量值与状态估计值的误差

这里实现了误差函数的线性化
上式
的协方差为：

按照上式一直传播，一直到时间tk(这一帧扫描的结束)，得到，注意后者不是前者的协方差，它是真值与前者误差的协方差（预测协方差）。

后向传播与运动补偿

到达时间tk后，新的特征点扫描应该与传播的状态及协方差融合去生成优化的状态更新
但是吧，最后一次采样的特征点时间并不是严格的等于tk，那怎么求tk时刻点的状态呢？
那时间pj和tk之间的相对运动应该怎么补偿？
那就是在xk预测值的基础上开始往回传播，一直找到时间j
反向传播函数如下

反向传播以特征点的频率执行，为什么特征点的频率远高于IMU的频率
对于所有处于两个IMU测量之间的点，将左侧的IMU测量量作为反向传播的输入–？应该是从右侧开始吧

是时刻tk开始往左传播一直到最后采样特征点所处的时间
最终，能够得到一个从时刻j到时刻k的变换矩阵
这样就可以求得j时刻激光雷达系j的点pj在k时刻，激光雷达系k下的位置
完成了运动补偿
（这里我有一个疑问：前文说过**请注意，最后一个激光雷达特征点是扫描的终点，也就是
**，我就认为在tk时刻有一个特征点，但是这里又与之前的不一致。所以应该是说理论上或者理想情况下pm=tk）

残差计算（测量函数）

在时刻k，假设当前迭代卡尔曼滤波为的迭代次数为k，对应的状态估计为，
将点转到全局坐标系下

假设啊，在地图中，它的领域特征点定义的离它最近的平面或边是它真正应该在的地方，那么残差就可以定义为这个点到最近平面或者边的距离，如下，u是平面法向量或者边方向向量。

有些不理解：平面时，边时，为什么？
师兄给我的解答是，所谓点到直线的残差，是以直线方向向量外积相应向量来描述的，当点在这个直线上时，外积后的范数值为零，当范数值小于0.5m时才作为约束。
至于点到直线的距离为何用方向向量反对称的形式（不考虑传统的点到直线公式距离），我是这样理解的
角度1：我们知道两个向量的叉乘能得到一个法向量垂直于这两个向量所在的平面，在学习罗德里格斯变换的时候，我知道了另一个看待叉乘的角度，那就是：针对b与a的叉乘（bxa），我们先建立一个平面（a,b所在的），在这个平面上做一条垂直于b的直线，a在这个直线上的投影为c，a在b上的投影为d，则bxa=bx(c+d)=bxc+bxd=bxc，也就是c绕b旋转了90°，这个c的模长就是a到b上垂线的长度。
如下图

角度2、已知在二维平面上，两个向量叉乘的范数是这两个向量构成的平行四边形的面积
如果方向向量为单位向量的话，||ux(p-q)||=S=||u||*h=h，因此可以直接用叉乘的范数表示点到直线的距离。

迭代的状态更新

将这个状态量融合到状态预测量
和协方差
需要线性化这个测量模型-这个模型将残差与真值状态
和测量噪声
关联起来。如下：

残差模型就可以改写为，将Lj系下的真值点转到全局系下，并与最近的平面和边求距离。（测量函数的一阶泰勒展开）

对这个非线性函数进行一阶泰勒展开

对应到扩展卡尔曼滤波这是测量函数

已经获得k时刻的测量函数，接下来计算k时刻的状态转移函数
（15）
k时刻第k次迭代的状态与第k次迭代得到的误差盒式加得到k时刻的真值状态，真值状态减去迭代开始时的估计状态
将测量误差函数与状态误差函数结合进行最小二乘–，它通过最小化误差的平方和寻找数据的最佳函数匹配。
接下来可以求迭代扩展的卡尔曼增益了
（17）

将（15）中的先验线性化代入（17），并优化所得的二次成本，得到标准迭代卡尔曼滤波器

？怎么是这个形式
这应该是迭代扩展卡尔曼滤波的特点—将k时刻第k次迭代的预测误差协方差作为卡尔曼增益中的预测协方差

P是状态函数第k次迭代的误差的协方差，J是状态函数第k次迭代误差对应的雅各比矩阵
R是测量函数误差的协方差，H是测量函数误差对应的雅各比矩阵
K = PH转置(HPH转置 + R)逆迭代卡尔曼的增益就是
因此第k+1次迭代的估计状态为（状态更新）
（这个更新形式也是迭代扩展卡尔曼滤波的特色–需要我去好好学习一下）

需要阅读迭代扩展KF了解状态迭代公式
在另一篇博客《迭代扩展卡尔曼滤波学习》中完成了迭代预测协方差，卡尔曼增益，更新公式的推导。不得不说，作者的文章让人获益匪浅

最终的状态和协方差

卡尔曼增益可以写为：新的形式，利用了矩阵求逆引理

地图更新

根据优化后的变换矩阵，将点变换到全局系下。