目录1. 系统描述2.普通ESKF流程2.1 预测量侧更新阶段2.2 观测更新阶段2.3 ESKF的局限3. IESKF流程3.1 预测量侧更新阶段3.1.1 雅可比推导前的准备3.1.2 i1时刻误差状态对i时刻误差状态的雅可比计算3.1.3 i1时刻误差状态对i时刻过程噪音的雅可比计算3.2 观测更新阶段3.2.1 先验分布3.2.1.1 雅可比的具体形式3.2.2 似然分布3.2.2.1 点-面残差定义3.2.2.2 雅可比计算3.2.3 最大后验估计3.2.4 迭代终止与协方差更新4. 新卡尔曼增益形式4.1 证明新形式与标准形式等价4.2 SMW恒等式FAST-LIO通过迭代误差状态卡尔曼滤波IESKF框架实现了IMU与Lidar数据的紧耦合。在FAST-LIO之前有一篇工作LINS也实现了IESKF只不过LINS是在LEGO-LOAM的基础上实现的假设使用多线激光雷达并且要求激光雷达水平安装在实际使用过程中会受到诸多限制。相较于LINSFAST-LIO提供了更完善的理论推导与代码实现并提出了一个新的卡尔曼增益形式降低计算消耗。1. 系统描述1符号约定1.名义状态。滤波算法过程中得到的名义估计值2.滤波算法观测更新结束后得到的最优估计值3.误差状态4.真值状态。5.代替原文中的箱形算子6. i两个Lidar帧之间第i个IMU数据7. k第k次前向传播8.第次迭代9. jLidar点云中第j个点2状态定义FAST-LIO中的状态以IMU为基准IMU系相对Global系的姿态Global系的重力。3误差状态4IMU输入5过程噪声IMU测量噪音IMU读数与IMU测量真值之间的关系测量值真值零偏噪声6IMU连续时间微分方程推导过程参考IMU惯性解算其中是 3x3 矩阵的导数属于在上的切向量。FAST-LIO 采用的流形表示对旋转分量定义广义加法其中代表将向量转换为反对称矩阵。是局部旋转增量增量以李代数切空间中的三维向量给出再经指数映射作用到上。相应地微分方程的第一行不再写而是替换为切空间中的增量速度FAST-LIO 把上面的模型写成统一的紧凑形式其中把影响状态演化的自变量分成三类待估计的状态IMU 的原始读数过程噪声。所有 IMU 相关的物理规律被封装进一个函数。7IMU离散模型中值积分连续时间模型描述状态的演化规律。对于相邻两个 IMU 采样时刻和记利用两个时刻的 IMU 读数对连续动力学进行中值积分离散化。去除零偏及测量噪声后的角速度、比力其中是IMU系的比力尚未加入重力表示离散 IMU 样本的测量噪声。区间平均角速度近似为全局坐标系中的平均加速度状态量更新简化表达2.普通ESKF流程1确定IMU名义状态递推起点第k-1个Lidar帧到第k个Lidar帧之间的n个IMU数据的递推起点为第k-1个Lidar帧到来时观测更新后状态的最优估计2.1 预测量侧更新阶段第k-1帧到第k帧Lidar数据之间的imu状态递推2名义状态积分递推3误差协方差传递其中是对的雅可比是对的雅可比为离散噪音的协方差。2.2 观测更新阶段当第k帧Lidar观测到来时进行观测更新4计算卡尔曼增益卡尔曼增益是按不确定度自动计算的、逐方向的最优加权矩阵标量情形最直观。设先验方差、观测方差取激光很不可信如退化环境、匹配残差大结果信 IMUIMU 先验很不准如长时间无观测、偏置未收敛信激光两者相当时折中。所以就是观测占多大话语权是先验占多大话语权。同时负责跨状态传播把激光的位姿残差通过状态间的相关性传递给速度、偏置、重力等未被直接观测的量。5更新误差状态6更新误差协方差其中为观测残差是观测残差对误差状态的雅可比。7将误差状态“注入”名义状态得到最优估计并将误差状态清零2.3 ESKF的局限ESKF 的更新公式来自对观测模型的一阶线性化这个近似只有在真实状态离先验状态足够近时即误差足够小时才成立。在 LIO 中1激光观测点到面 / 点到线残差关于位姿是强非线性的尤其是旋转部分2一帧扫描的先验来自 IMU 积分当快速运动、大角速度或 IMU 偏置估计不准时先验位姿误差较大3最近邻关联point-to-piont/point-to-plane 的匹配对本身依赖于当前位姿估计。位姿不准 → 关联错 → 残差和雅可比都有误差。所以在先验点处线性化一次得到的偏差可能较大一次更新无法收敛到最优解。IESKF 把一次线性化 一次更新改为反复重新线性化 反复更新本质上是用高斯—牛顿法求解最大后验MAP问题。3. IESKF流程1确定IMU名义状态递推起点第k-1个Lidar帧到第k个Lidar帧之间的n个IMU数据的递推起点为第k-1个Lidar帧观测更新后状态的最优估计3.1 预测量侧更新阶段第k-1帧到第k帧Lidar数据之间的imu状态递推前向传播2名义状态积分递推其中3误差协方差传递其中是对的雅可比是对的雅可比。3.1.1 雅可比推导前的准备令其中是关于自变量,的函数其他量均已知。令则的形式类似于其中对于非旋转部分对于旋转部分G 是向量 → 流形 → 向量的映射扰动进了内部并跨切空间所以必须采用李代数加法形式求导。1)BCH近似得其中为左雅可比为右雅可比。再使用伴随性质将所在项挪到等式最右边则2)BCH近似得则3.1.2 i1时刻误差状态对i时刻误差状态的雅可比计算复合函数求导1其中所以2)3)其中g 是流形 → 向量的映射旋转扰动以右乘群元素形式作用在向量上所以用扰动模型求导。3.1.3 i1时刻误差状态对i时刻过程噪音的雅可比计算复合函数求导1)2)3.2 观测更新阶段IESKF 把一次线性化 一次更新改为反复重新线性化 反复更新本质上是用高斯—牛顿法求解最大后验MAP问题。当第k帧Lidar观测到来时进行迭代更新3.2.1 先验分布1正向传播预测阶段给出的先验是零均值的虽然前向传播时推导了但其主要作用是用来推导协方差的更新公式的前向传播时误差潜藏在名义状态中只有观测到来时才能将误差具象化出来。所以前向传播时假设误差是遵循零均值的高斯分布的。2观测迭代更新第次迭代时其中在处做了一阶展开。的初值为。对两端取期望得再根据线性变换的协方差传播规律得到的协方差为。所以3.2.1.1 雅可比的具体形式与3.1中的推导类似对令对于非旋转部分对于旋转部分则3.2.2 似然分布观测方程其中为第j个Lidar测距点的噪音假设服从零均值高斯分布。1观测残差的物理意义对于第j个激光点将其投影到全局坐标系后到地图中对应平面的距离。理性状态该距离应该为0这里观测值为0是几何约束。2在迭代点线性化代入在处展开令是当前迭代点算出的第j个激光点的残差直接计算得到。并不等于原始激光点的测量噪声而是经过雅可比传递后的等效噪声3整理成似然。观测量的条件分布3.2.2.1 点-面残差定义FAST-LIO2中取消了特征提取对所有点统一计算点-面残差。1对原始Lidar点进行运动补偿Lidar局部坐标系中原始第j个Lidar点要将第j个Lidar点补偿到Lidar采样周期的末尾时刻k 先使用Lidar-IMU外参将Lidar点转换到IMU坐标系使用IMU坐标系的计算出的j时刻到k时刻的相对位姿对第j个Lidar点进行运动补偿再将Lidar点从IMU坐标系转换回Lidar坐标系2将Lidar点投影到全局坐标系可以简写为其中、、是已知量令则3计算点-面残差实际是以预测位姿名义状态将点a投影到全局坐标系 图中平面是由地图中距离点最近的几个点包括拟合出的点到平面的距离即为残差。点到平面的距离由向量“点乘”平面单位法向量得到。可以参考点到线/面ICP-残差及雅可比推导A-LOAM_点到面残差构建写法-CSDN博客假设平面单位法向量为则3.2.2.2 雅可比计算观测方程其中补充了Lidar点的测量噪音。12在本次观测迭代中冻结去畸变点和外参后残差直接依赖当前位姿所以则实际去畸变结果来自 IMU 轨迹估计依赖速度、偏置等状态卡尔曼增益会把激光的位姿残差通过状态间的相关性传递给速度、偏置、重力等未被直接观测的量。3.2.3 最大后验估计先验分布似然分布最大后验估计在 LIO 中共享地图、平面拟合以及去畸变误差可能引入相关性这里冻结相关量并忽略这些相关性。并假设给定误差状态后各激光点观测条件独立。由贝叶斯公式后验密度正比于先验密度与各点似然的乘积。对后验密度取负对数舍去与优化变量无关的常数项得到本轮线性化 MAP 优化问题代价函数为的二次型。令则代价函数令梯度为04计算卡尔曼增益定义卡尔曼增益5更新误差状态利用恒等式则则其中是把迭代点往先验拉的修正项。当迭代点偏离预测状态越远该项越大。第一次迭代时该项为0退化为ESKF。3.2.4 迭代终止与协方差更新重复3.2.13.2.3中的步骤每次迭代都要用新的重新对运动补偿后的点a投影到全局坐标系执行最近邻搜索计算残差与雅可比直到则认为优化收敛了输出本轮观测迭代更新的最优估计6更新误差协方差只在迭代收敛后进行一次后验协方差更新因为迭代过程使用的是同一帧数据不能把同一帧数据当作多次独立观测累计。7并将误差状态清零误差状态已经在3.2.3中“注入”名义状态得到最优估计4. 新卡尔曼增益形式标准形式新形式其中P是n x n维H是m x n维R是m x m维K是n x m维。n为状态维度FAST-LIO中18维FAST-LIO2中24维m为观测量维度一帧激光点云存在成千上万个激光测距点m维度远大于n。标准形式中存在需要对m x m维的矩阵求逆而新形式中只需要对n x n维矩阵求逆计算量大幅下降。4.1 证明新形式与标准形式等价新形式根据SMW公式代入新形式得令则代入则4.2 SMW恒等式验证上式成立直接对上式右乘 FAST-LIO IESKF推导
目录1. 系统描述2.普通ESKF流程2.1 预测量侧更新阶段2.2 观测更新阶段2.3 ESKF的局限3. IESKF流程3.1 预测量侧更新阶段3.1.1 雅可比推导前的准备3.1.2 i1时刻误差状态对i时刻误差状态的雅可比计算3.1.3 i1时刻误差状态对i时刻过程噪音的雅可比计算3.2 观测更新阶段3.2.1 先验分布3.2.1.1 雅可比的具体形式3.2.2 似然分布3.2.2.1 点-面残差定义3.2.2.2 雅可比计算3.2.3 最大后验估计3.2.4 迭代终止与协方差更新4. 新卡尔曼增益形式4.1 证明新形式与标准形式等价4.2 SMW恒等式FAST-LIO通过迭代误差状态卡尔曼滤波IESKF框架实现了IMU与Lidar数据的紧耦合。在FAST-LIO之前有一篇工作LINS也实现了IESKF只不过LINS是在LEGO-LOAM的基础上实现的假设使用多线激光雷达并且要求激光雷达水平安装在实际使用过程中会受到诸多限制。相较于LINSFAST-LIO提供了更完善的理论推导与代码实现并提出了一个新的卡尔曼增益形式降低计算消耗。1. 系统描述1符号约定1.名义状态。滤波算法过程中得到的名义估计值2.滤波算法观测更新结束后得到的最优估计值3.误差状态4.真值状态。5.代替原文中的箱形算子6. i两个Lidar帧之间第i个IMU数据7. k第k次前向传播8.第次迭代9. jLidar点云中第j个点2状态定义FAST-LIO中的状态以IMU为基准IMU系相对Global系的姿态Global系的重力。3误差状态4IMU输入5过程噪声IMU测量噪音IMU读数与IMU测量真值之间的关系测量值真值零偏噪声6IMU连续时间微分方程推导过程参考IMU惯性解算其中是 3x3 矩阵的导数属于在上的切向量。FAST-LIO 采用的流形表示对旋转分量定义广义加法其中代表将向量转换为反对称矩阵。是局部旋转增量增量以李代数切空间中的三维向量给出再经指数映射作用到上。相应地微分方程的第一行不再写而是替换为切空间中的增量速度FAST-LIO 把上面的模型写成统一的紧凑形式其中把影响状态演化的自变量分成三类待估计的状态IMU 的原始读数过程噪声。所有 IMU 相关的物理规律被封装进一个函数。7IMU离散模型中值积分连续时间模型描述状态的演化规律。对于相邻两个 IMU 采样时刻和记利用两个时刻的 IMU 读数对连续动力学进行中值积分离散化。去除零偏及测量噪声后的角速度、比力其中是IMU系的比力尚未加入重力表示离散 IMU 样本的测量噪声。区间平均角速度近似为全局坐标系中的平均加速度状态量更新简化表达2.普通ESKF流程1确定IMU名义状态递推起点第k-1个Lidar帧到第k个Lidar帧之间的n个IMU数据的递推起点为第k-1个Lidar帧到来时观测更新后状态的最优估计2.1 预测量侧更新阶段第k-1帧到第k帧Lidar数据之间的imu状态递推2名义状态积分递推3误差协方差传递其中是对的雅可比是对的雅可比为离散噪音的协方差。2.2 观测更新阶段当第k帧Lidar观测到来时进行观测更新4计算卡尔曼增益卡尔曼增益是按不确定度自动计算的、逐方向的最优加权矩阵标量情形最直观。设先验方差、观测方差取激光很不可信如退化环境、匹配残差大结果信 IMUIMU 先验很不准如长时间无观测、偏置未收敛信激光两者相当时折中。所以就是观测占多大话语权是先验占多大话语权。同时负责跨状态传播把激光的位姿残差通过状态间的相关性传递给速度、偏置、重力等未被直接观测的量。5更新误差状态6更新误差协方差其中为观测残差是观测残差对误差状态的雅可比。7将误差状态“注入”名义状态得到最优估计并将误差状态清零2.3 ESKF的局限ESKF 的更新公式来自对观测模型的一阶线性化这个近似只有在真实状态离先验状态足够近时即误差足够小时才成立。在 LIO 中1激光观测点到面 / 点到线残差关于位姿是强非线性的尤其是旋转部分2一帧扫描的先验来自 IMU 积分当快速运动、大角速度或 IMU 偏置估计不准时先验位姿误差较大3最近邻关联point-to-piont/point-to-plane 的匹配对本身依赖于当前位姿估计。位姿不准 → 关联错 → 残差和雅可比都有误差。所以在先验点处线性化一次得到的偏差可能较大一次更新无法收敛到最优解。IESKF 把一次线性化 一次更新改为反复重新线性化 反复更新本质上是用高斯—牛顿法求解最大后验MAP问题。3. IESKF流程1确定IMU名义状态递推起点第k-1个Lidar帧到第k个Lidar帧之间的n个IMU数据的递推起点为第k-1个Lidar帧观测更新后状态的最优估计3.1 预测量侧更新阶段第k-1帧到第k帧Lidar数据之间的imu状态递推前向传播2名义状态积分递推其中3误差协方差传递其中是对的雅可比是对的雅可比。3.1.1 雅可比推导前的准备令其中是关于自变量,的函数其他量均已知。令则的形式类似于其中对于非旋转部分对于旋转部分G 是向量 → 流形 → 向量的映射扰动进了内部并跨切空间所以必须采用李代数加法形式求导。1)BCH近似得其中为左雅可比为右雅可比。再使用伴随性质将所在项挪到等式最右边则2)BCH近似得则3.1.2 i1时刻误差状态对i时刻误差状态的雅可比计算复合函数求导1其中所以2)3)其中g 是流形 → 向量的映射旋转扰动以右乘群元素形式作用在向量上所以用扰动模型求导。3.1.3 i1时刻误差状态对i时刻过程噪音的雅可比计算复合函数求导1)2)3.2 观测更新阶段IESKF 把一次线性化 一次更新改为反复重新线性化 反复更新本质上是用高斯—牛顿法求解最大后验MAP问题。当第k帧Lidar观测到来时进行迭代更新3.2.1 先验分布1正向传播预测阶段给出的先验是零均值的虽然前向传播时推导了但其主要作用是用来推导协方差的更新公式的前向传播时误差潜藏在名义状态中只有观测到来时才能将误差具象化出来。所以前向传播时假设误差是遵循零均值的高斯分布的。2观测迭代更新第次迭代时其中在处做了一阶展开。的初值为。对两端取期望得再根据线性变换的协方差传播规律得到的协方差为。所以3.2.1.1 雅可比的具体形式与3.1中的推导类似对令对于非旋转部分对于旋转部分则3.2.2 似然分布观测方程其中为第j个Lidar测距点的噪音假设服从零均值高斯分布。1观测残差的物理意义对于第j个激光点将其投影到全局坐标系后到地图中对应平面的距离。理性状态该距离应该为0这里观测值为0是几何约束。2在迭代点线性化代入在处展开令是当前迭代点算出的第j个激光点的残差直接计算得到。并不等于原始激光点的测量噪声而是经过雅可比传递后的等效噪声3整理成似然。观测量的条件分布3.2.2.1 点-面残差定义FAST-LIO2中取消了特征提取对所有点统一计算点-面残差。1对原始Lidar点进行运动补偿Lidar局部坐标系中原始第j个Lidar点要将第j个Lidar点补偿到Lidar采样周期的末尾时刻k 先使用Lidar-IMU外参将Lidar点转换到IMU坐标系使用IMU坐标系的计算出的j时刻到k时刻的相对位姿对第j个Lidar点进行运动补偿再将Lidar点从IMU坐标系转换回Lidar坐标系2将Lidar点投影到全局坐标系可以简写为其中、、是已知量令则3计算点-面残差实际是以预测位姿名义状态将点a投影到全局坐标系 图中平面是由地图中距离点最近的几个点包括拟合出的点到平面的距离即为残差。点到平面的距离由向量“点乘”平面单位法向量得到。可以参考点到线/面ICP-残差及雅可比推导A-LOAM_点到面残差构建写法-CSDN博客假设平面单位法向量为则3.2.2.2 雅可比计算观测方程其中补充了Lidar点的测量噪音。12在本次观测迭代中冻结去畸变点和外参后残差直接依赖当前位姿所以则实际去畸变结果来自 IMU 轨迹估计依赖速度、偏置等状态卡尔曼增益会把激光的位姿残差通过状态间的相关性传递给速度、偏置、重力等未被直接观测的量。3.2.3 最大后验估计先验分布似然分布最大后验估计在 LIO 中共享地图、平面拟合以及去畸变误差可能引入相关性这里冻结相关量并忽略这些相关性。并假设给定误差状态后各激光点观测条件独立。由贝叶斯公式后验密度正比于先验密度与各点似然的乘积。对后验密度取负对数舍去与优化变量无关的常数项得到本轮线性化 MAP 优化问题代价函数为的二次型。令则代价函数令梯度为04计算卡尔曼增益定义卡尔曼增益5更新误差状态利用恒等式则则其中是把迭代点往先验拉的修正项。当迭代点偏离预测状态越远该项越大。第一次迭代时该项为0退化为ESKF。3.2.4 迭代终止与协方差更新重复3.2.13.2.3中的步骤每次迭代都要用新的重新对运动补偿后的点a投影到全局坐标系执行最近邻搜索计算残差与雅可比直到则认为优化收敛了输出本轮观测迭代更新的最优估计6更新误差协方差只在迭代收敛后进行一次后验协方差更新因为迭代过程使用的是同一帧数据不能把同一帧数据当作多次独立观测累计。7并将误差状态清零误差状态已经在3.2.3中“注入”名义状态得到最优估计4. 新卡尔曼增益形式标准形式新形式其中P是n x n维H是m x n维R是m x m维K是n x m维。n为状态维度FAST-LIO中18维FAST-LIO2中24维m为观测量维度一帧激光点云存在成千上万个激光测距点m维度远大于n。标准形式中存在需要对m x m维的矩阵求逆而新形式中只需要对n x n维矩阵求逆计算量大幅下降。4.1 证明新形式与标准形式等价新形式根据SMW公式代入新形式得令则代入则4.2 SMW恒等式验证上式成立直接对上式右乘