12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

detailed implementation of CombinedImuFactor noise propagation

release/4.3a0
Varun Agrawal 2021-09-28 18:22:15 -04:00
parent af714cdb5a
commit e38ea502d7
1 changed files with 44 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

68
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -110,17 +110,22 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
// and preintegrated measurements // and preintegrated measurements
// Single Jacobians to propagate covariance // Single Jacobians to propagate covariance
Matrix3 theta_H_biasOmega = C.topRows<3>(); Matrix3 theta_H_omega = C.topRows<3>();
Matrix3 pos_H_biasAcc = B.middleRows<3>(3); Matrix3 pos_H_acc = B.middleRows<3>(3);
Matrix3 vel_H_biasAcc = B.bottomRows<3>(); Matrix3 vel_H_acc = B.bottomRows<3>();
Matrix3 theta_H_biasOmegaInit = -theta_H_omega;
Matrix3 pos_H_biasAccInit = -pos_H_acc;
Matrix3 vel_H_biasAcc = -vel_H_acc;
Matrix3 vel_H_biasAccInit = -vel_H_acc;
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Eigen::Matrix<double, 15, 15> F; Eigen::Matrix<double, 15, 15> F;
F.setZero(); F.setZero();
F.block<9, 9>(0, 0) = A; F.block<9, 9>(0, 0) = A;
F.block<3, 3>(0, 12) = theta_H_biasOmega; F.block<3, 3>(0, 12) = theta_H_omega;
F.block<3, 3>(3, 9) = pos_H_biasAcc; F.block<3, 3>(3, 9) = pos_H_acc;
F.block<3, 3>(6, 9) = vel_H_biasAcc; F.block<3, 3>(6, 9) = vel_H_acc;
F.block<6, 6>(9, 9) = I_6x6; F.block<6, 6>(9, 9) = I_6x6;
// Update the uncertainty on the state (matrix F in [4]). // Update the uncertainty on the state (matrix F in [4]).
@ -131,37 +136,52 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
const Matrix3& aCov = p().accelerometerCovariance; const Matrix3& aCov = p().accelerometerCovariance;
const Matrix3& wCov = p().gyroscopeCovariance; const Matrix3& wCov = p().gyroscopeCovariance;
const Matrix3& iCov = p().integrationCovariance; const Matrix3& iCov = p().integrationCovariance;
const Matrix6& bInitCov = p().biasAccOmegaInit;
// first order uncertainty propagation // first order uncertainty propagation
// Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose() // Optimized matrix mult: (1/dt) * G * measurementCovariance * G.transpose()
Eigen::Matrix<double, 15, 15> G_measCov_Gt; Eigen::Matrix<double, 15, 15> G_measCov_Gt;
G_measCov_Gt.setZero(15, 15); G_measCov_Gt.setZero(15, 15);
Matrix3 aCov_updated = aCov + p().biasAccOmegaInit.block<3, 3>(0, 0);
Matrix3 wCov_updated = wCov + p().biasAccOmegaInit.block<3, 3>(3, 3);
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = (pos_H_biasAcc // D_R_R(&G_measCov_Gt) =
* (aCov_updated / dt) // (theta_H_omega * (wCov / dt) * theta_H_omega.transpose()) + //
* pos_H_biasAcc.transpose()) + (dt * iCov); (theta_H_biasOmegaInit * (bInitCov.block<3, 3>(3, 3) / dt) *
D_v_v(&G_measCov_Gt) = vel_H_biasAcc // theta_H_biasOmegaInit.transpose());
* (aCov_updated / dt) //
* (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = theta_H_biasOmega // D_t_t(&G_measCov_Gt) =
* (wCov_updated / dt) // (pos_H_acc * (aCov / dt) * pos_H_acc.transpose()) //
* (theta_H_biasOmega.transpose()); + (pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
pos_H_biasAccInit.transpose()) //
+ (dt * iCov);
D_v_v(&G_measCov_Gt) =
(vel_H_acc * (aCov / dt) * vel_H_acc.transpose()) //
+ (vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
vel_H_biasAccInit.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance; D_g_g(&G_measCov_Gt) = dt * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 temp = vel_H_biasAcc * p().biasAccOmegaInit.block<3, 3>(3, 0) D_R_t(&G_measCov_Gt) = theta_H_biasOmegaInit *
* theta_H_biasOmega.transpose(); (bInitCov.block<3, 3>(3, 0) / dt) *
D_v_R(&G_measCov_Gt) = temp; pos_H_biasAccInit.transpose();
D_v_t(&G_measCov_Gt) = vel_H_biasAcc * (aCov / dt) * pos_H_biasAcc.transpose(); D_R_v(&G_measCov_Gt) = theta_H_biasOmegaInit *
D_R_v(&G_measCov_Gt) = temp.transpose(); (bInitCov.block<3, 3>(3, 0) / dt) *
D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose(); vel_H_biasAccInit.transpose();
D_t_R(&G_measCov_Gt) = pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) *
theta_H_biasOmegaInit.transpose();
D_t_v(&G_measCov_Gt) =
(pos_H_acc * (aCov / dt) * vel_H_acc.transpose()) +
(pos_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
vel_H_biasAccInit.transpose());
D_v_R(&G_measCov_Gt) = vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 3) / dt) *
theta_H_biasOmegaInit.transpose();
D_v_t(&G_measCov_Gt) =
(vel_H_acc * (aCov / dt) * pos_H_acc.transpose()) +
(vel_H_biasAccInit * (bInitCov.block<3, 3>(0, 0) / dt) *
pos_H_biasAccInit.transpose());
preintMeasCov_.noalias() += G_measCov_Gt; preintMeasCov_.noalias() += G_measCov_Gt;
} }