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

Fix the noise covariances to conform to new error order...

release/4.3a0
dellaert 2015-07-29 15:43:42 +02:00
parent 96c139ac87
commit 5b9bf9affa
4 changed files with 55 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
gtsam/navigation/ImuFactor.cpp
View File

@ -48,6 +48,16 @@ void PreintegratedImuMeasurements::resetIntegration() {
preintMeasCov_.setZero();
}
// sugar for derivative blocks
#define D_R_R(H) (H)->block<3,3>(0,0)
#define D_R_t(H) (H)->block<3,3>(0,3)
#define D_R_v(H) (H)->block<3,3>(0,6)
#define D_t_R(H) (H)->block<3,3>(3,0)
#define D_t_t(H) (H)->block<3,3>(3,3)
#define D_t_v(H) (H)->block<3,3>(3,6)
#define D_v_R(H) (H)->block<3,3>(6,0)
#define D_v_t(H) (H)->block<3,3>(6,3)
#define D_v_v(H) (H)->block<3,3>(6,6)
//------------------------------------------------------------------------------
void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
@ -81,20 +91,19 @@ void PreintegratedImuMeasurements::integrateMeasurement(
// NOTE 2: computation of G * (1/deltaT) * measurementCovariance * G.transpose() done block-wise,
// as G and measurementCovariance are block-diagonal matrices
preintMeasCov_ = F * preintMeasCov_ * F.transpose();
preintMeasCov_.block<3, 3>(0, 0) += p().integrationCovariance * deltaT;
preintMeasCov_.block<3, 3>(3, 3) += dRij * p().accelerometerCovariance
* dRij.transpose() * deltaT;
preintMeasCov_.block<3, 3>(6, 6) += D_Rincr_integratedOmega
* p().gyroscopeCovariance * D_Rincr_integratedOmega.transpose() * deltaT;
D_t_t(&preintMeasCov_) += p().integrationCovariance * deltaT;
D_v_v(&preintMeasCov_) += dRij * p().accelerometerCovariance * dRij.transpose() * deltaT;
D_R_R(&preintMeasCov_) += D_Rincr_integratedOmega * p().gyroscopeCovariance
* D_Rincr_integratedOmega.transpose() * deltaT;
Matrix3 F_pos_noiseacc;
F_pos_noiseacc = 0.5 * dRij * deltaT * deltaT;
preintMeasCov_.block<3, 3>(0, 0) += (1 / deltaT) * F_pos_noiseacc
D_t_t(&preintMeasCov_) += (1 / deltaT) * F_pos_noiseacc
* p().accelerometerCovariance * F_pos_noiseacc.transpose();
Matrix3 temp = F_pos_noiseacc * p().accelerometerCovariance
* dRij.transpose(); // has 1/deltaT
preintMeasCov_.block<3, 3>(0, 3) += temp;
preintMeasCov_.block<3, 3>(3, 0) += temp.transpose();
D_t_v(&preintMeasCov_) += temp;
D_v_t(&preintMeasCov_) += temp.transpose();
// F_test and G_test are given as output for testing purposes and are not needed by the factor
if (F_test) {
@ -102,10 +111,11 @@ void PreintegratedImuMeasurements::integrateMeasurement(
}
if (G_test) {
// This in only for testing & documentation, while the actual computation is done block-wise
// intNoise accNoise omegaNoise
(*G_test) << I_3x3 * deltaT, F_pos_noiseacc, Z_3x3, // pos
Z_3x3, dRij * deltaT, Z_3x3, // vel
Z_3x3, Z_3x3, D_Rincr_integratedOmega * deltaT; // angle
// omegaNoise intNoise accNoise
(*G_test) <<
D_Rincr_integratedOmega * deltaT, Z_3x3, Z_3x3, // angle
Z_3x3, I_3x3 * deltaT, F_pos_noiseacc, // pos
Z_3x3, Z_3x3, dRij * deltaT; // vel
}
}
//------------------------------------------------------------------------------

2
gtsam/navigation/ImuFactor.h
View File

@ -63,7 +63,7 @@ class PreintegratedImuMeasurements: public PreintegrationBase {
protected:
Eigen::Matrix<double, 9, 9> preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
Matrix9 preintMeasCov_; ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION]
///< (first-order propagation from *measurementCovariance*).
/// Default constructor for serialization

27
gtsam/navigation/PreintegrationBase.cpp
View File

@ -63,25 +63,22 @@ void PreintegrationBase::updatePreintegratedMeasurements(
const Vector3& correctedAcc, const Rot3& incrR, const double deltaT,
OptionalJacobian<9, 9> F) {
const Matrix3 dRij = deltaRij_.matrix(); // expensive
const Vector3 i_acc = dRij * correctedAcc; // acceleration in i frame
// Calculate acceleration in *current* i frame, i.e., before rotation update below
Matrix3 D_acc_R;
const Vector3 i_acc = deltaRij_.rotate(correctedAcc, F? &D_acc_R : 0);
double dt22 = 0.5 * deltaT * deltaT;
deltaPij_ += deltaVij_ * deltaT + dt22 * i_acc;
deltaVij_ += deltaT * i_acc;
Matrix3 R_i, F_angles_angles;
Matrix3 F_angles_angles;
updateIntegratedRotationAndDeltaT(incrR, deltaT, F ? &F_angles_angles : 0);
if (F) {
const Matrix3 F_vel_angles = -dRij * skewSymmetric(correctedAcc) * deltaT;
Matrix3 F_pos_angles;
double dt22 = 0.5 * deltaT * deltaT;
deltaPij_ += dt22 * i_acc + deltaT * deltaVij_;
deltaVij_ += deltaT * i_acc;
// pos vel angle
*F << //
I_3x3, I_3x3 * deltaT, 0.5 * F_vel_angles * deltaT, // pos
Z_3x3, I_3x3, F_vel_angles, // vel
Z_3x3, Z_3x3, F_angles_angles; // angle
if (F) {
*F << // angle pos vel
F_angles_angles, Z_3x3, Z_3x3, // angle
dt22 * D_acc_R, I_3x3, I_3x3 * deltaT, // pos
deltaT * D_acc_R, Z_3x3, I_3x3; // vel
}
}

46
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -628,18 +628,15 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix FexpectedTop6(6, 9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9, 9);
Fexpected << FexpectedTop6, FexpectedBottom3;
Fexpected << //
dfr_dangle, Z_3x3, Z_3x3, //
dfpv_dangle, dfpv_dpos, dfpv_dvel;
EXPECT(assert_equal(Fexpected, Factual));
@ -659,25 +656,25 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9, 9);
Gexpected << GexpectedTop6, GexpectedBottom3;
Gexpected << //
dgr_dangle, Z_3x3, Z_3x3, //
dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
* I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
measurementCovariance << //
omegaNoiseVar * I_3x3, Z_3x3, Z_3x3, //
Z_3x3, intNoiseVar * I_3x3, Z_3x3, //
Z_3x3, Z_3x3, accNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose()
@ -745,18 +742,15 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
Matrix FexpectedTop6(6, 9);
FexpectedTop6 << dfpv_dpos, dfpv_dvel, dfpv_dangle;
// Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix FexpectedBottom3(3, 9);
FexpectedBottom3 << Z_3x3, Z_3x3, dfr_dangle;
Matrix Fexpected(9, 9);
Fexpected << FexpectedTop6, FexpectedBottom3;
Fexpected << //
dfr_dangle, Z_3x3, Z_3x3, //
dfpv_dangle, dfpv_dpos, dfpv_dvel;
EXPECT(assert_equal(Fexpected, Factual));
@ -776,25 +770,25 @@ TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
Matrix GexpectedTop6(6, 9);
GexpectedTop6 << dgpv_dintNoise, dgpv_daccNoise, dgpv_domegaNoise;
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix GexpectedBottom3(3, 9);
GexpectedBottom3 << Z_3x3, Z_3x3, dgr_dangle;
Matrix Gexpected(9, 9);
Gexpected << GexpectedTop6, GexpectedBottom3;
Gexpected << //
dgr_dangle, Z_3x3, Z_3x3, //
dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
measurementCovariance << intNoiseVar * I_3x3, Z_3x3, Z_3x3, Z_3x3, accNoiseVar
* I_3x3, Z_3x3, Z_3x3, Z_3x3, omegaNoiseVar * I_3x3;
measurementCovariance << //
omegaNoiseVar * I_3x3, Z_3x3, Z_3x3, //
Z_3x3, intNoiseVar * I_3x3, Z_3x3, //
Z_3x3, Z_3x3, accNoiseVar * I_3x3;
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose()