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

using cross product instead of skewSymm matrix in predict

release/4.3a0
Luca 2014-12-06 19:13:02 -05:00
parent d46224e8a1
commit 792d2656d0
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/PreintegrationBase.h
View File

@ -176,26 +176,24 @@ public:
const Rot3& Rot_i = pose_i.rotation(); const Rot3& Rot_i = pose_i.rotation();
const Vector3& pos_i = pose_i.translation().vector(); const Vector3& pos_i = pose_i.translation().vector();
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij() Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij()
+ delPdelBiasAcc() * biasAccIncr + delPdelBiasAcc() * biasAccIncr
+ delPdelBiasOmega() * biasOmegaIncr) + delPdelBiasOmega() * biasOmegaIncr)
+ vel_i * deltaTij() + vel_i * deltaTij()
- omegaCoriolisHat * vel_i * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper - omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij()*deltaTij(); + 0.5 * gravity * deltaTij()*deltaTij();
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij() Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij()
+ delVdelBiasAcc() * biasAccIncr + delVdelBiasAcc() * biasAccIncr
+ delVdelBiasOmega() * biasOmegaIncr) + delVdelBiasOmega() * biasOmegaIncr)
- 2 * omegaCoriolisHat * vel_i * deltaTij() // Coriolis term - 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
+ gravity * deltaTij()); + gravity * deltaTij());
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
pos_j += - 0.5 * omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij()*deltaTij(); // 2nd order coriolis term for position pos_j += - 0.5 * omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij()*deltaTij(); // 2nd order coriolis term for position
vel_j += - omegaCoriolisHat * omegaCoriolisHat * pos_i * deltaTij(); // 2nd order term for velocity vel_j += - omegaCoriolis.cross(omegaCoriolis.cross(pos_i)) * deltaTij(); // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr); const Rot3 deltaRij_biascorrected = biascorrectedDeltaRij(biasOmegaIncr);