Removed F/G tests: derivatives no longer matched and are checked at a lower level anyways.
dellaert
parent
7901077a7a
commit
b26bfb27ac
|
|
@ -66,8 +66,7 @@ void PreintegratedCombinedMeasurements::resetIntegration() {
|
|||
|
||||
//------------------------------------------------------------------------------
|
||||
void PreintegratedCombinedMeasurements::integrateMeasurement(
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT,
|
||||
OptionalJacobian<15, 15> F_test, OptionalJacobian<15, 21> G_test) {
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double deltaT) {
|
||||
|
||||
const Matrix3 dRij = deltaXij_.R(); // expensive when quaternion
|
||||
|
||||
|
|
@ -117,21 +116,6 @@ void PreintegratedCombinedMeasurements::integrateMeasurement(
|
|||
D_v_R(&G_measCov_Gt) = temp;
|
||||
D_R_v(&G_measCov_Gt) = temp.transpose();
|
||||
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;
|
||||
}
|
||||
}
|
||||
|
||||
//------------------------------------------------------------------------------
|
||||
|
|
|
|||
|
|
@ -143,9 +143,7 @@ class PreintegratedCombinedMeasurements : public PreintegrationBase {
|
|||
* frame)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double deltaT,
|
||||
OptionalJacobian<15, 15> F_test = boost::none,
|
||||
OptionalJacobian<15, 21> G_test = boost::none);
|
||||
const Vector3& measuredOmega, double deltaT);
|
||||
|
||||
/// methods to access class variables
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
|
|
|||
|
|
@ -48,21 +48,9 @@ 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 dt,
|
||||
OptionalJacobian<9, 9> outF, OptionalJacobian<9, 9> G) {
|
||||
const Vector3& measuredAcc, const Vector3& measuredOmega, double dt) {
|
||||
|
||||
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)
|
||||
Matrix93 G1, G2;
|
||||
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:
|
||||
// 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()
|
||||
+ Gi * (p().integrationCovariance * dt) * Gi.transpose() // NOTE(frank): (Gi*dt)*(C/dt)*(Gi'*dt)
|
||||
+ 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(
|
||||
const imuBias::ConstantBias& biasHat, const Matrix3& measuredAccCovariance,
|
||||
|
|
|
|||
|
|
@ -97,12 +97,9 @@ public:
|
|||
* @param measuredAcc Measured acceleration (in body frame, 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 F, F Jacobians used internally (only needed for testing)
|
||||
*/
|
||||
void integrateMeasurement(const Vector3& measuredAcc,
|
||||
const Vector3& measuredOmega, double dt, //
|
||||
OptionalJacobian<9, 9> F = boost::none, //
|
||||
OptionalJacobian<9, 9> G = boost::none);
|
||||
const Vector3& measuredOmega, double dt);
|
||||
|
||||
/// Return pre-integrated measurement covariance
|
||||
Matrix preintMeasCov() const { return preintMeasCov_; }
|
||||
|
|
|
|||
|
|
@ -40,40 +40,6 @@ using symbol_shorthand::V;
|
|||
using symbol_shorthand::B;
|
||||
|
||||
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
|
||||
// delPdelBiasAcc_ delPdelBiasOmega_ delVdelBiasAcc_ delVdelBiasOmega_ delRdelBiasOmega_
|
||||
|
|
@ -334,158 +300,6 @@ TEST(CombinedImuFactor, PredictRotation) {
|
|||
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() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
}
|
||||
|
||||
// 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
|
||||
/* ************************************************************************* */
|
||||
double accNoiseVar = 0.01;
|
||||
|
|
@ -565,227 +541,6 @@ TEST(ImuFactor, FirstOrderPreIntegratedMeasurements) {
|
|||
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,
|
||||
const NavState& state, const imuBias::ConstantBias& bias, double dt,
|
||||
|
|
|
|||
Loading…
Reference in New Issue