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

minor renaming

release/4.3a0
Luca 2014-12-11 12:20:54 -05:00
parent af04b834b9
commit 295fd7385a
2 changed files with 4 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
gtsam/navigation/PreintegratedRotation.h
View File

@ -96,6 +96,7 @@ public:
}
/// Get so<3> version of bias corrected rotation, with optional Jacobian
// Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated

6
gtsam/navigation/PreintegrationBase.h
View File

@ -266,8 +266,8 @@ public:
// Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5
// H5 will then be corrected below to take into account the Coriolis effect
Matrix3 H5temp;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &H5temp : 0);
Matrix3 D_cThetaRij_biasOmegaIncr;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
@ -351,7 +351,7 @@ public:
}
if(H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * H5temp;
const Matrix3 JbiasOmega = D_cDeltaRij_cOmega * D_cThetaRij_biasOmegaIncr;
H5->resize(9,6);
(*H5) <<
// dfP/dBias