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

OptionalJacobians

release/4.3a0
dellaert 2015-07-19 21:24:17 -07:00
parent 13cca7be00
commit 1229ca6fee
1 changed files with 4 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/navigation/ImuBias.h
View File

@ -78,20 +78,18 @@ public:
/** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */ /** Correct an accelerometer measurement using this bias model, and optionally compute Jacobians */
Vector3 correctAccelerometer(const Vector3& measurement, Vector3 correctAccelerometer(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const { OptionalJacobian<3, 6> H = boost::none) const {
if (H) { if (H) {
H->resize(3, 6); (*H) << -I_3x3, Z_3x3;
(*H) << -Matrix3::Identity(), Matrix3::Zero();
} }
return measurement - biasAcc_; return measurement - biasAcc_;
} }
/** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */ /** Correct a gyroscope measurement using this bias model, and optionally compute Jacobians */
Vector3 correctGyroscope(const Vector3& measurement, Vector3 correctGyroscope(const Vector3& measurement,
boost::optional<Matrix&> H = boost::none) const { OptionalJacobian<3, 6> H = boost::none) const {
if (H) { if (H) {
H->resize(3, 6); (*H) << Z_3x3, -I_3x3;
(*H) << Matrix3::Zero(), -Matrix3::Identity();
} }
return measurement - biasGyro_; return measurement - biasGyro_;
} }