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

Informative derivative names. Only compute if need to.

release/4.3a0
Duy-Nguyen Ta 2015-09-16 08:24:17 -04:00
parent 7f19e2ea86
commit 75abc90a90
2 changed files with 45 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/navigation/PreintegrationBase.cpp
View File

@ -77,7 +77,9 @@ bool PreintegrationBase::equals(const PreintegrationBase& other,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose( pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega) const { OptionalJacobian<3, 3> D_correctedAcc_measuredAcc,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega) const {
// Correct for bias in the sensor frame // Correct for bias in the sensor frame
Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc); Vector3 j_correctedAcc = biasHat_.correctAccelerometer(j_measuredAcc);
@ -93,6 +95,13 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
// Correct acceleration // Correct acceleration
j_correctedAcc = bRs * j_correctedAcc; j_correctedAcc = bRs * j_correctedAcc;
// Jacobians
if (D_correctedAcc_measuredAcc) *D_correctedAcc_measuredAcc = bRs;
if (D_correctedAcc_measuredOmega) *D_correctedAcc_measuredOmega = Matrix3::Zero();
if (D_correctedOmega_measuredOmega) *D_correctedOmega_measuredOmega = bRs;
// Centrifugal acceleration
const Vector3 b_arm = p().body_P_sensor->translation().vector(); const Vector3 b_arm = p().body_P_sensor->translation().vector();
if (!b_arm.isZero()) { if (!b_arm.isZero()) {
// Subtract out the the centripetal acceleration from the measured one // Subtract out the the centripetal acceleration from the measured one
@ -100,6 +109,7 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega); const Matrix3 body_Omega_body = skewSymmetric(j_correctedOmega);
const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm const Vector3 b_velocity_bs = body_Omega_body * b_arm; // magnitude: omega * arm
j_correctedAcc -= body_Omega_body * b_velocity_bs; j_correctedAcc -= body_Omega_body * b_velocity_bs;
// Update derivative: centrifugal causes the correlation between acc and omega!!!
if (D_correctedAcc_measuredOmega) { if (D_correctedAcc_measuredOmega) {
double wdp = j_correctedOmega.dot(b_arm); double wdp = j_correctedOmega.dot(b_arm);
*D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp)) *D_correctedAcc_measuredOmega = -(diag(Vector3::Constant(wdp))
@ -115,22 +125,32 @@ pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPos
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc, NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt, OptionalJacobian<9, 9> F, const Vector3& j_measuredOmega, const double dt,
OptionalJacobian<9, 3> G1, OptionalJacobian<9, 3> G2) const { OptionalJacobian<9, 9> D_updated_current,
OptionalJacobian<9, 3> D_updated_measuredAcc,
OptionalJacobian<9, 3> D_updated_measuredOmega) const {
Vector3 j_correctedAcc, j_correctedOmega; Vector3 j_correctedAcc, j_correctedOmega;
Matrix3 D_correctedAcc_measuredOmega; Matrix3 D_correctedAcc_measuredAcc, //
D_correctedAcc_measuredOmega, //
D_correctedOmega_measuredOmega;
bool needDerivs = D_updated_measuredAcc && D_updated_measuredOmega && p().body_P_sensor;
boost::tie(j_correctedAcc, j_correctedOmega) = boost::tie(j_correctedAcc, j_correctedOmega) =
correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega, D_correctedAcc_measuredOmega); correctMeasurementsByBiasAndSensorPose(j_measuredAcc, j_measuredOmega,
(needDerivs ? &D_correctedAcc_measuredAcc : 0),
(needDerivs ? &D_correctedAcc_measuredOmega : 0),
(needDerivs ? &D_correctedOmega_measuredOmega : 0));
// Do update in one fell swoop // Do update in one fell swoop
NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, F, G1, G2); Matrix93 D_updated_correctedAcc, D_updated_correctedOmega;
if (G1 && G2 && p().body_P_sensor) { NavState updated = deltaXij_.update(j_correctedAcc, j_correctedOmega, dt, D_updated_current,
const Matrix3 bRs = p().body_P_sensor->rotation().matrix(); (needDerivs ? D_updated_correctedAcc : D_updated_measuredAcc),
*G2 = *G2*bRs; (needDerivs ? D_updated_correctedOmega : D_updated_measuredOmega));
if (needDerivs) {
*D_updated_measuredAcc = D_updated_correctedAcc * D_correctedAcc_measuredAcc;
*D_updated_measuredOmega = D_updated_correctedOmega * D_correctedOmega_measuredOmega;
if (!p().body_P_sensor->translation().vector().isZero()) { if (!p().body_P_sensor->translation().vector().isZero()) {
*G2 += *G1 * D_correctedAcc_measuredOmega; *D_updated_measuredOmega += D_updated_correctedAcc * D_correctedAcc_measuredOmega;
} }
*G1 = *G1*bRs;
} }
return updated; return updated;
} }
@ -138,14 +158,16 @@ NavState PreintegrationBase::updatedDeltaXij(const Vector3& j_measuredAcc,
//------------------------------------------------------------------------------ //------------------------------------------------------------------------------
void PreintegrationBase::update(const Vector3& j_measuredAcc, void PreintegrationBase::update(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt, const Vector3& j_measuredOmega, const double dt,
Matrix3* D_incrR_integratedOmega, Matrix9* F, Matrix93* G1, Matrix93* G2) { Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* D_updated_measuredAcc, Matrix93* D_updated_measuredOmega) {
// Save current rotation for updating Jacobians // Save current rotation for updating Jacobians
const Rot3 oldRij = deltaXij_.attitude(); const Rot3 oldRij = deltaXij_.attitude();
// Do update // Do update
deltaTij_ += dt; deltaTij_ += dt;
deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt, F, G1, G2); // functional deltaXij_ = updatedDeltaXij(j_measuredAcc, j_measuredOmega, dt,
D_updated_current, D_updated_measuredAcc, D_updated_measuredOmega); // functional
// Update Jacobians // Update Jacobians
// TODO(frank): we are repeating some computation here: accessible in F ? // TODO(frank): we are repeating some computation here: accessible in F ?

15
gtsam/navigation/PreintegrationBase.h
View File

@ -196,22 +196,27 @@ public:
bool equals(const PreintegrationBase& other, double tol) const; bool equals(const PreintegrationBase& other, double tol) const;
/// Subtract estimate and correct for sensor pose /// Subtract estimate and correct for sensor pose
/// Compute the derivatives due to non-identity body_P_sensor (rotation and centrifugal acc)
/// Ignore D_correctedOmega_measuredAcc as it is trivially zero
std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose( std::pair<Vector3, Vector3> correctMeasurementsByBiasAndSensorPose(
const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none) const; OptionalJacobian<3, 3> D_correctedAcc_measuredAcc = boost::none,
OptionalJacobian<3, 3> D_correctedAcc_measuredOmega = boost::none,
OptionalJacobian<3, 3> D_correctedOmega_measuredOmega = boost::none) const;
/// Calculate the updated preintegrated measurement, does not modify /// Calculate the updated preintegrated measurement, does not modify
/// It takes measured quantities in the j frame /// It takes measured quantities in the j frame
NavState updatedDeltaXij(const Vector3& j_measuredAcc, NavState updatedDeltaXij(const Vector3& j_measuredAcc,
const Vector3& j_measuredOmega, const double dt, const Vector3& j_measuredOmega, const double dt,
OptionalJacobian<9, 9> F = boost::none, OptionalJacobian<9, 3> G1 = OptionalJacobian<9, 9> D_updated_current = boost::none,
boost::none, OptionalJacobian<9, 3> G2 = boost::none) const; OptionalJacobian<9, 3> D_updated_measuredAcc = boost::none,
OptionalJacobian<9, 3> D_updated_measuredOmega = boost::none) const;
/// Update preintegrated measurements and get derivatives /// Update preintegrated measurements and get derivatives
/// It takes measured quantities in the j frame /// It takes measured quantities in the j frame
void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega, void update(const Vector3& j_measuredAcc, const Vector3& j_measuredOmega,
const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* F, const double deltaT, Matrix3* D_incrR_integratedOmega, Matrix9* D_updated_current,
Matrix93* G1, Matrix93* G2); Matrix93* D_udpated_measuredAcc, Matrix93* D_updated_measuredOmega);
/// Given the estimate of the bias, return a NavState tangent vector /// Given the estimate of the bias, return a NavState tangent vector
/// summarizing the preintegrated IMU measurements so far /// summarizing the preintegrated IMU measurements so far