added unit test, improved computation
Luca
parent
9dbca87c86
commit
c4fafd9268
|
|
@ -178,7 +178,9 @@ public:
|
|||
//------------------------------------------------------------------------------
|
||||
PoseVelocityBias predict(const Pose3& pose_i, const Vector3& vel_i,
|
||||
const imuBias::ConstantBias& bias_i, const Vector3& gravity,
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false) const {
|
||||
const Vector3& omegaCoriolis, const bool use2ndOrderCoriolis = false,
|
||||
boost::optional<Vector3&> deltaPij_biascorrected_out = boost::none,
|
||||
boost::optional<Vector3&> deltaVij_biascorrected_out = boost::none) const {
|
||||
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
|
||||
|
|
@ -188,16 +190,20 @@ public:
|
|||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (deltaPij()
|
||||
+ delPdelBiasAcc() * biasAccIncr
|
||||
+ delPdelBiasOmega() * biasOmegaIncr)
|
||||
Vector3 deltaPij_biascorrected = deltaPij() + delPdelBiasAcc() * biasAccIncr + delPdelBiasOmega() * biasOmegaIncr;
|
||||
if(deltaPij_biascorrected_out)// if desired, store this
|
||||
*deltaPij_biascorrected_out = deltaPij_biascorrected;
|
||||
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * deltaPij_biascorrected
|
||||
+ vel_i * deltaTij()
|
||||
- omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij()*deltaTij();
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * (deltaVij()
|
||||
+ delVdelBiasAcc() * biasAccIncr
|
||||
+ delVdelBiasOmega() * biasOmegaIncr)
|
||||
Vector3 deltaVij_biascorrected = deltaVij() + delVdelBiasAcc() * biasAccIncr + delVdelBiasOmega() * biasOmegaIncr;
|
||||
if(deltaVij_biascorrected_out)// if desired, store this
|
||||
*deltaVij_biascorrected_out = deltaVij_biascorrected;
|
||||
|
||||
Vector3 vel_j = Vector3(vel_i + Rot_i.matrix() * deltaVij_biascorrected
|
||||
- 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
|
||||
+ gravity * deltaTij());
|
||||
|
||||
|
|
@ -231,15 +237,28 @@ public:
|
|||
boost::optional<Matrix&> H5) const {
|
||||
|
||||
// We need the mismatch w.r.t. the biases used for preintegration
|
||||
const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer();
|
||||
// const Vector3 biasAccIncr = bias_i.accelerometer() - biasHat().accelerometer(); // this is not necessary
|
||||
const Vector3 biasOmegaIncr = bias_i.gyroscope() - biasHat().gyroscope();
|
||||
|
||||
// we give some shorter name to rotations and translations
|
||||
const Rot3& Ri = pose_i.rotation();
|
||||
const Rot3& Rj = pose_j.rotation();
|
||||
const Vector3& pos_i = pose_i.translation().vector();
|
||||
const Vector3& pos_j = pose_j.translation().vector();
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 deltaPij_biascorrected, deltaVij_biascorrected;
|
||||
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
|
||||
omegaCoriolis, use2ndOrderCoriolis, deltaPij_biascorrected, deltaVij_biascorrected);
|
||||
|
||||
// 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() );
|
||||
|
||||
// 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 will be computed later. Note: it is the same as: fR = (predictedState_j.pose.translation()).between(Rot_j)
|
||||
|
||||
// Jacobian computation
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
// Get Get so<3> version of bias corrected rotation
|
||||
|
|
@ -248,7 +267,7 @@ public:
|
|||
Vector3 biascorrectedOmega = biascorrectedThetaRij(biasOmegaIncr, H5);
|
||||
|
||||
// Coriolis term, note inconsistent with AHRS, where coriolisHat is *after* integration
|
||||
const Matrix3 omegaCoriolisHat = skewSymmetric(omegaCoriolis);
|
||||
const Matrix3 Ritranspose_omegaCoriolisHat = Ri.transpose() * skewSymmetric(omegaCoriolis);
|
||||
const Vector3 coriolis = integrateCoriolis(Ri, omegaCoriolis);
|
||||
Vector3 correctedOmega = biascorrectedOmega - coriolis;
|
||||
|
||||
|
|
@ -271,27 +290,23 @@ public:
|
|||
fRrot = correctedDeltaRij.between(Ri.between(Rj));
|
||||
fR = Rot3::Logmap(fRrot);
|
||||
}
|
||||
|
||||
if(H1) {
|
||||
H1->resize(9,6);
|
||||
|
||||
Matrix3 dfPdPi = -I_3x3;
|
||||
Matrix3 dfVdPi = Z_3x3;
|
||||
if(use2ndOrderCoriolis){
|
||||
dfPdPi += 0.5 * Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij()*deltaTij();
|
||||
dfVdPi += Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix() * deltaTij();
|
||||
// this is the same as: Ri.transpose() * omegaCoriolisHat * omegaCoriolisHat * Ri.matrix()
|
||||
Matrix3 temp = Ritranspose_omegaCoriolisHat * (-Ritranspose_omegaCoriolisHat.transpose());
|
||||
dfPdPi += 0.5 * temp * deltaTij()*deltaTij();
|
||||
dfVdPi += temp * deltaTij();
|
||||
}
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
skewSymmetric( Ri.inverse().matrix() * (pos_j - pos_i - vel_i * deltaTij()
|
||||
+ omegaCoriolis.cross(vel_i) * deltaTij()*deltaTij() // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
- 0.5 * gravity *deltaTij()*deltaTij())),
|
||||
skewSymmetric(fp + deltaPij_biascorrected),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
skewSymmetric( Ri.inverse().matrix() *
|
||||
(vel_j - vel_i - gravity * deltaTij() + 2 * omegaCoriolis.cross(vel_i) * deltaTij() // Coriolis term
|
||||
) ),
|
||||
skewSymmetric(fv + deltaVij_biascorrected),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
|
|
@ -304,10 +319,10 @@ public:
|
|||
(*H2) <<
|
||||
// dfP/dVi
|
||||
- Ri.transpose() * deltaTij()
|
||||
+ Ri.transpose() * omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ Ritranspose_omegaCoriolisHat * deltaTij() * deltaTij(), // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- Ri.transpose()
|
||||
+ 2 * Ri.transpose() * omegaCoriolisHat * deltaTij(), // Coriolis term
|
||||
+ 2 * Ritranspose_omegaCoriolisHat * deltaTij(), // Coriolis term
|
||||
// dfR/dVi
|
||||
Z_3x3;
|
||||
}
|
||||
|
|
@ -343,20 +358,6 @@ public:
|
|||
// dfR/dBias
|
||||
Z_3x3, D_fR_fRrot * ( - fRrot.inverse().matrix() * JbiasOmega);
|
||||
}
|
||||
|
||||
// Evaluate residual error, according to [3]
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
PoseVelocityBias predictedState_j = predict(pose_i, vel_i, bias_i, gravity,
|
||||
omegaCoriolis, use2ndOrderCoriolis);
|
||||
|
||||
// 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() );
|
||||
|
||||
// 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)
|
||||
|
||||
Vector r(9); r << fp, fv, fR;
|
||||
return r;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -315,6 +315,71 @@ TEST( ImuFactor, ErrorAndJacobianWithBiases )
|
|||
EXPECT(assert_equal(H5e, H5a));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, ErrorAndJacobianWith2ndOrderCoriolis )
|
||||
{
|
||||
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0.1, 0, 0.3)); // Biases (acc, rot)
|
||||
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/10.0), Point3(5.0, 1.0, -50.0));
|
||||
Vector3 v1(Vector3(0.5, 0.0, 0.0));
|
||||
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/10.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
|
||||
Vector3 v2(Vector3(0.5, 0.0, 0.0));
|
||||
|
||||
// Measurements
|
||||
Vector3 gravity; gravity << 0, 0, 9.81;
|
||||
Vector3 omegaCoriolis; omegaCoriolis << 0, 0.1, 0.1;
|
||||
Vector3 measuredOmega; measuredOmega << 0, 0, M_PI/10.0+0.3;
|
||||
Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector() + Vector3(0.2,0.0,0.0);
|
||||
double deltaT = 1.0;
|
||||
|
||||
ImuFactor::PreintegratedMeasurements pre_int_data(imuBias::ConstantBias(Vector3(0.2, 0.0, 0.0),
|
||||
Vector3(0.0, 0.0, 0.1)), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero());
|
||||
pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT);
|
||||
|
||||
// Create factor
|
||||
Pose3 bodyPsensor = Pose3();
|
||||
bool use2ndOrderCoriolis = true;
|
||||
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis, bodyPsensor, use2ndOrderCoriolis);
|
||||
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
Vector errorActual = factor.evaluateError(x1, v1, x2, v2, bias);
|
||||
SETDEBUG("ImuFactor evaluateError", false);
|
||||
|
||||
// Expected error (should not be zero in this test, as we want to evaluate Jacobians
|
||||
// at a nontrivial linearization point)
|
||||
// Vector errorExpected(9); errorExpected << 0, 0, 0, 0, 0, 0, 0, 0, 0;
|
||||
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
|
||||
|
||||
// Expected Jacobians
|
||||
Matrix H1e = numericalDerivative11<Vector,Pose3>(
|
||||
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
|
||||
Matrix H2e = numericalDerivative11<Vector,Vector3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
|
||||
Matrix H3e = numericalDerivative11<Vector,Pose3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
|
||||
Matrix H4e = numericalDerivative11<Vector,Vector3>(
|
||||
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
|
||||
Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
|
||||
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
|
||||
|
||||
// Check rotation Jacobians
|
||||
Matrix RH1e = numericalDerivative11<Rot3,Pose3>(
|
||||
boost::bind(&evaluateRotationError, factor, _1, v1, x2, v2, bias), x1);
|
||||
Matrix RH3e = numericalDerivative11<Rot3,Pose3>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, v1, _1, v2, bias), x2);
|
||||
Matrix RH5e = numericalDerivative11<Rot3,imuBias::ConstantBias>(
|
||||
boost::bind(&evaluateRotationError, factor, x1, v1, x2, v2, _1), bias);
|
||||
|
||||
// Actual Jacobians
|
||||
Matrix H1a, H2a, H3a, H4a, H5a;
|
||||
(void) factor.evaluateError(x1, v1, x2, v2, bias, H1a, H2a, H3a, H4a, H5a);
|
||||
|
||||
EXPECT(assert_equal(H1e, H1a));
|
||||
EXPECT(assert_equal(H2e, H2a));
|
||||
EXPECT(assert_equal(H3e, H3a));
|
||||
EXPECT(assert_equal(H4e, H4a));
|
||||
EXPECT(assert_equal(H5e, H5a));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ImuFactor, PartialDerivative_wrt_Bias )
|
||||
{
|
||||
|
|
|
|||
Loading…
Reference in New Issue