MSCKF算法之Matlab版代码解读

人生导航CLK

已于 2025-04-07 09:55:52 修改

阅读量1k

点赞数 24

分类专栏： SLAM VINS 文章标签：算法

于 2025-03-24 09:48:00 首次发布

本文链接：https://blog.youkuaiyun.com/clk88888/article/details/145663014

版权

SLAM 同时被 2 个专栏收录

2 篇文章

订阅专栏

VINS

1 篇文章

订阅专栏

参考资料和源码

明确参数的含义

步骤1：加载数据

步骤2：初始化imu预测协方差和测量协方差矩阵

步骤3：导入测量数据和参考值

步骤4：初始化MSCKF

步骤5：惯导更新（更新状态量）

步骤6：MSCKF预测更新（更新协方差矩阵）

步骤7：增广相机的状态，增广雅克比，增广协方差矩阵，并更新协方差矩阵

步骤8：量测更新

误差计算

问题解答

（1）在MSCKF之中，当来一帧图像时并满足量测更新条件时，是否会更新滑动窗内的所有相机位姿和对应的IMU位姿，那么这些相机中间时刻IMU的位姿怎么处理？

（2）IMU每进行一次预测更新时，是否会利用外参生成相应的相机位姿并加入状态变量之中？

参考资料和源码

代码的源码在github上：https://github.com/yuzhou42/MSCKF

参考资料：https://zhuanlan.zhihu.com/p/595203993

明确参数的含义

qCG=q^C_G

p_C_G=p^G_{GC}

步骤1：加载数据

导入的参数的含义如下所示（压缩包里面有各个参数的含义）

b：立体相机基线，b[m]

立体相机基线是两个相机光心之间的直线距离。在双目立体视觉系统中，两个相机从不同的位置观察同一场景，从而产生视差，通过测量视差可以计算出物体的深度信息。基线的长度决定了视差的大小和深度感知的精度。

Cc v：车体坐标系(vehicle)到相机坐标系(Camera)之间的旋转矩阵

Cu、Cv就是下式公式的Cx（立体相机的水平光学中心）和Cy（立体相机的垂直光学中心）[像素pixels]

fu、fv就是下式公式的fx（立体相机的水平焦距）和fy（立体相机的垂直焦距）[像素pixels]

$\begin{array}{l} \left\{ \begin{array}{l} u = \alpha x + {c_x}\\ v = \beta y + {c_y} \end{array} \right.\\ \Rightarrow \le \left\{ \begin{array}{l} u = \alpha x + {c_x} = \alpha f\frac{{{X_c}}}{{{Z_c}}} + {c_x} = {f_x}\frac{{{X_c}}}{{{Z_c}}} + {c_x}\\ v = \beta y + {c_y} = \beta f\frac{{{Y_c}}}{{{Z_c}}} + {c_y} = {f_y}\frac{{{Y_c}}}{{{Z_c}}} + {c_y} \end{array} \right. \end{array}$

此处就是计算相机的内参，其目的在于由已知的像素坐标通过相机内参可以得到其在相机坐标系下的值，再和对应三维空间中的点去求解载体的位姿变化。（SLAM技术通过分析三维空间中的点与它们在相机坐标系下的投影之间的几何关系，不断求解和优化相机的旋转和平移，从而实现对自身位置的定位和对周围环境的三维地图构建）

KStart和KEnd是图像开始和结束计算的标志

r_i_vk_i：是位置参考值[m]，globle到imu，即P_I_G，具体值G系的原点指向I系的原点在G系下的坐标

rho_v_c_v：车体坐标系(vehicle)到相机坐标系(Camera)之间的距离

t：相机图像采集的时间 [s] 大约0.1s即采样频率为10hz，具体应该是unix时间；

theta_vk_i：是等效旋转矢量，表示G系到I系的旋转。

v_vk_vk_i：是测量的平移速度[m/s]

w_vk_vk_i：是测量的转速，即陀螺的输出[rad/s]

y_k_j：表示观测到的像素pixels，维数为4*108*707，维数的含义为4*frames*features，其中 1：2是左相机特征点在像素坐标系下的坐标系，3：4是右相机的；108是图像的帧数；707是得到的特征点个数；即一个时间点得到一帧数据，一共有4个通道，707个特征点，但是这707个不一定都有效，如下图所示：

图中表示第一个特征点在108帧图像中的位置，后面为-1表示跟踪失败了，即该特征点不在图像中了。

初始化相机内参

numLandmarks = size(y_k_j,3); //特征点的个数

camera.c_u      = cu;                   % Principal point [u pixels]
camera.c_v      = cv;                   % Principal point [v pixels]
camera.f_u      = fu;                   % Focal length [u pixels]
camera.f_v      = fv;                   % Focal length [v pixels]
camera.b        = b;                    % Stereo baseline [m]
camera.q_CI     = rotMatToQuat(C_c_v);  % 4x1 IMU-to-Camera rotation quaternion  车体（IMU）到相机坐标系的转换关系，通过提前标定获得？
camera.p_C_I    = rho_v_c_v;            % 3x1 Camera position in IMU frame

步骤2：初始化imu预测协方差和测量协方差矩阵

%步骤2.1：初始化测量协方差矩阵
w_var = 4e-2 * ones(1,3);              % rot vel var0.04   (°/√h)^2
%速度协方差
v_var = 4e-2 * ones(1,3);              % lin vel var  (ug/sqrt(Hz))^2
%陀螺仪零偏协方差
dbg_var = 1e-6 * ones(1,3);            % gyro bias change var (°/h)^2
%速度零偏协方差
dbv_var = 1e-6 * ones(1,3);            % vel bias change var  (ug)^2
%构造所有状态量的初始协方差矩阵
noiseParams.Q_imu = diag([w_var, dbg_var, v_var, dbv_var]);

%步骤2.2：初始化状态量预测协方差矩阵
q_var_init = 1e-6 * ones(1,3);         % init rot var
p_var_init = 1e-6 * ones(1,3);         % init pos var
bg_var_init = 1e-6 * ones(1,3);        % init gyro bias var
bv_var_init = 1e-6 * ones(1,3);        % init vel bias var
noiseParams.initialIMUCovar = diag([q_var_init, bg_var_init, bv_var_init, p_var_init]);

从中可以看出状态变量分别为：姿态、陀螺零偏、加表零偏、位置

msckfParams.minTrackLength = 10;        % Set to inf to dead-reckon only
msckfParams.maxTrackLength = Inf;      % Set to inf to wait for features to go out of view
msckfParams.maxGNCostNorm  = 1e-2;     % Set to inf to allow any triangulation, no matter 
msckfParams.minRCOND       = 1e-12;
msckfParams.doNullSpaceTrick = true;
msckfParams.doQRdecomp = true;

minTrackLength和maxTrackLength两个参数的设置涉及到MSCKF进行量测更新的策略：

不同于卫星信息作为观测量，视觉信息作为观测量时没有固定的滤波周期，而是采取一定的策略进行滤波更新，滤波更新在两种情况下会被触发：

（1）第一种情况是某一个特征点跟踪结束，特征点被跟踪的越长，包含该特征点的帧数就越多，在估计特征点三维位置的时候基线就越长，得到的结果也就更加准确，特征点被跟踪的越短，包含该特征点的帧数就越少，估计特征点三维位置得到的结果也就不具有可靠性，因此需要设置跟踪长度最小阈值。

（2）第二种情况是状态向量中的滤波状态达到了上限。对状态向量中的相机状态维数设置最大阈值，当达到此阈值时，删除部分相机状态，在删除的同时，利用已有的观测信息对这些状态进行滤波更新。

maxGNCostNorm用于控制三角化（triangulation）过程中的成本函数的范数。

minRCOND用于控制线性方程求解过程中的条件数阈值。

doNullSpaceTrick是一个布尔参数，用于控制是否启用零空间技巧。

doQRdecomp是一个布尔参数，用于控制是否启用QR分解。

y_k_j(y_k_j == -1) = NaN;

是将像素点在不同时刻未被检测到的坐标置为NAN

步骤3：导入测量数据和参考值

（1）导入陀螺和加表的值

（2）将所有图像中所有有效的特征点，由像素坐标系反投影到相机坐标系（归一化）

（3）导入真实参考值：

利用陀螺的值去获取相机和G系之间的关系

步骤4：初始化MSCKF

初始化就是要将之后要用到的参数进行赋值，初始化主要采用两个函数，initializeMSCKF.m和updateStateHistory.m。

（1）在函数initializeMSCKF.m之中，输入是第一帧IMU的状态，IMU测量值，相机参数和噪声参数等定义来定义MSCKF的状态(msckfState)。

①初始化IMU的值

其中第一帧IMU的状态firstImuState包括姿态和位置，靠真值来定的，陀螺和加表零偏默认为0；第一帧中imu的状态作为参数的目的在于初始化msckf中imustate；

②相机的初始化是靠IMU的初始值来确定的（通过当前IMU的位姿得到当前相机的位姿）：

$q_G^C = {\left( {q_C^I} \right)^T}q_G^I$ 和 $p_{GC}^G = p_{GI}^G + C_I^Gp_{IC}$

VINS 中的视觉初始化主要得到以下内容：

①相机位姿：通过特征点匹配、三角剖分以及 PnP 等方法，估计出图像帧的姿态，得到相机在视觉参考系下的位姿信息。不过，此时的位姿还未与世界坐标系对齐，通常是相对位姿关系。

②特征点位置：对图像中观测到的特征点进行三角剖分，得到特征点在相机坐标系下的三维坐标。这些特征点位置信息对于后续的视觉惯性联合优化以及地图构建等任务非常重要。

③尺度信息（相对尺度）：单目视觉初始化可以得到一个相对尺度，但不是真实的绝对尺度。通常会任意设置一个初始尺度，然后通过后续与 IMU 数据的融合来确定真实的绝对尺度。

④速度（视觉参考系下）：在初始化过程中，可以根据 IMU 测量和视觉测量之间的关系，估计出图像帧在视觉参考系下的速度。

获取相机位姿的原因：

①传感器互补特性：IMU 能够高频地测量加速度和角速度，可快速获取运动信息，不受光照、纹理等环境因素影响，但存在积分漂移问题。而相机能提供丰富的环境视觉信息，可用于精确的位置和姿态估计，但帧率相对较低，且在一些特殊情况下（如运动模糊、低纹理场景、光照突变）性能会下降。通过将 IMU 的位姿转换为相机的位姿，可以融合两者的优势，利用 IMU 的快速响应来弥补相机在高频运动下的不足，同时利用相机的视觉信息来修正 IMU 的漂移误差。

②系统标定需求：在 VINS 中，需要知道相机和 IMU 之间的相对位姿关系，即外参标定。通常在初始化阶段假设相机和 IMU 之间的相对位姿是已知的（通过离线或在线标定得到），那么通过 IMU 的位姿就可以推算出相机的位姿，这有助于后续进行视觉惯性联合优化时统一坐标系，使得不同传感器的数据能够在同一框架下进行处理。

③MSCKF的状态(msckfState)

（2）反馈校正updateStateHistory

校正前是msckfState.imuState，利用msckfState.camStates去更新IMU的状态imuStates（用相机的状态更新对应时刻imu的位姿状态），并公式如下所示：

$q_G^I = q_C^Iq_G^C$

$p_{GI}^G = p_{GC}^G + C_I^GC_C^Ip_{CI}^C$

步骤5：惯导更新（更新状态量）

（1）根据IMU测量值更新IMU的状态：

采用propagateImuState.m函数

姿态更新：

$\Delta \theta = \left( {\omega - {b_g}} \right) \cdot dt$

${q_{k + 1}} = {q_k} + \frac{1}{2}\left( {\Delta \theta \times } \right){q_k}$

速度

$\Delta v = \left( {v - {b_a}} \right) \cdot dt$

$p_{GI,k + 1}^G = p_{GI,k}^G + C_I^G\Delta v$

步骤6：MSCKF预测更新（更新协方差矩阵）

当有相机数据来临时，采用propagateMsckfStateAndCovar进行状态更新（预测更新），更新协方差矩阵：

由于仿真代码中的循环变量采用的是相机的序列号，所有每一次循环都会有相机数据，即更新状态量和协方差矩阵是一起的，而在实际情况是以IMU的序列号为循环变量，每进行一次循环都会进行惯导更新，但是不一定会进行协方差矩阵的更新，因此更新状态量和协方差矩阵不是一个函数里面，此处要尤为注意！

（1）建立状态转移方程：

$\left[ {\begin{array}{*{20}{c}} {{{X'}_{IMU}}}&{{{X'}_{CAM}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} \Phi &0\\ 0&I \end{array}} \right] * \left[ {\begin{array}{*{20}{c}} {{{X'}_{IMU}}}&{{{X'}_{CAM}}} \end{array}} \right]$

状态转移矩阵F和噪声驱动矩阵G的计算由不同的算法决定，维数由状态变量决定。

状态转移矩阵F的公式如下所示：

    omegaHat = measurements_k.omega - imuState_k.b_g;
    vHat = measurements_k.v - imuState_k.b_v;
    C_IG = quatToRotMat(imuState_k.q_IG);

    F(1:3,1:3) = -crossMat(omegaHat);
    F(1:3,4:6) = -eye(3);
    F(10:12,1:3) = -C_IG' * crossMat(vHat); 
    F(10:12,7:9) = -C_IG'; %

噪声驱动矩阵计算如下所示：

    G = zeros(12,12);
    
    C_IG = quatToRotMat(imuState_k.q_IG);
    
    G(1:3,1:3) = -eye(3);
    G(4:6,4:6) = eye(3);
    G(7:9,10:12) = eye(3);
    G(10:12,7:9) = -C_IG';

（2）更新协方差矩阵，公式如下所示：

$\left[ {\begin{array}{*{20}{c}} {P_{IMU}^\prime }&{P_{IMU - CAM}^\prime } {P_{IMU - CAM}^{\prime \prime }}&{P_{CAM}^\prime } \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {\rm{\Phi }}&0\\ 0&I \end{array}} \right] * \left[ {\begin{array}{*{20}{c}} {{P_{IMU}}}&{{P_{IMU - CAM}}}\\ {P_{IMU - CAM}^T}&{{P_{CAM}}} \end{array}} \right] * {\left[ {\begin{array}{*{20}{c}} {\rm{\Phi }}&0\\ 0&I \end{array}} \right]^T}$

因此：

$\begin{array}{l} P_{IMU}^\prime = {\rm{\Phi }} * {P_{IMU}} * {{\rm{\Phi }}^T}\\ {P_{CAM}} = {P_{CAM}}\\ P_{IMU - CAM}^\prime = {\rm{\Phi }} * {P_{IMU - CAM}} \end{array}$

再采用updateStateHistory更新状态量。

步骤7：增广相机的状态，增广雅克比，增广协方差矩阵，并更新协方差矩阵

当有图像信息来时，利用IMU的姿态和IMU和相机之间的固连关系，计算相机和G系之间的位姿关系。

步骤8：量测更新

使用逆深度参数构造重投影误差函数，用高斯牛顿优化的方法估计特征点3D坐标p_f_G

计算优化后特征3D坐标与相机匹配特征点之间的重投影残差

计算重投影误差对MSCKF状态量的雅克比矩阵

误差计算

(1) MSCKF解算值和理想值之间的误差：

①位置误差：

理想的位置误差： $p_{GC}^{GT} = p_{GI}^{GT} + p_{IC}^G$ ，其中 $p_{IC}^G$ 是已知的， $p_{GI}^{GT}$ 是由真值提供。

实际的位置误差：解算的值 $p_{GC}^G$

②姿态误差：

$\begin{array}{c} C_I^{C'} = C_C^{C'}C_G^CC_I^G\\ = \left( {I - \phi \times } \right)C_G^CC_I^G \end{array}$

所以 $\phi = I - C_I^{C'}{\left( {C_I^G} \right)^T}{\left( {C_G^C} \right)^T} = I - C_G^{C'}{\left( {C_I^C \cdot C_G^I} \right)^T}$ ，其中 $C_G^{C'}$ 是由MSCKF解算得到的相机姿态值。

(2) 纯惯导解算的IMU和理想IMU输出之间的误差：

①位置误差：

理想的位置误差和上述一致，实际的位置误差为解算的值 $p_{GC}^G = p_{GI}^G + p_{IC}^G$

②姿态误差：

公式和上述相同，区别在于 $C_G^{C'} = C_I^{C'}C_G^I$ ，其中 $C_G^I$ 是纯惯导解算得到的。

问题解答

（1）在MSCKF之中，当来一帧图像时并满足量测更新条件时，是否会更新滑动窗内的所有相机位姿和对应的IMU位姿，那么这些相机中间时刻IMU的位姿怎么处理？

答：量测更新仅直接作用于滑动窗口内的所有相机位姿及其对应的IMU历史状态。每个相机位姿通过固定的外参与某一时刻的IMU位姿绑定。当视觉观测通过多视图几何约束调整这些相机位姿时，其对应的IMU位姿会通过外参关系被间接修正。

滑动窗口内未被显式包含的中间IMU位姿（即未被相机位姿覆盖的时刻）不会直接被更新，但会通过以下方式间接优化：

①IMU积分与协方差传递：IMU的动力学模型通过积分连续传递状态，协方差矩阵记录了历史状态与当前状态的相关性。当窗口内的IMU位姿被修正后，协方差矩阵中的关联项会将误差修正传递至当前状态，从而隐式优化中间时刻的IMU轨迹。

②窗口滑动与局部优化：滑动窗口的移动会不断纳入新帧并剔除旧帧。窗口内的局部优化持续修正关键节点的IMU位姿，而窗口外的中间状态则依赖IMU积分和协方差传递保持一致性，避免全局优化带来的计算负担。

（2）IMU每进行一次预测更新时，是否会利用外参生成相应的相机位姿并加入状态变量之中？

答：在MSCKF中，当新的图像到达时，系统会记录当前的IMU状态，并通过外参计算出对应的相机位姿，然后将这个相机位姿加入到滑动窗口的状态变量中。滑动窗口的大小是固定的，当新帧加入时，旧帧可能会被移除。因此，相机位姿的加入是与图像帧的到来同步的，而不是每次IMU预测都发生，即相机位姿的加入频率取决于图像的帧率，而不是IMU的预测频率。

为何不每次IMU预测都加入相机位姿？

①计算效率考量

IMU的高频预测（每秒数百次）若每次生成相机位姿并加入状态向量，会导致状态维度爆炸，远超实时计算能力。滑动窗口机制通过限制窗口大小（仅保留关键帧）平衡精度与效率。

②视觉观测的稀疏性

图像帧的频率通常远低于IMU（如30Hz vs 100Hz），仅在图像到来时生成相机位姿，避免冗余数据。中间时刻的IMU状态通过积分连续传递，无需额外存储。

③多帧约束的必要性

视觉优化依赖多帧间的特征匹配（如特征点被3帧以上观测），单次IMU预测对应的孤立相机位姿无法提供有效约束，需积累多帧后联合优化。