MSCKF第6讲：状态与协方差

MSCKF中状态与协方差

1 状态

msckf_vio.h

状态 StateServer state_server;

注意到，实际上imu_state和cam_states近似同步，并不是所有的imu数据都会建立一个状态。但是会处理区间内所有的imu数据。

struct StateServer {
    IMUState imu_state;
    Cam cam_states;

    // State covariance matrix	
    Eigen::MatrixXd state_cov;				// 误差状态(包括IMU和Cam)协方差矩阵
    Eigen::Matrix<double, 12, 12> continuous_noise_cov;		// IMU连续噪声协方差 4*3，即运动方程中的噪声方差
};

// state_cov = A*state_cov*A + continuous_noise_cov 就是满足高斯运算，误差状态协方差由上一个时刻递推，然后再加上噪声协方差矩阵。

  下面G表示世界系global，I表示IMU坐标系（作为body系），cam0表示左目相机系。关于坐标系表示的申明，下面这种表达方式${}_G^{I}R$等价于$R_{IG}$，是一种常见的转换关系。

2 IMU状态与噪声协方差

2.1 相关定义

(4+3+3+3+3)×1 = 16×1
$${\mathbf{X}}_{\mathrm{I}}={\left[\begin{array}{ccccc}{}_G^I{\bar{q}}^T& {\mathbf{b}}_g^T& {}^G{\mathbf{v}}_I^T& {\mathbf{b}}_a^T& {}^G{\mathbf{p}}_I^T\end{array}\right]}^T$$

• ${}_G^I{\bar{q}}^T$表示 the rotation from frame {G} to frame {I}，描述世界系到惯性系的旋转，用单位四元数表示，4维。
• ${}^G{\mathbf{p}}_I^T$和${}^G{\mathbf{v}}_I^T$表示位置和速度，即平移量，the IMU position and velocity with respect to {G},在世界系下表示。
• ${\mathbf{b}}_g^T$和${\mathbf{b}}_a^T$分别表示the biases affecting the gyroscope and accelerometer measurements，即陀螺仪和加速度计的偏差。

imu_state.h

状态量	含义
Eigen::Vector4d orientation	$q_{iw}$
Eigen::Vector3d position	$t_{wi}$表示IMU在世界坐标系中的位置
Eigen::Vector3d velocity	${}^{w}v$表示IMU在世界坐标系中的速度
Eigen::Vector3d gyro_bias	$b_g$ 角速度偏差，IMU静态初始化计算
Eigen::Vector3d acc_bias	$b_a$加速度偏差，IMU静态初始化计算
Eigen::Matrix3d R_imu_cam0	$R_{c_{0}i}$
Eigen::Vector3d t_cam0_imu	$t_{i{c}_0}$
static double gyro_noise、acc_noise	$n_g$与$n_a$，均值为0的高斯噪声，主要利用协方差
static double gyro_bias_noise、acc_bias_noise	$n_{bg}$与$n_{ba}$
static Eigen::Isometry3d T_imu_body	identity
static Eigen::Vector3d gravity	${}^{w}g$
Eigen::Vector4d orientation_null	可观性约束
Eigen::Vector3d position_null
Eigen::Vector3d velocity_null

  注意gyro_bias和gyro_noise的区别，一个是角速度偏置（均值），一个是角速度噪声方差

2.2 运动方程—IMU连续噪声协方差

   state_server.continuous_noise_cov是运动方程中的噪声方差，由使用者提供，计算一次后不再发生变化。

imu参数—噪声标准差

nh.param<double>("noise/gyro", IMUState::gyro_noise, 0.001);
nh.param<double>("noise/acc", IMUState::acc_noise, 0.01);
nh.param<double>("noise/gyro_bias", IMUState::gyro_bias_noise, 0.001);
nh.param<double>("noise/acc_bias", IMUState::acc_bias_noise, 0.01);
// 特征的噪声
nh.param<double>("noise/feature", Feature::observation_noise, 0.01);

对应方差

IMUState::gyro_noise *= IMUState::gyro_noise;
IMUState::acc_noise *= IMUState::acc_noise;
IMUState::gyro_bias_noise *= IMUState::gyro_bias_noise;
IMUState::acc_bias_noise *= IMUState::acc_bias_noise;
Feature::observation_noise *= Feature::observation_noise;

构建连续噪声协方差

state_server.continuous_noise_cov = Matrix<double, 12, 12>::Zero();
state_server.continuous_noise_cov.block<3, 3>(0, 0) = Matrix3d::Identity() * IMUState::gyro_noise;
state_server.continuous_noise_cov.block<3, 3>(3, 3) = Matrix3d::Identity() * IMUState::gyro_bias_noise;
state_server.continuous_noise_cov.block<3, 3>(6, 6) = Matrix3d::Identity() * IMUState::acc_noise;
state_server.continuous_noise_cov.block<3, 3>(9, 9) = Matrix3d::Identity() * IMUState::acc_bias_noise;

应用：卡尔曼滤波应用阶段–计算先验误差状态协方差矩阵

state_cov = A*state_cov*A + continuous_noise_cov

Matrix<double, 21, 21> Q =
        Phi * G * state_server.continuous_noise_cov * G.transpose() * Phi.transpose() * dtime;

state_server.state_cov.block<21, 21>(0, 0) =
        Phi * state_server.state_cov.block<21, 21>(0, 0) * Phi.transpose() + Q;

2.3 误差状态协方差矩阵

配置文件yaml中提供了误差状态初始协方差state_server.state_cov

<!-- These values should be covariance -->
    <param name="initial_covariance/velocity" value="0.25"/>
    <param name="initial_covariance/gyro_bias" value="0.01"/>
    <param name="initial_covariance/acc_bias" value="0.01"/>
    <param name="initial_covariance/extrinsic_rotation_cov" value="2.742e-3"/>
    <param name="initial_covariance/extrinsic_translation_cov" value="4e-4"/>

初始化：旋转和平移误差状态协方差初始置为0，每一个变量是3维矩阵，7个变量一共21维度。一开始这个误差状态协方差的值是通过配置文件给出的！

外参

(4+3)×1
$${}_C^I{\mathbf{q}}^{\top }{}^I{\mathbf{p}}_C^{\top }$$

state_server.state_cov = MatrixXd::Zero(21, 21);
for (int i = 3; i < 6; ++i)
    state_server.state_cov(i, i) = gyro_bias_cov;
for (int i = 6; i < 9; ++i)
    state_server.state_cov(i, i) = velocity_cov;
for (int i = 9; i < 12; ++i)
    state_server.state_cov(i, i) = acc_bias_cov;
for (int i = 15; i < 18; ++i)
    // 外参 Rbc
    state_server.state_cov(i, i) = extrinsic_rotation_cov;
for (int i = 18; i < 21; ++i)
    // 外参tbc
    state_server.state_cov(i, i) = extrinsic_translation_cov;

卡尔曼滤波预测阶段

state_server.state_cov.block<21, 21>(0, 0) =
        Phi * state_server.state_cov.block<21, 21>(0, 0) * Phi.transpose() + Q;

预测之后更新cam部分，就是imu变了，cam也改变

    // 7. 如果有相机状态量，那么更新imu状态量与相机状态量交叉的部分
    if (state_server.cam_states.size() > 0)
    {
        // 起点是0 21  然后是21行 state_server.state_cov.cols() - 21 列的矩阵
        // 也就是整个协方差矩阵的右上角，这部分说白了就是imu状态量与相机状态量的协方差，imu更新了，这部分也需要更新
        state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21) =
            Phi * state_server.state_cov.block(0, 21, 21, state_server.state_cov.cols() - 21);
	.....
    }

状态增广（new cam come from）后的误差状态协方差

// 4. 增广协方差矩阵
// 简单地说就是原来的协方差是 21 + 6n 维的，现在新来了一个伙计，维度要扩了
// 并且对应位置的值要根据雅可比跟这个时刻（也就是最新时刻）的imu协方差计算
// 4.1 扩展矩阵大小 conservativeResize函数不改变原矩阵对应位置的数值
// Resize the state covariance matrix.
size_t old_rows = state_server.state_cov.rows();
size_t old_cols = state_server.state_cov.cols();
state_server.state_cov.conservativeResize(old_rows + 6, old_cols + 6);

卡尔曼滤波更新阶段

// 4. 更新协方差
MatrixXd I_KH = MatrixXd::Identity(K.rows(), H_thin.cols()) - K * H_thin;
state_server.state_cov = I_KH * state_server.state_cov;

滑动窗口溢出后

if (cam_state_end < state_server.state_cov.rows())
{
    state_server.state_cov.block(cam_state_start, 0,
                                 state_server.state_cov.rows() - cam_state_end,
                                 state_server.state_cov.cols()) =
        state_server.state_cov.block(cam_state_end, 0,
                                     state_server.state_cov.rows() - cam_state_end,
                                     state_server.state_cov.cols());

    state_server.state_cov.block(0, cam_state_start,
                                 state_server.state_cov.rows(),
                                 state_server.state_cov.cols() - cam_state_end) =
        state_server.state_cov.block(0, cam_state_end,
                                     state_server.state_cov.rows(),
                                     state_server.state_cov.cols() - cam_state_end);

    state_server.state_cov.conservativeResize(
        state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);
}
else
{
    state_server.state_cov.conservativeResize(
        state_server.state_cov.rows() - 6, state_server.state_cov.cols() - 6);
}

3 cam状态

  利用滑动窗口来记录的，不只是一帧的相机姿态。at any time instant the EKF state vector comprises (i) the evolving IMU state (ii)a history of up to N max past poses of the camera
$${\mathbf{X}}_{\mathrm{C}}={\left[\begin{array}{c}{\mathbf{T}}_m^T\cdots {\mathbf{T}}_n^T\end{array}\right]}^T$$
  注意相机的每个T都是由${}^C{q}_G$和${}^G{\mathbf{p}}_{Cam0}$表示（C也是cam0，简单表示，(4+3)×1×N ）

  说白了就是给每一帧创建了一个结构体，里面存储了基本变量，类似于ORB中的Frame

cam_state.h

状态量	含义
Eigen::Vector4d orientation	$q_{cw}$
Eigen::Vector3d position	$t_{wc}$
Eigen::Isometry3d T_cam0_cam1	$T_{c_1{c}_0}$
Eigen::Vector4d orientation_null	修改测量雅可比矩阵，后面再细看
Eigen::Vector3d position_null
StateIDType id	当前帧状态id
double time	当前帧对应时间戳