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

fixed evaluate error to compy with additive Gaussian noise model. Still to be optimized, but unit tests pass :-)

release/4.3a0
Luca 2014-12-10 20:51:26 -05:00
parent 1dfd9d2ae7
commit 9dbca87c86
1 changed files with 24 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
gtsam/navigation/PreintegrationBase.h
View File

@ -237,6 +237,7 @@ public:
// we give some shorter name to rotations and translations // we give some shorter name to rotations and translations
const Rot3& Ri = pose_i.rotation(); const Rot3& Ri = pose_i.rotation();
const Rot3& Rj = pose_j.rotation(); const Rot3& Rj = pose_j.rotation();
const Vector3& pos_i = pose_i.translation().vector();
const Vector3& pos_j = pose_j.translation().vector(); const Vector3& pos_j = pose_j.translation().vector();
// Jacobian computation // Jacobian computation
@ -274,25 +275,23 @@ public:
if(H1) { if(H1) {
H1->resize(9,6); H1->resize(9,6);
Matrix3 dfPdPi; Matrix3 dfPdPi = -I_3x3;
Matrix3 dfVdPi; Matrix3 dfVdPi = Z_3x3;
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
dfPdPi = - Ri.matrix() + 0.5 * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij(); dfPdPi += 0.5 * Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij();
dfVdPi = omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij(); dfVdPi += Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij();
}
else{
dfPdPi = - Ri.matrix();
dfVdPi = Z_3x3;
} }
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
Ri.matrix() * skewSymmetric(deltaPij() skewSymmetric( Ri.inverse().matrix() * (pos_j - pos_i - vel_i * deltaTij()
+ delPdelBiasOmega() * biasOmegaIncr + delPdelBiasAcc() * biasAccIncr), + omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
- 0.5 * gravity *deltaTij()*deltaTij())),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
Ri.matrix() * skewSymmetric(deltaVij() skewSymmetric( Ri.inverse().matrix() *
+ delVdelBiasOmega() * biasOmegaIncr + delVdelBiasAcc() * biasAccIncr), (vel_j - vel_i - gravity * deltaTij() + 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
) ),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
@ -304,11 +303,11 @@ public:
H2->resize(9,3); H2->resize(9,3);
(*H2) << (*H2) <<
// dfP/dVi // dfP/dVi
- I_3x3 * deltaTij() - Ri.transpose() * deltaTij()
+ omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper + Ri.transpose() * omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi // dfV/dVi
- I_3x3 - Ri.transpose()
+ 2 * omegaCoriolisHat * deltaTij(), // Coriolis term + 2 * Ri.transpose() * omegaCoriolisHat * deltaTij(), // Coriolis term
// dfR/dVi // dfR/dVi
Z_3x3; Z_3x3;
} }
@ -316,7 +315,7 @@ public:
H3->resize(9,6); H3->resize(9,6);
(*H3) << (*H3) <<
// dfP/dPosej // dfP/dPosej
Z_3x3, Rj.matrix(), Z_3x3, Ri.transpose() * Rj.matrix(),
// dfV/dPosej // dfV/dPosej
Matrix::Zero(3,6), Matrix::Zero(3,6),
// dfR/dPosej // dfR/dPosej
@ -328,7 +327,7 @@ public:
// dfP/dVj // dfP/dVj
Z_3x3, Z_3x3,
// dfV/dVj // dfV/dVj
I_3x3, Ri.transpose(),
// dfR/dVj // dfR/dVj
Z_3x3; Z_3x3;
} }
@ -338,14 +337,11 @@ public:
H5->resize(9,6); H5->resize(9,6);
(*H5) << (*H5) <<
// dfP/dBias // dfP/dBias
- Ri.matrix() * delPdelBiasAcc(), - delPdelBiasAcc(), - delPdelBiasOmega(),
- Ri.matrix() * delPdelBiasOmega(),
// dfV/dBias // dfV/dBias
- Ri.matrix() * delVdelBiasAcc(), - delVdelBiasAcc(), - delVdelBiasOmega(),
- Ri.matrix() * delVdelBiasOmega(),
// dfR/dBias // dfR/dBias
Matrix::Zero(3,3), Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
} }
// Evaluate residual error, according to [3] // Evaluate residual error, according to [3]
@ -353,9 +349,11 @@ public:
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity, PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
omegaCoriolis, use2ndOrderCoriolis); omegaCoriolis, use2ndOrderCoriolis);
const Vector3 fp = pos_j - predictedState_j.pose.translation().vector(); // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fp = Ri.transpose() * ( pos_j - predictedState_j.pose.translation().vector() );
const Vector3 fv = vel_j - predictedState_j.velocity; // Ri.transpose() is important here to preserve a model with *additive* Gaussian noise of correct covariance
const Vector3 fv = Ri.transpose() * ( vel_j - predictedState_j.velocity );
// fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j) // fR was computes earlier. Note: it is the same as: dR = (predictedState_j.pose.translation()).between(Rot_j)