ESKF的相关理解

对随机噪声和干扰的积分

ESKF中的处理手段：

针对陀螺仪和加速度计的bias和随机噪声进行处理，控制噪声u~对应于IMU测量中的附加噪声(wn)，而扰动会影响IMU的偏置(bias)。
噪声(wn)，白噪声（波动较快）
bias， 缓慢变化

bias会有一个初始的估计值，然后就有两部分的误差：bias估计值的误差和随机噪声；
一般认为在一个时间间隔内的bias是不变化的，所以一个时间间隔内bias的误差归结为测量噪声，在协方差中是关于时间间隔的一次项；（也是扰动项）
随机噪声是在时间间隔内对随机变量进行积分的，Fu是和时间间隔成正比的，在协方差中的二次项就是二次的。

可以把测量噪声和随机噪声都组合成一个误差脉冲，使用标准形式处理噪声和扰动，扰动作为输入，随机噪声即是噪声项
或者也可以使用简化形式，将扰动和噪声都作为误差脉冲，放在一起，更为常用

参考链接

http://epsilonjohn.club/2020/03/30/IMU%E7%9B%B8%E5%85%B3/%E5%9B%9B%E5%85%83%E6%95%B0%E7%9A%84%E7%8A%B6%E6%80%81%E8%AF%AF%E5%B7%AE%E5%8D%A1%E5%B0%94%E6%9B%BC-%E5%AF%B9%E9%9A%8F%E6%9C%BA%E5%99%AA%E5%A3%B0%E5%92%8C%E5%B9%B2%E6%89%B0%E7%9A%84%E7%A7%AF%E5%88%86/

预积分中的处理手段

预积分项中包含估计的bias，将随机噪声作为预积分项的测量噪声。同时在状态估计中会估计bias的变化量，通过预积分相对bias的雅可比矩阵，利用一阶泰勒展开进行更新。

ESKF中IMU、GPS不同解算坐标系的处理

坐标系问题全局坐标系采用东北天(ENU)坐标系,即X指向东,Y轴指向北,Z轴指向天空。坐标原点为初始时刻位置。
由第一个GPS信息初始化全局坐标系；由重力加速度引出的ENU-IMU坐标系转换的初始化 ，得到了IMU相对ENU全局坐标系的转换矩阵以后，就可以把IMU的解算结果转换到全局系了，同时不断的去更新该矩阵；GPS信息就根据第一个GPS信息不断转换到该全局坐标系中

全局坐标系的初始化

在初始化的过程中，是等到了第一个GPS信息，才开始使初始化标志为true，参考了VINS-Fusion的做法,采用了 GeographicLib 库,先把GPS数据转换至ENU笛卡尔坐标系，即以 第一个GPS的经纬高为原点，并把它转换为x-y-z水平坐标系

GPS坐标系的转换

GPS信息就根据第一个GPS信息GeographicLib 库,不断转换到该全局坐标系中。

重力加速度引出的ENU-IMU坐标系转换的初始化

一开始几秒加速度计的测量均值结果作为重力向量的估值。
使用条件：

1.一开始IMU不能有太大的加速度，偏向静止
那么此时加速度计的输出就只包含重力加速度部分，较为粗略的估计手段；

2. IMU不能位于当地水平面
因为需要用重力加速度感知水平面，即将重力加速度的方向和天向对齐，否则当初始的xy平面位于水平面时，意味着重力加速度方向已经和当地水平面的z轴对齐，重力加速度的方向无法使用了。相对地理系的roll、pitch是能感知的，只有yaw无法感知，所以其实从IMU系到ENU地理系的旋转矩阵也是不唯一的。

论文中的相关解释：

• Robust initialization of monocular visual-inertial estimation on aerial robots

参考Robust initialization of monocular visual-inertial estimation on aerial robots中提到

In usual, the average of IMU
measurements in first few seconds is treated as gravity vector
and IMU integration result is treated as the initial guess.
However, this treatment is improper when IMU measure-
ments are influenced by non-trivial bias or in accelerated
movements.

• Quaternion kinematics for the error-state Kalman filter

在上面的公式中，重力向量gt将由滤波器来估计,它有一个恒定的演化方程(130f), 相当于一个已知的常数大小。系统从一个固定的和任意已知的初始方向开始qt(t=0)=q0，(由于一般不在水平面上，使得初始重力矢量一般未知)。为了简单起见，通常采用以下这种方法: $q_0=(1,0,0,0)，因此有R_0=R{q}_0=I，然后我们估计重力向量g_t在q_0下的表示，而不是q_t$在水平坐标系中表达，因此，初始的方向不确定度转化为重力方向的初始不确定度。

open-vins也是相关的使用方法
可以参考下文代码实现部分，与之类似。

3. 在对齐当地水平面后，怎么修正yaw的偏差？

• vins-mono 中将yaw和位置x、y、z进行四自由度优化，利用回环检测进行优化。
  VINS-MONO 视惯融合的输出参考都是第一帧：将重力对齐到当地水平面使得pitch、roll是已知的，已经得到绝对姿态了，而载体待估计的状态只剩yaw和x、y、z是相对参考坐标系的。不是绝对坐标系下的。
  所以在位姿优化的时候，只对四个自由度进行优化（回环修正：由于vins系统IMU的使用，使得位姿估计只有4自由度不客观，分别是3自由度的平移和1自由度的yaw角,因此估计系统也只有在这4自由度上发生漂移。）

Benefiting from the inertial measurement of the gravity, the roll and pitch angles are fully observable in the VINS。
Therefore, the roll and pitch are absolute states in the world frame, while the x, y, z, and yaw are relative estimates with respect to the reference frame. The
accumulated drift only occurs in x, y, z, and yaw angles. To take full advantage of valid information and correct drift efficiently, we fix the drift-free roll and pitch, and only perform pose graph
optimization in 4-DOF.

所以我的问题又变成了怎么将相对参考系的量，优化到相对绝对坐标系呢？
https://zhuanlan.zhihu.com/p/90495876

cur_t ,cur_r分别为优化后当前帧的位姿， vio_t, vio_r分别为优化前有漂移的当前帧位姿。可以将在同一坐标系下位姿的漂移看作是该位姿所在坐标系的漂移，这是一个相对的过程。而我们需要求解的就是漂移后的坐标系vio到一致坐标系world的变换关系r_drift , t_drift。

回环修正：
可以将在同一坐标系下位姿的漂移看作是该位姿所在坐标系的漂移，这是一个相对的过程。
代码实现-Three axises of the ENU frame in the IMU frame
找到ENU坐标系的三个轴向第一个IMU帧坐标系的旋转矩阵
Three axises of the ENU frame in the IMU frame.这句话是关键，
因为加速度计的输出均值就是重力加速度的值和方向，默认值采用9.8与当地纬度相关，不再进行估计，所以对均值结果归一化以后就是方向。同时加速度计的输出是以imu 坐标系为参考的，归一化以后的结果也就是重力向量(ENU的天向)在IMU系的表达，即ENU系向IMU系变换的旋转矩阵的第三列；
再解算出其中一个与z向垂直的水平面即x、y轴，构成旋转矩阵的第一、二列。

// z-axis.
    const Eigen::Vector3d& z_axis = mean_acc.normalized(); //加速度的均值作为z轴

    // x-axis.
    Eigen::Vector3d x_axis = 
        Eigen::Vector3d::UnitX() - z_axis * z_axis.transpose() * Eigen::Vector3d::UnitX();
    x_axis.normalize();//normalize无返回值，而normalized()有返回模值

    // y-axis.
    Eigen::Vector3d y_axis = z_axis.cross(x_axis);
    y_axis.normalize();

    Eigen::Matrix3d I_R_G;
    I_R_G.block<3, 1>(0, 0) = x_axis;
    I_R_G.block<3, 1>(0, 1) = y_axis;
    I_R_G.block<3, 1>(0, 2) = z_axis;

    *G_R_I = I_R_G.transpose();

状态量的初始化

[R、P、V、ba、bg] 共有这四个状态量，在坐标系处理过程中，已经初始化了R，即IMU向全局坐标系的转换矩阵；

因为不知道P、V的信息，可以设为零，但是给出较大的协方差，它的逆很小，代表可信度较小。
同时yaw方向的不确定度大与pitch、roll

     // Set initial mean.
    state->G_p_I.setZero();

    // We have no information to set initial velocity. 
    // So, just set it to zero and given big covariance.
    state->G_v_I.setZero();
      // Set covariance. [R、P、V、ba、bg] 15维状态
    state->cov.setZero();
    state->cov.block<3, 3>(0, 0) = 100. * Eigen::Matrix3d::Identity(); // position std: 10 m
    state->cov.block<3, 3>(3, 3) = 100. * Eigen::Matrix3d::Identity(); // velocity std: 10 m/s
    // roll pitch std 10 degree.
    state->cov.block<2, 2>(6, 6) = 10. * kDegreeToRadian * 10. * kDegreeToRadian * Eigen::Matrix2d::Identity();
    state->cov(8, 8)             = 100. * kDegreeToRadian * 100. * kDegreeToRadian; // yaw std: 100 degree.
    // Acc bias.
    state->cov.block<3, 3>(9, 9) = 0.0004 * Eigen::Matrix3d::Identity();
    // Gyro bias.
    state->cov.block<3, 3>(12, 12) = 0.0004 * Eigen::Matrix3d::Identity();

关于local坐标系的定义

角速度ω被定义为相对于nominal-state是local的(The angular rates ω are defined locally with respect to the nominal quaternion)，这允许我们直接使用角速度测量值ωm，它提供了body系的角速度。
角度误差δθ被定义为相对于nominal-state是local的，这未必是最佳的方式，但这与大多数imu积分过程中的选择是一致的(经典方法)。

精华理解

https://blog.csdn.net/wubaobao1993/article/details/84327700
1). 优化过程中，当得到新的bias之后，使用一阶泰勒展开的形式更新所有的预积分结果；2). 优化过程中，作为求解bias增量时的jacobian。
开始的时候，笔者一直没有搞懂为什么上面的过程得到的状态转移矩阵可以用在这些地方，毕竟我们知道，上面的两个地方本质上都是泰勒的阶展开，而一阶展开明确的公式就是f ( x + δ x ) = f ( x ) + J δ x f(x+δx)=f(x)+Jδxf(x+δx)=f(x)+Jδx，而上面的状态转移矩阵中的各个项是否能够代替上面的jacobian呢？答案是肯定的！
比如我们对速度V求与加速度bias的导数，那么根据求导的定义

作差的部分按我的理解就是预积分相关的项

完整参考链接

https://www.guyuehome.com/8353
https://blog.csdn.net/wubaobao1993/article/details/84327700
http://epsilonjohn.club/2020/03/30/IMU%E7%9B%B8%E5%85%B3/%E5%9B%9B%E5%85%83%E6%95%B0%E7%9A%84%E7%8A%B6%E6%80%81%E8%AF%AF%E5%B7%AE%E5%8D%A1%E5%B0%94%E6%9B%BC-%E5%AF%B9%E9%9A%8F%E6%9C%BA%E5%99%AA%E5%A3%B0%E5%92%8C%E5%B9%B2%E6%89%B0%E7%9A%84%E7%A7%AF%E5%88%86/