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

Fixed-size

release/4.3a0
dellaert 2014-11-23 11:35:35 +01:00
parent 5ec6039988
commit 02ed59b65d
1 changed files with 8 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
gtsam/navigation/ImuBias.h
View File

@ -57,8 +57,8 @@ namespace imuBias {
} }
/** return the accelerometer and gyro biases in a single vector */ /** return the accelerometer and gyro biases in a single vector */
Vector vector() const { Vector6 vector() const {
Vector v(6); Vector6 v;
v << biasAcc_, biasGyro_; v << biasAcc_, biasGyro_;
return v; return v;
} }
@ -70,18 +70,18 @@ namespace imuBias {
const Vector3& gyroscope() const { return biasGyro_; } const Vector3& gyroscope() const { return biasGyro_; }
/** 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& measurement, boost::optional<Matrix&> H=boost::none) const { Vector3 correctAccelerometer(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H){ if (H) {
H->resize(3,6); H->resize(3, 6);
(*H) << -Matrix3::Identity(), Matrix3::Zero(); (*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 */
Vector correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const { Vector3 correctGyroscope(const Vector3& measurement, boost::optional<Matrix&> H=boost::none) const {
if (H){ if (H) {
H->resize(3,6); H->resize(3, 6);
(*H) << Matrix3::Zero(), -Matrix3::Identity(); (*H) << Matrix3::Zero(), -Matrix3::Identity();
} }
return measurement - biasGyro_; return measurement - biasGyro_;