loam算法_LINS算法分析

概述

LINS采用了非常特别的迭代误差状态卡尔曼滤波（IESKF）技术，对位姿误差状态进行估计和更新。误差状态卡尔曼滤波（ESKF）已经足够复杂，LINS还用上了迭代卡尔曼滤波（IEKF），两者合起来就是IESKF，恐怕是目前复杂度最高的LOAM算法之一了。关于ESKF参见论文：

Quaternion kinematics for the error-state Kalman filte​arxiv.org

ESKF论文详细介绍了四元数、李群李代数和ESKF的相关概念和推导过程，完全可以当做相关方面的教材来仔细研读和学习，比某高数教材强千百倍。

LINS的位姿误差状态估计部分与ESKF论文几乎完全一致，主要是利用IMU的数据实现位姿估计，这里不再赘述。LINS最特别的地方在位姿状态更新部分，下面重点介绍。

在ISKF中，状态更新可以看成是一个优化问题，即对位姿状态先验

的偏差，以及基于观测模型引入的残差函数

的优化问题。

表示第

帧数据到第

帧数据的位姿变换。其中包含了位移、速度、旋转、加速度偏差和角速度偏差。

观测方程

在给定从第

帧数据到第

帧数据的位姿变换

的前提下，基于观测模型引入的观测方程

定义如下：