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

minor refactoring

release/4.3a0
Varun Agrawal 2021-09-21 12:30:04 -04:00
parent bbde7b980c
commit a2bf0c4da4
1 changed files with 10 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -123,6 +123,9 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
F.block<3, 3>(6, 9) = vel_H_biasAcc; F.block<3, 3>(6, 9) = vel_H_biasAcc;
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]).
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
// propagate uncertainty // propagate uncertainty
// TODO(frank): use noiseModel routine so we can have arbitrary noise models. // TODO(frank): use noiseModel routine so we can have arbitrary noise models.
const Matrix3& aCov = p().accelerometerCovariance; const Matrix3& aCov = p().accelerometerCovariance;
@ -138,15 +141,15 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3); Matrix3 wCov_updated = wCov + p().biasAccOmegaInt.block<3, 3>(3, 3);
// BLOCK DIAGONAL TERMS // BLOCK DIAGONAL TERMS
D_t_t(&G_measCov_Gt) = (pos_H_biasAcc D_t_t(&G_measCov_Gt) = (pos_H_biasAcc //
* (aCov / dt) // aCov_updated / dt * (aCov_updated / dt) //
* pos_H_biasAcc.transpose()) + (dt * iCov); * pos_H_biasAcc.transpose()) + (dt * iCov);
D_v_v(&G_measCov_Gt) = vel_H_biasAcc D_v_v(&G_measCov_Gt) = vel_H_biasAcc //
* (aCov / dt) // aCov_updated / dt * (aCov_updated / dt) //
* (vel_H_biasAcc.transpose()); * (vel_H_biasAcc.transpose());
D_R_R(&G_measCov_Gt) = theta_H_biasOmega D_R_R(&G_measCov_Gt) = theta_H_biasOmega //
* (wCov / dt) // wCov_updated / dt * (wCov_updated / dt) //
* (theta_H_biasOmega.transpose()); * (theta_H_biasOmega.transpose());
D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance; D_a_a(&G_measCov_Gt) = dt * p().biasAccCovariance;
@ -160,7 +163,7 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
D_R_v(&G_measCov_Gt) = temp.transpose(); D_R_v(&G_measCov_Gt) = temp.transpose();
D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose(); D_t_v(&G_measCov_Gt) = pos_H_biasAcc * (aCov / dt) * vel_H_biasAcc.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; preintMeasCov_.noalias() += G_measCov_Gt;
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------