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

fixed potential bug when IMU-to-body trasformation is not the identity

release/4.3a0
Luca 2014-12-03 18:42:44 -05:00
parent 6b56b609f2
commit c4b62929bf
2 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/navigation/CombinedImuFactor.cpp
View File

@ -101,9 +101,8 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
* skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
delRdelBiasOmega_ = Rincr.inverse().matrix() * delRdelBiasOmega_ - Jr_theta_incr * deltaT;
@ -177,7 +176,7 @@ void CombinedImuFactor::CombinedPreintegratedMeasurements::integrateMeasurement(
if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT;
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;

5
gtsam/navigation/ImuFactor.cpp
View File

@ -87,7 +87,6 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
const Vector3 theta_incr = correctedOmega * deltaT; // rotation vector describing rotation increment computed from the current rotation rate measurement
const Rot3 Rincr = Rot3::Expmap(theta_incr); // rotation increment computed from the current rotation rate measurement
const Matrix3 Jr_theta_incr = Rot3::rightJacobianExpMapSO3(theta_incr); // Right jacobian computed at theta_incr
// Update Jacobians
@ -98,7 +97,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
}else{
delPdelBiasAcc_ += delVdelBiasAcc_ * deltaT - 0.5 * deltaRij_.matrix() * deltaT*deltaT;
delPdelBiasOmega_ += delVdelBiasOmega_ * deltaT - 0.5 * deltaRij_.matrix()
* skewSymmetric(biasHat_.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega_;
* skewSymmetric(correctedAcc) * deltaT*deltaT * delRdelBiasOmega_;
}
delVdelBiasAcc_ += -deltaRij_.matrix() * deltaT;
delVdelBiasOmega_ += -deltaRij_.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega_;
@ -157,7 +156,7 @@ void ImuFactor::PreintegratedMeasurements::integrateMeasurement(
if(!use2ndOrderIntegration_){
deltaPij_ += deltaVij_ * deltaT;
}else{
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * biasHat_.correctAccelerometer(measuredAcc) * deltaT*deltaT;
deltaPij_ += deltaVij_ * deltaT + 0.5 * deltaRij_.matrix() * correctedAcc * deltaT*deltaT;
}
deltaVij_ += deltaRij_.matrix() * correctedAcc * deltaT;
deltaRij_ = deltaRij_ * Rincr;