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

Merged in fix/CombinedImuFactor_DiscreteNoise (pull request #182) 

Fix computation of bias covariance from continous-time noise density
release/4.3a0
Frank Dellaert 2015-10-01 10:29:10 -07:00
parent 096effb5d2 efc5025d73
commit 4e5a5cc999
1 changed files with 3 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -107,8 +107,8 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
D_R_R(&G_measCov_Gt) = (1 / deltaT) * (H_angles_biasomega)
* (p().gyroscopeCovariance + p().biasAccOmegaInit.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose());
D_a_a(&G_measCov_Gt) = (1 / deltaT) * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = (1 / deltaT) * p().biasOmegaCovariance;
D_a_a(&G_measCov_Gt) = deltaT * p().biasAccCovariance;
D_g_g(&G_measCov_Gt) = deltaT * p().biasOmegaCovariance;
// OFF BLOCK DIAGONAL TERMS
Matrix3 temp = H_vel_biasacc * p().biasAccOmegaInit.block<3, 3>(3, 0)
@ -270,3 +270,4 @@ void CombinedImuFactor::Predict(const Pose3& pose_i, const Vector3& vel_i,
}
} /// namespace gtsam