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

Moved a derivative down

release/4.3a0
dellaert 2015-07-19 00:40:42 -07:00
parent c72a8344ec
commit 814b8c22bf
1 changed files with 1 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
gtsam/navigation/AHRSFactor.cpp
View File

@ -143,7 +143,6 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
// Coriolis term // Coriolis term
const Vector3 coriolis = _PIM_.integrateCoriolis(Ri); const Vector3 coriolis = _PIM_.integrateCoriolis(Ri);
const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Vector3 correctedOmega = biascorrectedOmega - coriolis; const Vector3 correctedOmega = biascorrectedOmega - coriolis;
// Prediction // Prediction
@ -161,7 +160,7 @@ Vector AHRSFactor::evaluateError(const Rot3& Ri, const Rot3& Rj,
if (H1) { if (H1) {
// dfR/dRi // dfR/dRi
H1->resize(3, 3); H1->resize(3, 3);
Matrix3 D_coriolis = -D_cDeltaRij_cOmega * coriolisHat; Matrix3 D_coriolis = -D_cDeltaRij_cOmega * skewSymmetric(coriolis);
(*H1) (*H1)
<< D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis); << D_fR_fRrot * (-actualRij.transpose() - fRrot.transpose() * D_coriolis);
} }