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

addressed some of Frank's comments

release/4.3a0
Luca 2014-12-07 15:03:48 -05:00
parent aee20d669d
commit ab54ca1697
2 changed files with 14 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/ImuFactor.cpp
View File

@ -122,13 +122,13 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
if(G_test){ if(G_test){
// Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT // Extended version, without approximation: Gt * Qt * G =(approx)= measurementCovariance_contTime * deltaT
// This in only for testing // This in only for testing
G_test->resize(9,9); G_test->resize(9,9);
// intNoise accNoise omegaNoise // intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos (*G_test) << I_3x3 * deltaT, Z_3x3, Z_3x3, // pos
Z_3x3, R_i * deltaT, Z_3x3, // vel Z_3x3, R_i * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle Z_3x3, Z_3x3, Jrinv_theta_j * Jr_theta_incr * deltaT;// angle
//preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose(); // Propagation with no approximation:
// preintMeasCov = F * preintMeasCov * F.transpose() + G_test * (1/deltaT) * measurementCovariance * G_test.transpose();
} }
} }

15
gtsam/navigation/PreintegrationBase.h
View File

@ -125,7 +125,6 @@ public:
deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT; deltaPij_ += deltaVij_ * deltaT + 0.5 * temp * deltaT;
} }
deltaVij_ += temp; deltaVij_ += temp;
// TODO: we update rotation *after* the others. Is that correct?
updateIntegratedRotationAndDeltaT(incrR,deltaT); updateIntegratedRotationAndDeltaT(incrR,deltaT);
} }
@ -143,7 +142,6 @@ public:
} }
delVdelBiasAcc_ += -dRij * deltaT; delVdelBiasAcc_ += -dRij * deltaT;
delVdelBiasOmega_ += temp; delVdelBiasOmega_ += temp;
// TODO: we update rotation *after* the others. Is that correct?
update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT); update_delRdelBiasOmega(Jr_theta_incr,incrR,deltaT);
} }
@ -244,11 +242,16 @@ public:
// Prediction // Prediction
const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected ); const Rot3 deltaRij_corrected = Rot3::Expmap( theta_corrected );
// TODO: these are not always needed // Residual rotation error
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 Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis); // Accessory matrix, used to build the jacobians
const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); Matrix3 Jr_theta_bcc, Jtheta, Jrinv_fRhat;
if(H1 || H3 || H5){
Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat));
Jr_theta_bcc = Rot3::rightJacobianExpMapSO3(theta_corrected);
Jtheta = -Jr_theta_bcc * skewSymmetric(coriolis);
}
if(H1) { if(H1) {
H1->resize(9,6); H1->resize(9,6);