从零入门激光SLAM（十八）——ESKF实现组合导航

本节参照高博大佬的书《自动驾驶与机器人中的SLAM技术》
代码地址
slam_in_autonomous_driving/src/ch3 at master · gaoxiang12/slam_in_autonomous_driving · GitHub

一、状态方程建立

1.定义状态变量

在误差状态卡尔曼滤波器中，真值等于名义状态变量+误差状态变量首先定义状态变量。p为平移，v为速度，R为旋转，b_g,b_a为零偏，g为重力，状态变量在连续时间下的运动方程为,其中带下标t的表示真值
x=[p,v,R,b_g,b_a,g]^⊤,
IMU的读数为

则状态变量的运动方程为

将状态变量拆为两部分：名义状态变量与误差如下所示

2.误差变量的运动方程

旋转部分用李代数表示，可以把误差变量的运动学方程整理如下：

具体的推导过程见高博大佬的
简明ESKF推导 - 知乎

3.离散时间ESKF的运动方程

名义状态变量的离散运动方程如下

而误差状态的离散形式则只需要处理连续形式中的旋转部分。参考角速度的积分公式，可以将误差状态方程写为：

1. 右侧部分我们省略了括号里的（t）以简化公式；
2. 关于旋转部分的积分，我们可以将连续形式看成关于角度的微分方程然后求解。求解过程类似于对角速度进行积分。
3. 噪声项并不参与递推，需要把它们单独归入噪声部分中。连续时间的噪声项可以视为随机过程的能量谱密度，而离散时间下的噪声变量就是我们日常看到的随机变量了。
   至此，我们给出了如何在ESKF中进行IMU递推的过程，对应于卡尔曼滤波器中的状态方程。为了让滤波器收敛，我们通常需要外部的观测来对卡尔曼滤波器进行修正，也就是所谓的组合导航。

二、预测

详情请见...

 从零入门激光SLAM（十八）——ESKF实现组合导航 - 古月居 (guyuehome.com)