MsckfVio Estimator/Filter

0.引言

文章只是自己过一遍代码的笔记，大部分参考自网络，尽量都列出出处。

主要参考文章：

• 数学基础

• note

• 从前端接收的数据：

ID 是全局ID，通过ID来建立的跟踪关系。

• 后端数据结构：

StateServer:

MapServer:

void MsckfVio::addFeatureObservations(
    const CameraMeasurementConstPtr& msg) {

  StateIDType state_id = state_server.imu_state.id;
  int curr_feature_num = map_server.size();
  int tracked_feature_num = 0;

  // step 3.1.Add new observations for existing features or new
  // features in the map server.
  for (const auto& feature : msg->features) {
    if (map_server.find(feature.id) == map_server.end()) {
      // This is a new feature.
      map_server[feature.id] = Feature(feature.id);
      map_server[feature.id].observations[state_id] =
        Vector4d(feature.u0, feature.v0,
            feature.u1, feature.v1);
    } else {
      // This is an old feature.
      map_server[feature.id].observations[state_id] =
        Vector4d(feature.u0, feature.v0,
            feature.u1, feature.v1);
      ++tracked_feature_num;
    }
  }
  // step 3.2.计算跟踪成功率
  tracking_rate =
    static_cast<double>(tracked_feature_num) /
    static_cast<double>(curr_feature_num);

  return;
}

系统框架:

0.1.符号定义

0.2.EKF回顾

1.三种状态模型

使用误差状态模型对状态模型进行更新，需要做的有三步骤：

• 1.对 nominal-state 进行积分
• 2.对 error-state 进行积分[EKF在这步里计算]
• 3.true-state = nominal-state + error-state

2.nominal-state IMU 状态量预测（Propagation/Prediction）

${\mathbf{x}}_{t_n}-积分\to {\mathbf{x}}_{t_{n+1}}$:离散时间的积分，得到状态递推公式。

nominal-state不同状态量的积分方式选择不同：

2.1.nominal-state 位置、速度的四阶龙格-库塔积分

回顾一下龙格库塔积分法：龙格 - 库塔( Runge-Kutta )方法是解决常微分方程初值问题的高精度单步法,这类方法与泰勒( Taylor )展开有密切联系。参考数值分析：

具有四阶 Taylor展开精度，原理推导见数值分析教材。

回到正题，计算 位置 $\mathbf{p}$， 速度 $\mathbf{v}$ 的积分：

位置和速度的微分方程为

$$\begin{array}{rl}& \dot{\mathbf{p}}=\mathbf{v}\\ & \dot{\mathbf{v}}=\mathbf{a}=\mathbf{R}\left({\mathbf{a}}_m-{\mathbf{a}}_b\right)+\mathbf{g}\end{array}$$

计算 Runge-Kutta有，

nominal-state中的 ${\mathbf{a}}_m-{\mathbf{a}}_b$ 实际计算时直接使用的测量值 ${\mathbf{a}}_m=acc$， 疑问： ${\mathbf{R}}_{(t_{n+1}+t_n)/2}$ 是如何计算的，是对数运算后取一般角度吗？

2.2.nominal-state 四元数积分

2.3.其他微分为 0 的项，显然其值保持不变

3.error-state IMU 误差状态量

3.1.error-state IMU 运动方程

3.2.系统状态协方差预测

一阶微分方程，具有闭式解：
$$\begin{array}{rl}& \dot{\mathbf{x}}=\mathbf{F}(t)\mathbf{x}(t)+\mathbf{G}(t)\mathbf{w}(t)\end{array}$$

• IMU 的状态，在2.nominal-state IMU 状态量预测（Propagation/Prediction）新小节进行更新
• 在误差状态系统里，状态是由噪声驱动，状态的均值为0，只需更新协方差，对于IMU的协方差矩阵, 还是需要先求取状态转移矩阵和离散的运动噪声协方差矩阵,如下:

$$\begin{gathered} {\Phi }_k=\Phi (t_{k+1},t_k)=\text{exp}({\int }_{t_k}^{t_{k+1}}\text{F}(\tau )d\tau ) \\ {\text{Q}}_k={\int }_{t_k}^{t_{k+1}}\Phi (t_{k+1},\tau )\text{GQG}\Phi (t_{k+1},\tau {)}^{T}d\tau \end{gathered}$$

整个状态(IMU+Camera)的covariance传播过程如图所示:

那么对于左上角 IMU 的 corvariance 的传播，则为此小节误差状态求得的协方差矩阵：

$${\mathbf{P}}_{I{I}_{k+1\mid k}}={\mathbf{\Phi }}_k{\mathbf{P}}_{I{I}_{k\mid k}}{\mathbf{\Phi }}_k^{\top }+{\mathbf{Q}}_k$$

其中Camera的covariance暂时还没有变化是因为这个时间段图像还没有到来,只有IMU的影响,但是会影响到IMU与Camera协方差,即上图灰色矩形块:

4.状态增广 State Augmentation

4.1.绝对观测模型与相对观测模型

MSCKF 相比EKF-SLAM特殊之处在于:

• 1.状态量中不维护3D路标点(状态量就会少很多，也会降低计算量）
• 2.利用零空间投影边缘化3D路标点建立相机之间的相对观测约束
• 3.通过状态增广的方式维护多个历史相机位姿状态
  

4.2.增广相机状态向

对 error-state 进行状态量增广。

• 根据 IMU 位姿求取 Camera 位姿：

• 对误差状态向量增广相机状态向量:$$\delta {\mathbf{\theta }}_{w2c}^T,{\delta }^{\mathrm{w}}{\mathbf{p}}_c^T$$

4.3.状态协方差扩增

误差状态由噪声驱动，误差状态的协方差矩阵就是系统的协方差矩阵？

当图像到来时,需要对当前相机姿态做增广,这个时刻的相机姿态是由上一时刻的IMU propagate的结果结合外参得到的:
$$\delta {\mathbf{\theta }}_{w2c}^T,{\delta }^{\mathrm{w}}{\mathbf{p}}_c^T$$ XX ,从代码看这里增广类似于 nominal-state 状态量添加的是 $$R_{w2c},t_{c2w}{(}^w{p}_c)$$
外参用到了 ${\mathbf{T}}_{w2i}$， ${\mathbf{T}}_{i2c}$.

假设上一时刻共有 N 个相机姿态在状态向量中,那么当新一帧图像到来时,这个时候整个滤波器的状态变成了 21+6(N+1) 的向量, 那么它对应的 covariance 维度变为 $(21+6(N+1))\times (21+6(N+1))$ .其数学表达式为:

$${\text{P}}_{k|k}=\left(\begin{array}{c}{\text{I}}_{21+6N}\\ \text{J}\end{array}\right){\text{P}}_{k|k}{\left(\begin{array}{c}{\text{I}}_{21+6N}\\ \text{J}\end{array}\right)}^T$$

${\mathbf{x}}_j={\mathbf{J}}_{ji}{\mathbf{x}}_i$ 这个式子怎么来的 ？

高斯分布线性变换的协方差矩阵性质：
假设有高斯随机变量: $\mathbf{x}\in {\mathbb{R}}^N\sim \mathcal{N}\left({\mathbf{\mu }}_x,{\mathbf{\Sigma }}_{xx}\right)$, 以及另一个与 $\mathbf{x}$ 线性相关的随机变量 $\mathbf{y}\in {\mathbb{R}}^M$ :
$$\mathbf{y}=\mathbf{G}\mathbf{x}$$
其中 $\mathbf{G}\in {\mathbb{R}}^{M\times N}$ 是一个常量矩阵。我们的目标是研究 $\mathbf{y}$ 的统计特性。一个简单的方法是直接计算期望和方差:
$$\begin{array}{rl}{\mathbf{\mu }}_y& =E[\mathbf{y}]=E[\mathbf{G}\mathbf{x}]=\mathbf{G}E[\mathbf{x}]=\mathbf{G}{\mathbf{\mu }}_x\\ {\mathbf{\Sigma }}_{yy}& =E\left[\left(\mathbf{y}-{\mathbf{\mu }}_y\right){\left(\mathbf{y}-{\mathbf{\mu }}_y\right)}^{\mathrm{T}}\right]\\ & =\mathbf{G}E\left[\left(\mathbf{x}-{\mathbf{\mu }}_x\right){\left(\mathbf{x}-{\mathbf{\mu }}_x\right)}^{\mathrm{T}}\right]{\mathbf{G}}^{\mathrm{T}}=\mathbf{G}{\mathbf{\Sigma }}_{xx}{\mathbf{G}}^{\mathrm{T}}\end{array}$$那么, 我们就得到 $\mathbf{y}\sim \mathcal{N}\left({\mathbf{\mu }}_y,{\mathbf{\Sigma }}_{yy}\right)=\mathcal{N}\left(\mathbf{G}{\mathbf{\mu }}_x,\mathbf{G}{\mathbf{\Sigma }}_{xx}{\mathbf{G}}^{\mathrm{T}}\right)$ 。

因此，对状态量协方差矩阵的增广就可以通过 $\begin{array}{rl}\underset{(6,21+6\mathrm{N})}{J}=& {\left[{\left(\frac{\partial {\mathbf{x}}_c}{\partial {\mathbf{x}}_{imu}^T}\right)}^T,{\left(\frac{\partial {\mathbf{x}}_c}{\partial {\mathbf{x}}_c^T}\right)}^T\right]}^T\end{array}$ 计算得出，如PPT所示。重点就转为求 ${\left(\frac{\partial {\mathbf{x}}_c}{\partial {\mathbf{x}}_{imu}^T}\right)}^T$：

论文中的结果：

5.特征点三角化

计算获取世界坐标系下的 3D 特征点,代码在 Feature::initializePosition

5.1. 对极几何计算初始深度

据对极几何原理,三角化计算初始深度,得到初始三维点坐标:

5.2.LM优化3D点

• 通过 L-M 算法迭代优化得到更加精确的世界系三维点

迭代优化求解：

5.3. 投影世界系三维点到相机系**

position = T_c0_w.linear()*final_position + T_c0_w.translation();

6.系统测量更新

使用相机测量进行系统进行校正。

6.1. 线性化测量模型

使用相机进行更新，使用 测量的 error-state ？

线性化测量模型怎么推导出来的？–>【挖坑】OC-EKF

这里 $H$ 为啥是两部分：一部分为 error-state 状态量【对IMU的 $H$ 也全为0，只有与相机相关的部分有值】，一部分为 相机3D点，但状态量中没有，所以需要进行左零空间投影。

视觉测量残差可近似表示为

$r$ 为重投影误差。

  // 3D点世界坐标系 wP_{f_i} = [x,y,z]^T
  const Vector3d& p_w = feature.position;
  const Vector4d& z = feature.observations.find(cam_state_id)->second;

  // Convert the feature position from the world frame to
  // the cam0 and cam1 frame.
  // 3D点世界坐标系下左右目相机观测
  Vector3d p_c0 = R_w_c0 * (p_w-t_c0_w);
  Vector3d p_c1 = R_w_c1 * (p_w-t_c1_w);

  // Compute the Jacobians.
  // 式（1）
  Matrix<double, 4, 3> dz_dpc0 = Matrix<double, 4, 3>::Zero();
  dz_dpc0(0, 0) = 1 / p_c0(2);
  dz_dpc0(1, 1) = 1 / p_c0(2);
  dz_dpc0(0, 2) = -p_c0(0) / (p_c0(2)*p_c0(2));
  dz_dpc0(1, 2) = -p_c0(1) / (p_c0(2)*p_c0(2));
  // 式（1）
  Matrix<double, 4, 3> dz_dpc1 = Matrix<double, 4, 3>::Zero();
  dz_dpc1(2, 0) = 1 / p_c1(2);
  dz_dpc1(3, 1) = 1 / p_c1(2);
  dz_dpc1(2, 2) = -p_c1(0) / (p_c1(2)*p_c1(2));
  dz_dpc1(3, 2) = -p_c1(1) / (p_c1(2)*p_c1(2));

  Matrix<double, 3, 6> dpc0_dxc = Matrix<double, 3, 6>::Zero();
  dpc0_dxc.leftCols(3) = skewSymmetric(p_c0);  // 式（2）
  dpc0_dxc.rightCols(3) = -R_w_c0;  // 式（3）

  Matrix<double, 3, 6> dpc1_dxc = Matrix<double, 3, 6>::Zero();
  dpc1_dxc.leftCols(3) = R_c0_c1 * skewSymmetric(p_c0); // 式（4）
  dpc1_dxc.rightCols(3) = -R_w_c1; // 式（5）

  Matrix3d dpc0_dpg = R_w_c0; // 式（6）
  Matrix3d dpc1_dpg = R_w_c1; // 式（7）

  H_x = dz_dpc0*dpc0_dxc + dz_dpc1*dpc1_dxc;
  H_f = dz_dpc0*dpc0_dpg + dz_dpc1*dpc1_dpg;

6.2.边缘化移除H中的3D点

进行相机状态边缘化后，继续对方程进行QR分解，将误差状态量中关于多帧相机的状态量进行压缩：

6.4.EKF Update

7.TODO

阅读参考文献：

本文论文：

• A Multi-State Constraint Kalman Filter for Vision-aided Inertial Navigation
• Robust Stereo Visual Inertial Odometry for Fast Autonomous Flight

基础知识：

• Quaternion kinematics for the error-state KF
• Indirect Kalman Filter for 3D Attitude Estimation-A Tutorial for Quaternion Algebra

能观测性分析 OC-EKF 相关：

• Consistency Analysis and IMprovement of Vision-aided Inertial Navigation
• On the Consistency of Vision-Aided Inertial Navigation

其他参考博客：

• MSCKF-VIO系列
• MSCKF那些事
• Quaternion kinematics for the error-state KF翻译
• 对 Error-State Kalman Filter 的理解