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

Fix computation of bias covariance from continous-time noise density (issue #252).

release/4.3a0
Christian Forster 2015-08-21 18:00:00 +02:00
parent 2146eb1b45
commit 3ce789e577
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -133,8 +133,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega) G_measCov_Gt.block<3, 3>(6, 6) = (1 / deltaT) * (H_angles_biasomega)
* (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3)) * (gyroscopeCovariance() + biasAccOmegaInit_.block<3, 3>(3, 3))
* (H_angles_biasomega.transpose()); * (H_angles_biasomega.transpose());
G_measCov_Gt.block<3, 3>(9, 9) = (1 / deltaT) * biasAccCovariance_; G_measCov_Gt.block<3, 3>(9, 9) = deltaT * biasAccCovariance_;
G_measCov_Gt.block<3, 3>(12, 12) = (1 / deltaT) * biasOmegaCovariance_; G_measCov_Gt.block<3, 3>(12, 12) = deltaT * biasOmegaCovariance_;
// OFF BLOCK DIAGONAL TERMS // OFF BLOCK DIAGONAL TERMS
Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0) Matrix3 block23 = H_vel_biasacc * biasAccOmegaInit_.block<3, 3>(3, 0)
* H_angles_biasomega.transpose(); * H_angles_biasomega.transpose();