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 /// Get so<3> version of bias corrected rotation, with optional Jacobian
// Implements: log( deltaRij_ * expmap(delRdelBiasOmega_ * biasOmegaIncr) )
Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr, Vector3 biascorrectedThetaRij(const Vector3& biasOmegaIncr,
OptionalJacobian<3, 3> H = boost::none) const { OptionalJacobian<3, 3> H = boost::none) const {
// First, we correct deltaRij using the biasOmegaIncr, rotated // 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 // Get Get so<3> version of bias corrected rotation
// If H5 is asked for, we will need the Jacobian, which we store in H5 // 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 // H5 will then be corrected below to take into account the Coriolis effect
Matrix3 H5temp; Matrix3 D_cThetaRij_biasOmegaIncr;
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &H5temp : 0); Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5 ? &D_cThetaRij_biasOmegaIncr : 0);
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis); const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
@ -351,7 +351,7 @@ public:
} }
if(H5) { if(H5) {
// H5 by this point already contains 3*3 biascorrectedThetaRij derivative // 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->resize(9,6);
(*H5) << (*H5) <<
// dfP/dBias // dfP/dBias