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

Removed F/G tests: derivatives no longer matched and are checked at a lower level anyways.

release/4.3a0
dellaert 2015-07-31 15:42:22 -07:00
parent 7901077a7a
commit b26bfb27ac
6 changed files with 7 additions and 472 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -66,8 +66,7 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegratedCombinedMeasurements::integrateMeasurement( void PreintegratedCombinedMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT, const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {
OptionalJacobian<15, 15> F_test, OptionalJacobian<15, 21> G_test) {
const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
@ -117,21 +116,6 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
D_v_R(&G_measCov_Gt) = temp; D_v_R(&G_measCov_Gt) = temp;
D_R_v(&G_measCov_Gt) = temp.transpose(); D_R_v(&G_measCov_Gt) = temp.transpose();
preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt; preintMeasCov_ = F * preintMeasCov_ * F.transpose() + G_measCov_Gt;
// F_test and G_test are used for testing purposes and are not needed by the factor
if (F_test) {
(*F_test) << F;
}
if (G_test) {
// This is for testing & documentation
///< measurementCovariance_ : cov[integrationError measuredAcc measuredOmega biasAccRandomWalk biasOmegaRandomWalk biasAccInit biasOmegaInit] in R^(21 x 21)
(*G_test) << //
-H_angles_biasomega, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, H_angles_biasomega, //
Z_3x3, I_3x3 * deltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, -H_vel_biasacc, Z_3x3, Z_3x3, H_vel_biasacc, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3, Z_3x3, //
Z_3x3, Z_3x3, Z_3x3, Z_3x3, I_3x3, Z_3x3, Z_3x3;
}
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------

4
gtsam/navigation/CombinedImuFactor.h
View File

@ -143,9 +143,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
* frame) * frame)
*/ */
void integrateMeasurement(const Vector3& measuredAcc, void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double deltaT, const Vector3& measuredOmega, double deltaT);
OptionalJacobian<15, 15> F_test = boost::none,
OptionalJacobian<15, 21> G_test = boost::none);
/// methods to access class variables /// methods to access class variables
Matrix preintMeasCov() const { return preintMeasCov_; } Matrix preintMeasCov() const { return preintMeasCov_; }

21
gtsam/navigation/ImuFactor.cpp
View File

@ -48,21 +48,9 @@ void PreintegratedImuMeasurements::resetIntegration() {
preintMeasCov_.setZero(); 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( void PreintegratedImuMeasurements::integrateMeasurement(
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt, const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
OptionalJacobian<9, 9> outF, OptionalJacobian<9, 9> G) {
static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished(); static const Matrix93 Gi = (Matrix93() << Z_3x3, I_3x3, Z_3x3).finished();
@ -70,7 +58,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx) Matrix9 F; // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix93 G1, G2; Matrix93 G1, G2;
Matrix3 D_incrR_integratedOmega; Matrix3 D_incrR_integratedOmega;
updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt, &D_incrR_integratedOmega, &F, &G1, &G2); updatePreintegratedMeasurements(measuredAcc, measuredOmega, dt,
&D_incrR_integratedOmega, &F, &G1, &G2);
// first order covariance propagation: // first order covariance propagation:
// as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF // as in [2] we consider a first order propagation that can be seen as a prediction phase in EKF
@ -82,10 +71,8 @@ void PreintegratedImuMeasurements::integrateMeasurement(
+ G1 * (p().accelerometerCovariance / dt) * G1.transpose() + G1 * (p().accelerometerCovariance / dt) * G1.transpose()
+ Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt) + Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
+ G2 * (p().gyroscopeCovariance / dt) * G2.transpose(); + G2 * (p().gyroscopeCovariance / dt) * G2.transpose();
if (outF) *outF = F;
if (G) *G << G2, Gi*dt, G1; // NOTE(frank): order here is R,P,V
} }
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
PreintegratedImuMeasurements::PreintegratedImuMeasurements( PreintegratedImuMeasurements::PreintegratedImuMeasurements(
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance, const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,

5
gtsam/navigation/ImuFactor.h
View File

@ -97,12 +97,9 @@ public:
* @param measuredAcc Measured acceleration (in body frame, as given by the sensor) * @param measuredAcc Measured acceleration (in body frame, as given by the sensor)
* @param measuredOmega Measured angular velocity (as given by the sensor) * @param measuredOmega Measured angular velocity (as given by the sensor)
* @param dt Time interval between this and the last IMU measurement * @param dt Time interval between this and the last IMU measurement
* @param F, F Jacobians used internally (only needed for testing)
*/ */
void integrateMeasurement(const Vector3& measuredAcc, void integrateMeasurement(const Vector3& measuredAcc,
const Vector3& measuredOmega, double dt, // const Vector3& measuredOmega, double dt);
OptionalJacobian<9, 9> F = boost::none, //
OptionalJacobian<9, 9> G = boost::none);
/// Return pre-integrated measurement covariance /// Return pre-integrated measurement covariance
Matrix preintMeasCov() const { return preintMeasCov_; } Matrix preintMeasCov() const { return preintMeasCov_; }

186
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -40,40 +40,6 @@ using symbol_shorthand::V;
using symbol_shorthand::B; using symbol_shorthand::B;
namespace { namespace {
/* ************************************************************************* */
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector updatePreintegratedMeasurementsTest(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * (correctedAcc - bias_old.accelerometer()) * deltaT;
Vector3 deltaPij_new;
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
Vector3 deltaVij_new = deltaVij_old + temp;
Rot3 deltaRij_new = deltaRij_old
* Rot3::Expmap((correctedOmega - bias_old.gyroscope()) * deltaT);
Vector3 logDeltaRij_new = Rot3::Logmap(deltaRij_new); // not important any more
imuBias::ConstantBias bias_new(bias_old);
Vector result(15);
result << logDeltaRij_new, deltaPij_new, deltaVij_new, bias_new.vector();
return result;
}
Rot3 updatePreintegratedMeasurementsRot(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const imuBias::ConstantBias& bias_old, const Vector3& correctedAcc,
const Vector3& correctedOmega, const double deltaT) {
Vector result = updatePreintegratedMeasurementsTest(deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, correctedAcc, correctedOmega,
deltaT);
return Rot3::Expmap(result.segment < 3 > (6));
}
// Auxiliary functions to test preintegrated Jacobians // Auxiliary functions to test preintegrated Jacobians
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_ // delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
@ -334,158 +300,6 @@ TEST(CombinedImuFactor, PredictRotation) {
EXPECT(assert_equal(expectedPose, x2, tol)); EXPECT(assert_equal(expectedPose, x2, tol));
} }
/* ************************************************************************* */
TEST( CombinedImuFactor, JacobianPreintegratedCovariancePropagation ) {
// Linearization point
imuBias::ConstantBias bias_old = imuBias::ConstantBias(); ///< Current estimate of acceleration and rotation rate biases
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
// Actual pim values
CombinedImuFactor::CombinedPreintegratedMeasurements pim =
evaluatePreintegratedMeasurements(bias_old, measuredAccs, measuredOmegas,
deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = pim.preintMeasCov();
Matrix Factual, Gactual;
pim.integrateMeasurement(newMeasuredAcc, newMeasuredOmega, newDeltaT, Factual,
Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected F wrt positions (15,3)
Matrix df_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaPij_old);
// rotation part has to be done properly, on manifold
df_dpos.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, _1, deltaVij_old,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaPij_old);
EXPECT(assert_equal(df_dpos, Factual.block<15, 3>(0, 3), 1e-5));
// Compute expected F wrt velocities (15,3)
Matrix df_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaVij_old);
// rotation part has to be done properly, on manifold
df_dvel.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old, _1,
deltaRij_old, bias_old, newMeasuredAcc, newMeasuredOmega, newDeltaT
), deltaVij_old);
EXPECT(assert_equal(df_dvel, Factual.block<15, 3>(0, 6), 1e-5));
// Compute expected F wrt angles (15,3)
Matrix df_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT), deltaRij_old);
// rotation part has to be done properly, on manifold
df_dangle.block<3, 3>(0, 0) = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, _1, bias_old, newMeasuredAcc, newMeasuredOmega,
newDeltaT), deltaRij_old);
EXPECT(assert_equal(df_dangle, Factual.block<15, 3>(0, 0), 1e-5));
// Compute expected F wrt biases (15,6)
Matrix df_dbias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT), bias_old);
// rotation part has to be done properly, on manifold
df_dbias.block<3, 6>(0, 0) =
numericalDerivative11<Rot3, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT), bias_old);
EXPECT(assert_equal(df_dbias, Factual.block<15, 6>(0, 9), 1e-5));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected G wrt integration noise
Matrix df_dintNoise(15, 3);
df_dintNoise << I_3x3 * newDeltaT, Z_3x3, Z_3x3, Z_3x3, Z_3x3;
// Compute expected G wrt acc noise (15,3)
Matrix df_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT
), newMeasuredAcc);
// rotation part has to be done properly, on manifold
df_daccNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, _1, newMeasuredOmega, newDeltaT
), newMeasuredAcc);
// Compute expected G wrt gyro noise (15,3)
Matrix df_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT
), newMeasuredOmega);
// rotation part has to be done properly, on manifold
df_domegaNoise.block<3, 3>(6, 0) = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, bias_old, newMeasuredAcc, _1, newDeltaT
), newMeasuredOmega);
// Compute expected G wrt bias random walk noise (15,6)
Matrix df_rwBias(15, 6); // random walk on the bias does not appear in the first 9 entries
df_rwBias.setZero();
df_rwBias.block<6, 6>(9, 0) = I_6x6;
// Compute expected G wrt gyro noise (15,6)
Matrix df_dinitBias = numericalDerivative11<Vector, imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsTest, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT), bias_old);
// rotation part has to be done properly, on manifold
df_dinitBias.block<3, 6>(6, 0) = numericalDerivative11<Rot3,
imuBias::ConstantBias>(
boost::bind(&updatePreintegratedMeasurementsRot, deltaPij_old,
deltaVij_old, deltaRij_old, _1, newMeasuredAcc, newMeasuredOmega,
newDeltaT), bias_old);
df_dinitBias.block<6, 6>(9, 0) = Z_6x6; // only has to influence first 9 rows
Matrix Gexpected(15, 21);
Gexpected << df_dintNoise, df_daccNoise, df_domegaNoise, df_rwBias, df_dinitBias;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix newPreintCovarianceExpected = Factual * oldPreintCovariance
* Factual.transpose() + (1 / newDeltaT) * Gactual * Gactual.transpose();
Matrix newPreintCovarianceActual = pim.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

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

@ -53,30 +53,6 @@ Rot3 evaluateRotationError(const ImuFactor& factor, const Pose3& pose_i,
factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3)); factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).head(3));
} }
// Auxiliary functions to test Jacobians F and G used for
// covariance propagation during preintegration
/* ************************************************************************* */
Vector6 updatePreintegratedPosVel(const Vector3 deltaPij_old,
const Vector3& deltaVij_old, const Rot3& deltaRij_old,
const Vector3& correctedAcc, const Vector3& correctedOmega,
const double deltaT) {
Matrix3 dRij = deltaRij_old.matrix();
Vector3 temp = dRij * correctedAcc * deltaT;
Vector3 deltaPij_new;
deltaPij_new = deltaPij_old + deltaVij_old * deltaT + 0.5 * temp * deltaT;
Vector3 deltaVij_new = deltaVij_old + temp;
Vector6 result;
result << deltaPij_new, deltaVij_new;
return result;
}
Rot3 updatePreintegratedRot(const Rot3& deltaRij_old,
const Vector3& correctedOmega, const double deltaT) {
Rot3 deltaRij_new = deltaRij_old * Rot3::Expmap(correctedOmega * deltaT);
return deltaRij_new;
}
// Define covariance matrices // Define covariance matrices
/* ************************************************************************* */ /* ************************************************************************* */
double accNoiseVar = 0.01; double accNoiseVar = 0.01;
@ -565,227 +541,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega())); assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega()));
} }
/* ************************************************************************* */
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation) {
// Measurements
const Vector3 measuredAcc(0.1, 0.0, 0.0);
const Vector3 measuredOmega(M_PI / 100.0, 0.0, 0.0);
const double deltaT = 0.01;
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(measuredAcc);
measuredOmegas.push_back(measuredOmega);
deltaTs.push_back(deltaT);
measuredAccs.push_back(measuredAcc);
measuredOmegas.push_back(measuredOmega);
deltaTs.push_back(deltaT);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(deltaT);
}
// Actual preintegrated values
PreintegratedImuMeasurements pim = evaluatePreintegratedMeasurements(
kZeroBias, measuredAccs, measuredOmegas, deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Rot3 deltaRij_old = pim.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = pim.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = pim.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = pim.preintMeasCov();
Matrix Factual, Gactual;
pim.integrateMeasurement(measuredAcc, measuredOmega, deltaT,
Factual, Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
boost::function<Vector6(const Vector3&, const Vector3&, const Rot3&)> f =
boost::bind(&updatePreintegratedPosVel, _1, _2, _3, measuredAcc,
measuredOmega, deltaT);
Matrix63 dfpv_dpos = numericalDerivative31(f, deltaPij_old, deltaVij_old,
deltaRij_old);
// Compute expected f_pos_vel wrt velocities
Matrix63 dfpv_dvel = numericalDerivative32(f, deltaPij_old, deltaVij_old,
deltaRij_old);
// Compute expected f_pos_vel wrt angles
Matrix63 dfpv_dangle = numericalDerivative33(f, deltaPij_old, deltaVij_old,
deltaRij_old);
// Compute expected f_rot wrt angles
Matrix3 dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, measuredOmega, deltaT),
deltaRij_old);
Matrix Fexpected(9, 9);
Fexpected << //
dfr_dangle, Z_3x3, Z_3x3, //
dfpv_dangle, dfpv_dpos, dfpv_dvel;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6, 3);
dgpv_dintNoise << I_3x3 * deltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix63 dgpv_daccNoise = numericalDerivative11<Vector6, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, measuredOmega, deltaT), measuredAcc);
// Compute expected F wrt gyro noise
Matrix63 dgpv_domegaNoise = numericalDerivative11<Vector6, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, measuredAcc, _1, deltaT), measuredOmega);
// Compute expected f_rot wrt gyro noise
Matrix3 dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, deltaT),
measuredOmega);
Matrix Gexpected(9, 9);
Gexpected << //
dgr_dangle, Z_3x3, Z_3x3, //
dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
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()
+ (1 / deltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = pim.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */
TEST(ImuFactor, JacobianPreintegratedCovariancePropagation_2ndOrderInt) {
// Measurements
list<Vector3> measuredAccs, measuredOmegas;
list<double> deltaTs;
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
measuredAccs.push_back(Vector3(0.1, 0.0, 0.0));
measuredOmegas.push_back(Vector3(M_PI / 100.0, 0.0, 0.0));
deltaTs.push_back(0.01);
for (int i = 1; i < 100; i++) {
measuredAccs.push_back(Vector3(0.05, 0.09, 0.01));
measuredOmegas.push_back(
Vector3(M_PI / 100.0, M_PI / 300.0, 2 * M_PI / 100.0));
deltaTs.push_back(0.01);
}
// Actual preintegrated values
PreintegratedImuMeasurements preintegrated =
evaluatePreintegratedMeasurements(kZeroBias, measuredAccs, measuredOmegas,
deltaTs);
// so far we only created a nontrivial linearization point for the preintegrated measurements
// Now we add a new measurement and ask for Jacobians
const Vector3 newMeasuredAcc = Vector3(0.1, 0.0, 0.0);
const Vector3 newMeasuredOmega = Vector3(M_PI / 100.0, 0.0, 0.0);
const double newDeltaT = 0.01;
const Rot3 deltaRij_old = preintegrated.deltaRij(); // before adding new measurement
const Vector3 deltaVij_old = preintegrated.deltaVij(); // before adding new measurement
const Vector3 deltaPij_old = preintegrated.deltaPij(); // before adding new measurement
Matrix oldPreintCovariance = preintegrated.preintMeasCov();
Matrix Factual, Gactual;
preintegrated.integrateMeasurement(newMeasuredAcc, newMeasuredOmega,
newDeltaT, Factual, Gactual);
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR F
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute expected f_pos_vel wrt positions
Matrix dfpv_dpos = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, _1, deltaVij_old, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaPij_old);
// Compute expected f_pos_vel wrt velocities
Matrix dfpv_dvel = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, _1, deltaRij_old,
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaVij_old);
// Compute expected f_pos_vel wrt angles
Matrix dfpv_dangle = numericalDerivative11<Vector, Rot3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old, _1,
newMeasuredAcc, newMeasuredOmega, newDeltaT), deltaRij_old);
// Compute expected f_rot wrt angles
Matrix dfr_dangle = numericalDerivative11<Rot3, Rot3>(
boost::bind(&updatePreintegratedRot, _1, newMeasuredOmega, newDeltaT),
deltaRij_old);
Matrix Fexpected(9, 9);
Fexpected << //
dfr_dangle, Z_3x3, Z_3x3, //
dfpv_dangle, dfpv_dpos, dfpv_dvel;
EXPECT(assert_equal(Fexpected, Factual));
//////////////////////////////////////////////////////////////////////////////////////////////
// COMPUTE NUMERICAL DERIVATIVES FOR G
//////////////////////////////////////////////////////////////////////////////////////////////
// Compute jacobian wrt integration noise
Matrix dgpv_dintNoise(6, 3);
dgpv_dintNoise << I_3x3 * newDeltaT, Z_3x3;
// Compute jacobian wrt acc noise
Matrix dgpv_daccNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, _1, newMeasuredOmega, newDeltaT), newMeasuredAcc);
// Compute expected F wrt gyro noise
Matrix dgpv_domegaNoise = numericalDerivative11<Vector, Vector3>(
boost::bind(&updatePreintegratedPosVel, deltaPij_old, deltaVij_old,
deltaRij_old, newMeasuredAcc, _1, newDeltaT), newMeasuredOmega);
// Compute expected f_rot wrt gyro noise
Matrix dgr_dangle = numericalDerivative11<Rot3, Vector3>(
boost::bind(&updatePreintegratedRot, deltaRij_old, _1, newDeltaT),
newMeasuredOmega);
Matrix Gexpected(9, 9);
Gexpected << //
dgr_dangle, Z_3x3, Z_3x3, //
dgpv_domegaNoise, dgpv_dintNoise, dgpv_daccNoise;
EXPECT(assert_equal(Gexpected, Gactual));
// Check covariance propagation
Matrix9 measurementCovariance;
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()
+ (1 / newDeltaT) * Gactual * measurementCovariance * Gactual.transpose();
Matrix newPreintCovarianceActual = preintegrated.preintMeasCov();
EXPECT(assert_equal(newPreintCovarianceExpected, newPreintCovarianceActual));
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim, Matrix9 MonteCarlo(const PreintegratedImuMeasurements& pim,
const NavState& state, const imuBias::ConstantBias& bias, double dt, const NavState& state, const imuBias::ConstantBias& bias, double dt,