博客

矩阵维度必须一致_OpenVINS (7)- 能观一致性分析和FEJ

  • Post author:
  • Post category:其他


首先罗列出文献中总结的以下几个结论：

  1. 能观性矩阵的零空间维度反映了系统不可观的维度
  2. VINS系统不可观维度等于4，通常为平移和绕重力方向的yaw
  3. 能观不一致性的原因是EKF的转移和观测Jacobian矩阵的线性化点不同而造成的
  4. EKF错误的可观现象导致yaw的协方差较小，从而系统整体更新出现累计误差

能观性和相应的解决方法已经有很多作者都进行了分析和求解，本文主要介绍黄国权，Roumeliotis和李明杨三个作者从各自的角度阐述上述问题。


一. 能观性分析和解决方法



1. OC-EKF， Huangguoquan

step1 首先定义标准的EKF过程如下：

a.定义EKF状态

90c645ce69733fe852d9bd19228ecbf6.png

b.定义EKF运动方程

391c53b5e412e8f93543cb62612424d9.png

c.定义误差的转移方程

94efff076a14f489862ba6247ae86595.png

注意这里的pRk|k 表示的是上一时刻的EKF状态更新值

d. 测量更新

945f0b0e48f1c36d9e0de4eab2188e35.png

注意这里的pLk|k-1 表示的是上一时刻的EKF状态估计值

step 2 分析一个非线性SLAM系统的不可观维度，如2D SLAM系统如下：

c97269feeb63758c783df0a8640e9519.png

可见，零空间的维度=3，旋转θ和平移量xy不可观

step 3 构造EKF线性化系统的能观性矩阵

1afa6d5ac2b81af819236a760cb42244.png

step 4 对于理想EKF系统的情况下，假设真值已知，且

b287a601db49f5743ee950c83b0936b3.png

零空间的维度仍然为3

180c0a23569b08c8cd359e58249bc1de.png

step 5 对于实际EKF系统的情况下，真值未知，且

23a8868e2d2f013b4a09d1ac66535413.png

可见，实际EKF系统的能观性矩阵零空间的维度为2

efe44b191b27859e2c3a3399cab63f52.png

可见，不同的线性化点对于系统可观性的影响，文章原话如下：

In turn, this implies that the choice of linearization points affects the observability properties of the linearized error-state system of the EKF.

这一现象导致系统认为旋转量被错误的可观，从而协方差较小，可信度较高，对其更新量较小，从而导致旋转方向上的误差累计越来越多，整个系统的误差水平也越来越大。


解决上述的能观性不一致的思路，需要满足

8688fe1f144957a83e18bcd470246073.png

即，在每次的EKF传递和更新过程中，保证N空间的维度始终等于3

step 6 解决方案1，OC-EKF1，也就是First-Estimates-Jacobian (FEJ)-EKF

这个方法直接用上一时刻的估计值替换EKF上一时刻的预测值，带入到转移和观测矩阵

a55514a071e57547a8eaaee9d4bc3757.png

替换

cce1ab62d7d3406a1ce84354ee4fedcb.png

step 7 解决方案2，OC-EKF2

方法一的精度和稳定性取决于 线性化点估计值的准确性

Even though the OC-EKF1 typically performs substantially better than the standard EKF (see Sections 2.6 and 2.7), it relies heavily on the initial state estimates; if these estimates are inaccurate, the linearization errors become large and thus the performance of the estimator may

degrade. This could be the case when the first estimates of the landmark positions are of poor quality (e.g., in bearing-only SLAM).

所以作者提出了一种已知条件的优化问题

5b73060683f55df5167f8a2676be6b8c.png

拉格朗日乘子法（Lagrange Multiplier）参考https://blog.csdn.net/zhangyingjie09/article/details/80368494

已知了λ之后，可以对转移和观测矩阵进行调整：

94fc84d92a299683c3c062b5c81b4edc.png

step 8 解决方案3，OC-EKF3

与OC-EKF2先求取线性化点的变量，再求取转移和观测矩阵不同，该方法直接计算观测矩阵

1634db26bf08c09043f331ae20eff335.png

上述三种方案总结如下：

3db65fcca8fad8d2944ce0aabfc03511.png

参考：

2008 Analysis and improvement of the consistency of extended kalman filter based slam

2010 Observability-based Rules for Designing Consistent EKF SLAM Estimators.

博士论文 Improving the Consistency of Nonlinear Estimators Analysis Algorithms and Applications



2. OC-VINS，Stergios I. Roumeliotis

这个能观性分析和调整方法用在了kumar s-MSCKF中，对转移和观测矩阵进行调整。同时，注意到他也将特征点分成跟踪OF和SLAM特征点DF。

step 1 定义EKF状态（除了IMU的状态之外，还含有SLAM特征点坐标，IMU状态用MSCKF的方法来更新，SLAM特征点用EKF的方法来更新）

a4a69f2cf9793e9032fb15c44fcae407.png

step 2 定义 EKF误差状态

36b4ca82fc793f010d85f42a2c5faee2.png

step 3 EKF状态转移和协方差矩阵

04b7c7001dabef8b8b98b32ef9ab27a0.png

step 4 EKF 观测矩阵

0e7397d88bd3a525128301fd0050515f.png

step 5 EKF 残差和权值

087a78150e9947f6665c2ec153761f93.png

step 6 EKF更新

dbe852c32b0c1fc9504e22a116e70d5b.png

step 7 定义能观性矩阵

2ac62da6d43ea984a7acfc2eb08724b9.png

step 8 带入转移和观测矩阵

70c110c57e09216f898bd98e113ff64d.png

step 9 零空间分析

c9404b4e67a2f178c05f778bc4ee9bb6.png


为了保证能观一致性，需要满足

d2eb694bc469d435096fb44957e569c1.png

注，与huangguoquan的能观性修正方法的思路一致，都是要满足上述公式，只不过具体求解转移和观测矩阵的封闭解过程不同。

step 10 调整转移矩阵

已知

2cc76c0d012cd3825dbfee5d8d3ed19f.png

则

4e84f0fb7926224cd9380c5c5cdb21eb.png

非零的三行写成：

f8219988b5bb6aa9b07073acfb1a19f0.png

上述三个式子构成了拉格朗日乘法优化目标函数：

e7a5ea5a6fc41dccf2a0d119d73579fe.png

求解

ef75d96ef2756850c8011ef44e06eed9.png

step 11 调整观测矩阵

9b7a6854cea7be232b860b8d1fa26af4.png

则

8e51e7c2020009ef87b5220c0abb6975.png

上述两个式子构成了拉格朗日乘法优化目标函数：

96d9281e1067e430d5ede7d086b349b9.png

求解A*并且更新H

77ffd737198d4faa27fc12eb37e65ce9.png

参考：

2012 Observability-constrained vision-aided inertial navigation

2014 Consistency analysis and improvement of vision-aided inertial navigation



3. MSCKF2.0， limingyang

step 1 分析非线性系统，如一个典型的运动和观测系统

85419b2284cddc26e03388214c45e96c.png

用EKF线性化操作近似后

dcef57005831ee6c86476c2f3f3347e6.png

能观性不一致性的原因是因为Jacobian求导（用了不同的线性化点），原本yaw 不可观，变成可观，从而估计器的协方差很小，导致整个系统更新不正确。原文如下

EKF: we prove that, due to the way the EKF Jacobians are computed, even though the IMU’s rotation about gravity (the yaw) is not observable in VIO, it appears to be observable in the linearized system model used by the MSCKF { and the same occurs in EKF-SLAM. Thus, the estimator erroneously believes it has more information than it actually does, and reports a covariance matrix for the state that underestimates the actual one.

Specically, due to the way in which the Jacobians are computed in the EKF, the global orientation appears to be observable in the linearized model, while it is not in the actual system. As a result of this mismatch, the lter produces too small values for the state covariance matrix.

step 2 构造能观性矩阵

0f509b872891374e4bd0c51a8f98f9e5.png

step 3 求取转移矩阵

已知旋转误差项的左乘公式

1e6f76666d6c4dc0a5b7ee05a7294984.png

则，IMU旋转的误差传递关系如下

0d8e2ce1b94ee5b525b00e898b3cd5ef.png

则

ce7264c97ee8e1ec05b4c3992f1dcdf5.png

速度的误差传递关系

e4580246fd13c291eeaa6eb80f26ddfb.png

位置的误差传递关系

2b51534b563bcdd5983796bbc69e90c3.png

整理得到：

42021ae1909925cb104124caee31cc6f.png

019896a933803430caac5584b9135392.png

step 4 求取观测矩阵

观测矩阵

557cdbefc0aea28868a33ec46a7a45c2.png

step 5 构造能观性矩阵

4da361df3314d92ace3cc3e53f52cd54.png

step 6 理想情况下的能观性矩阵和零空间

To derive the “ideal” observability matrix, we evaluate the state transition matrix as Φ(xEk+1; xEk) , and evaluate the Jacobian matrix using the true states.

能观性矩阵

f704b71e7f3f76d079d5232421e79726.png

零空间

25e4521906ed32d6b5080aef546fb4ba.png

可见，维度=4

f6d18ff9dd3a650cb8f6ec60fe9c2216.png

step 7 实际的能观性矩阵和零空间

788fce3ffe3fd5fc2517fbccf8aa9e47.png

其中

bd1f9526aa8af0b333264f6f0e84293f.png

可见，与式35相比，能观性矩阵O 式38中第一项多了一个扰动项ΔΓ，如原文：

The key dierence is that when the Jacobians are evaluated using the state estimates instead of the true states, the disturbance” term ΔΓ appears.

观察这个扰动项中，Eq，Ep和Ev都反映了线性化点不同导致。能观性不一致的原因：

in the original MSCKF dierent estimates of the same states are used for computing Jacobians, and this leads to an infusion of fictitious” information about the yaw. Specically, the use of dierent estimates for the IMU position and velocity result in nonzero values for the disturbance terms ΔΓ, which change the dimension of the nullspace of the observability matrix.

step 7 改进方案

a. 左乘改成“右乘”，调整转移矩阵

这里用了数学上的右乘数值近似，非本质上矩阵的右乘。

c88029f41ca0f0e0fed9cc7e83033234.png

与式24 相比，转移矩阵中与R相关的都去除了。且，观测矩阵仍然为

7036983a6a1602c12b9a68ea9981f8c7.png

带入能观性矩阵，得

c444b81dac8025f18feab73eb08cf060.png

b. FEJ

为了消除上述遇到的Ep，Eq和Ev。原文如下

97b5abf0aa27b153160345ede4292766.png

则：

c55ba8e47f212853504ccbc3b8a7ef5c.png

参考

2012 Improving the accuracy of ekf-based visual-inertial odometry

2013 High-Precision, Consistent EKF-based Visual-Inertial Odometry

博士论文 Visual-Inertial Odometry on Resource-Constrained Systems


二 . OpenVINS 代码中的FEJ的应用



1 保存状态的估计值

1 在Propagator::predict_and_compute预积分后更新MSCKF的IMU状态

state->imu()->set_value(imu_x);

state->imu()->set_fej(imu_x);

2 初始化 时间偏差，IMU到相机外的参数和相机内参数

state->calib_dt_CAMtoIMU()->set_value(temp_camimu_dt);

state->calib_dt_CAMtoIMU()->set_fej(temp_camimu_dt);

state->get_intrinsics_CAM(i)->set_value(cam_calib);

state->get_intrinsics_CAM(i)->set_fej(cam_calib);

state->get_calib_IMUtoCAM(i)->set_value(cam_eigen);

state->get_calib_IMUtoCAM(i)->set_fej(cam_eigen);


3 在Landmark::set_from_xyz设置特征点（5种不同的情况下）

if(isfej) set_fej(p_FinG);

if(isfej) set_fej(p_invFinG);

if(isfej) set_fej(p_invFinA_MSCKF);

这个函数是在UpdaterSLAM::delayed_init中调用，对长期的slam点更新MSCKF状态之后。

if(FeatureRepresentation::is_relative_representation(feat.feat_representation)) {

landmark->_anchor_cam_id = feat.anchor_cam_id;

landmark->_anchor_clone_timestamp = feat.anchor_clone_timestamp;

landmark->set_from_xyz(feat.p_FinA, false);

landmark->set_from_xyz(feat.p_FinA_fej, true);

} else {

landmark->set_from_xyz(feat.p_FinG, false);

landmark->set_from_xyz(feat.p_FinG_fej, true);

}


UpdaterSLAM::perform_anchor_change（若特征点表示方法是局部的情况下调用）

landmark->set_from_xyz(new_feat.p_FinA, false);

landmark->set_from_xyz(new_feat.p_FinA_fej, true);


4 在UpdaterSLAM::delayed_init取长期的特征点作为更新MSCKF 特征点状态

// Save the position and its fej value

if(FeatureRepresentation::is_relative_representation(feat.feat_representation)) {

feat.anchor_cam_id = (*it2)->anchor_cam_id;

feat.anchor_clone_timestamp = (*it2)->anchor_clone_timestamp;

feat.p_FinA = (*it2)->p_FinA;

feat.p_FinA_fej = (*it2)->p_FinA;

} else {

feat.p_FinG = (*it2)->p_FinG;

feat.p_FinG_fej = (*it2)->p_FinG;

}

5 在UpdaterSLAM::update 取slam 特征点集中的landmark 作为更新MSCKF 特征点状态

// Save the position and its fej value

if(FeatureRepresentation::is_relative_representation(feat.feat_representation)) {

feat.anchor_cam_id = landmark->_anchor_cam_id;

feat.anchor_clone_timestamp = landmark->_anchor_clone_timestamp;

feat.p_FinA = landmark->get_xyz(false);

feat.p_FinA_fej = landmark->get_xyz(true);

} else {

feat.p_FinG = landmark->get_xyz(false);

feat.p_FinG_fej = landmark->get_xyz(true);

}

6 在UpdaterMSCKF::update 取传统的msckf特征点集作为更新MSCKF状态

// Save the position and its fej value

if(FeatureRepresentation::is_relative_representation(feat.feat_representation)) {

feat.anchor_cam_id = (*it2)->anchor_cam_id;

feat.anchor_clone_timestamp = (*it2)->anchor_clone_timestamp;

feat.p_FinA = (*it2)->p_FinA;

feat.p_FinA_fej = (*it2)->p_FinA;

} else {

feat.p_FinG = (*it2)->p_FinG;

feat.p_FinG_fej = (*it2)->p_FinG;

}


7 在UpdaterSLAM::perform_anchor_change 的局部坐标系转换需要设置

old_feat.p_FinA_fej = landmark->get_xyz(true);

new_feat.p_FinA_fej = R_OLDtoNEW_fej*landmark->get_xyz(true)+p_OLDinNEW_fej;



2 求解Φ和H Jacobian矩阵的时候使用

1 在UpdaterHelper::get_feature_jacobian_representation 中获取特征点在世界坐标系下的坐标

// Get the feature linearization point

Eigen::Matrix<double,3,1> p_FinG = (state->options().do_fej)? feature.p_FinG_fej : feature.p_FinG;

2 在UpdaterHelper::get_feature_jacobian_representation 中获取特征点的在局部坐标系的坐标，用到IMU到世界坐标系的关系

// If I am doing FEJ, I should FEJ the anchor states (should we fej calibration???)

// Also get the FEJ position of the feature if we are

if(state->options().do_fej) {

// “Best” feature in the global frame

Eigen::Vector3d p_FinG_best = R_GtoI.transpose() * R_ItoC.transpose()*(feature.p_FinA – p_IinC) + p_IinG;

// Transform the best into our anchor frame using FEJ

R_GtoI = state->get_clone(feature.anchor_clone_timestamp)->Rot_fej();

p_IinG = state->get_clone(feature.anchor_clone_timestamp)->pos_fej();

p_FinA = (R_GtoI.transpose()*R_ItoC.transpose()).transpose()*(p_FinG_best – p_IinG) + p_IinC;

}

3 UpdaterHelper::get_feature_jacobian_full 特征点的在局部or全局坐标系下的表示

// Calculate the position of this feature in the global frame FEJ

// If anchored, then we can use the “best” p_FinG since the value of p_FinA does not matter

Eigen::Vector3d p_FinG_fej = feature.p_FinG_fej;

if(FeatureRepresentation::is_relative_representation(feature.feat_representation)) {

p_FinG_fej = p_FinG;

}

4 UpdaterHelper::get_feature_jacobian_full IMU状态到世界坐标系的关系，特征点分别在IMU和相机坐标系下的坐标

// If we are doing first estimate Jacobians, then overwrite with the first estimates

if(state->options().do_fej) {

R_GtoIi = clone_Ii->Rot_fej();

p_IiinG = clone_Ii->pos_fej();

p_FinIi = R_GtoIi*(p_FinG_fej-p_IiinG);

p_FinCi = R_ItoC*p_FinIi+p_IinC;

}

5 Propagator::predict_and_compute 在更新协方差的时候用到

// Now compute Jacobian of new state wrt old state and noise

if (state->options().do_fej) {

// This is the change in the orientation from the end of the last prop to the current prop

// This is needed since we need to include the “k-th” updated orientation information

Eigen::Matrix<double,3,3> Rfej = state->imu()->Rot_fej();

Eigen::Matrix<double,3,3> dR = quat_2_Rot(new_q)*Rfej.transpose(); 和非FEJ不同，这里求的是imu这一旋转量关于世界坐标系的关系


Eigen::Matrix<double,3,1> v_fej = state->imu()->vel_fej();

Eigen::Matrix<double,3,1> p_fej = state->imu()->pos_fej();

F.block(th_id, th_id, 3, 3) = dR;

F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-w_hat * dt) * dt;和非FEJ不同，这里直接用的是上一时刻IMU状态x 相对IMU状态得到的结果

//F.block(th_id, bg_id, 3, 3).noalias() = -dR * Jr_so3(-log_so3(dR)) * dt;

F.block(bg_id, bg_id, 3, 3).setIdentity();

F.block(v_id, th_id, 3, 3).noalias() = -skew_x(new_v-v_fej+_gravity*dt)*Rfej.transpose();

//F.block(v_id, th_id, 3, 3).noalias() = -Rfej.transpose() * skew_x(Rfej*(new_v-v_fej+_gravity*dt)); 和非FEJ不同，这里直接用的速度对称阵x IMU状态得到的结果

F.block(v_id, v_id, 3, 3).setIdentity();

F.block(v_id, ba_id, 3, 3) = -Rfej.transpose() * dt;和非FEJ不同，这里直接用的上一时刻的IMU状态得到的结果

F.block(ba_id, ba_id, 3, 3).setIdentity();

F.block(p_id, th_id, 3, 3).noalias() = -skew_x(new_p-p_fej-v_fej*dt+0.5*_gravity*dt*dt)*Rfej.transpose(); 和非FEJ不同，这里直接用的位置对称阵x IMU状态得到的结果

//F.block(p_id, th_id, 3, 3).noalias() = -0.5 * Rfej.transpose() * skew_x(2*Rfej*(new_p-p_fej-v_fej*dt+0.5*_gravity*dt*dt));

F.block(p_id, v_id, 3, 3) = Eigen::Matrix<double, 3, 3>::Identity() * dt;

F.block(p_id, ba_id, 3, 3) = -0.5 * Rfej.transpose() * dt * dt;和非FEJ不同，这里直接用的上一时刻的IMU状态得到的结果

F.block(p_id, p_id, 3, 3).setIdentity();

G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-w_hat * dt) * dt; 和非FEJ不同，这里直接用的是上一时刻IMU状态x 相对IMU状态得到的结果

//G.block(th_id, 0, 3, 3) = -dR * Jr_so3(-log_so3(dR)) * dt;

G.block(v_id, 3, 3, 3) = -Rfej.transpose() * dt;和非FEJ不同，这里直接用的上一时刻的IMU状态得到的结果

G.block(p_id, 3, 3, 3) = -0.5 * Rfej.transpose() * dt * dt;和非FEJ不同，这里直接用的上一时刻的IMU状态得到的结果

G.block(bg_id, 6, 3, 3) = dt*Eigen::Matrix<double,3,3>::Identity();

G.block(ba_id, 9, 3, 3) = dt*Eigen::Matrix<double,3,3>::Identity();

}