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

Re-factored conversion from sensor to body

release/4.3a0
dellaert 2015-07-26 07:55:43 +02:00
parent 18ff25b67f
commit 654cb4d31a
1 changed files with 18 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
gtsam/navigation/PreintegrationBase.cpp
View File

@ -129,22 +129,27 @@ void PreintegrationBase::updatePreintegratedJacobians(const Vector3& correctedAc
std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose( std::pair<Vector3, Vector3> PreintegrationBase::correctMeasurementsByBiasAndSensorPose(
const Vector3& measuredAcc, const Vector3& measuredOmega, const Vector3& measuredAcc, const Vector3& measuredOmega,
boost::optional<const Pose3&> body_P_sensor) const { boost::optional<const Pose3&> body_P_sensor) const {
Vector3 correctedAcc, correctedOmega; // Correct for bias in the sensor frame
correctedAcc = biasHat_.correctAccelerometer(measuredAcc); Vector3 s_correctedAcc, s_correctedOmega;
correctedOmega = biasHat_.correctGyroscope(measuredOmega); s_correctedAcc = biasHat_.correctAccelerometer(measuredAcc);
s_correctedOmega = biasHat_.correctGyroscope(measuredOmega);
// Then compensate for sensor-body displacement: we express the quantities // Compensate for sensor-body displacement if needed: we express the quantities
// (originally in the IMU frame) into the body frame // (originally in the IMU frame) into the body frame
// Equations below assume the "body" frame is the CG
if (body_P_sensor) { if (body_P_sensor) {
Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); Matrix3 bRs = body_P_sensor->rotation().matrix();
correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame Vector3 b_correctedOmega = bRs * s_correctedOmega; // rotation rate vector in the body frame
Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); Matrix3 body_omega_body__cross = skewSymmetric(b_correctedOmega);
correctedAcc = body_R_sensor * correctedAcc Vector3 b_arm = body_P_sensor->translation().vector();
- body_omega_body__cross * body_omega_body__cross Vector3 b_velocity_bs = b_correctedOmega.cross(b_arm); // magnitude: omega * arm
* body_P_sensor->translation().vector(); // Subtract out the the centripetal acceleration from the measured one
// linear acceleration vector in the body frame // to get linear acceleration vector in the body frame:
} Vector3 b_correctedAcc = bRs * s_correctedAcc
return std::make_pair(correctedAcc, correctedOmega); - b_correctedOmega.cross(b_velocity_bs);
return std::make_pair(b_correctedAcc, b_correctedOmega);
} else
return std::make_pair(correctedAcc, s_correctedOmega);
} }
/// Predict state at time j /// Predict state at time j