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

Fixed bug in Imu Bias Jacobian sizes

release/4.3a0
Stephen Williams 2013-05-19 20:25:57 +00:00
parent dbc05045cd
commit cff6d814eb
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/navigation/ImuBias.h
View File

@ -72,7 +72,7 @@ namespace imuBias {
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector correctAccelerometer(const Vector3& measurment, boost::optional<Matrix&> H=boost::none) const { Vector correctAccelerometer(const Vector3& measurment, boost::optional<Matrix&> H=boost::none) const {
if (H){ if (H){
H->resize(6,3); H->resize(3,6);
(*H) << -Matrix3::Identity(), Matrix3::Zero(); (*H) << -Matrix3::Identity(), Matrix3::Zero();
} }
return measurment - biasAcc_; return measurment - biasAcc_;
@ -81,7 +81,7 @@ namespace imuBias {
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector correctGyroscope(const Vector3& measurment, boost::optional<Matrix&> H=boost::none) const { Vector correctGyroscope(const Vector3& measurment, boost::optional<Matrix&> H=boost::none) const {
if (H){ if (H){
H->resize(6,3); H->resize(3,6);
(*H) << Matrix3::Zero(), -Matrix3::Identity(); (*H) << Matrix3::Zero(), -Matrix3::Identity();
} }
return measurment - biasGyro_; return measurment - biasGyro_;