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

Fixed confusion in naming.

release/4.3a0
krunalchande 2014-12-06 17:42:07 -05:00
parent 4e6534eff7
commit 943a18b124
1 changed files with 7 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam/navigation/PreintegrationBase.h
View File

@ -241,7 +241,8 @@ public:
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration // Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis); const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
const Matrix3 coriolisHat = skewSymmetric(omegaCoriolis); const Matrix3 coriolisHat = skewSymmetric(coriolis);
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
Vector3 theta_corrected = theta_biascorrected - coriolis; Vector3 theta_corrected = theta_biascorrected - coriolis;
// Prediction // Prediction
@ -250,7 +251,7 @@ public:
// TODO: these are not always needed // TODO: these are not always needed
const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj)); const Rot3 fRhat = deltaRij_corrected.between(Ri.between(Rj));
const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected); const Matrix3 Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
const Matrix3 Jtheta = -Jr_theta_bcc * skewSymmetric(Ri.inverse().matrix() * omegaCoriolis * deltaTij()); const Matrix3 Jtheta = -Jr_theta_bcc * coriolisHat;
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
if(H1) { if(H1) {
@ -259,8 +260,8 @@ public:
Matrix3 dfPdPi; Matrix3 dfPdPi;
Matrix3 dfVdPi; Matrix3 dfVdPi;
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
dfPdPi = - Ri.matrix() + 0.5 * coriolisHat * coriolisHat * Ri.matrix() * deltaTij()*deltaTij(); dfPdPi = - Ri.matrix() + 0.5 * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij();
dfVdPi = coriolisHat * coriolisHat * Ri.matrix() * deltaTij(); dfVdPi = omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij();
} }
else{ else{
dfPdPi = - Ri.matrix(); dfPdPi = - Ri.matrix();
@ -287,10 +288,10 @@ public:
(*H2) << (*H2) <<
// dfP/dVi // dfP/dVi
- I_3x3 * deltaTij() - I_3x3 * deltaTij()
+ coriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi // dfV/dVi
- I_3x3 - I_3x3
+ 2 * coriolisHat * deltaTij(), // Coriolis term + 2 * omegaCoriolisHat * deltaTij(), // Coriolis term
// dfR/dVi // dfR/dVi
Z_3x3; Z_3x3;
} }