51 boost::optional<gtsam::Matrix &> H1 = boost::none,
52 boost::optional<gtsam::Matrix &> H2 = boost::none)
const override {
54 static const Vector3 b_accel{0.0, 0.0, 0.0};
55 static const Vector3 b_omega{0.0, 0.0, 0.0};
57 Matrix99 predicted_H_x1;
58 NavState predicted = x1.
update(b_accel, b_omega, dt_, H1 ? &predicted_H_x1 :
nullptr, {}, {});
60 Matrix99 error_H_predicted;
64 *H1 = error_H_predicted * predicted_H_x1;
Navigation state composing of attitude, position, and velocity.
Non-linear factor base classes.
Global functions in a separate testing namespace.
Definition chartTesting.h:28
noiseModel::Base::shared_ptr SharedNoiseModel
Aliases.
Definition NoiseModel.h:724
std::uint64_t Key
Integer nonlinear key type.
Definition types.h:100
Binary factor for applying a constant velocity model to a moving body represented as a NavState.
Definition ConstantVelocityFactor.h:29
gtsam::Vector evaluateError(const NavState &x1, const NavState &x2, boost::optional< gtsam::Matrix & > H1=boost::none, boost::optional< gtsam::Matrix & > H2=boost::none) const override
Caclulate error: (x2 - x1.update(dt))) where X1 and X1 are NavStates and dt is the time difference in...
Definition ConstantVelocityFactor.h:50
Navigation state: Pose (rotation, translation) + velocity NOTE(frank): it does not make sense to make...
Definition NavState.h:34
Vector9 localCoordinates(const NavState &g, OptionalJacobian< 9, 9 > H1=boost::none, OptionalJacobian< 9, 9 > H2=boost::none) const
localCoordinates with optional derivatives
Definition NavState.cpp:135
NavState update(const Vector3 &b_acceleration, const Vector3 &b_omega, const double dt, OptionalJacobian< 9, 9 > F, OptionalJacobian< 9, 3 > G1, OptionalJacobian< 9, 3 > G2) const
Integrate forward in time given angular velocity and acceleration in body frame Uses second order int...
Definition NavState.cpp:171
double error(const Values &c) const override
Calculate the error of the factor.
Definition NonlinearFactor.cpp:138
A convenient base class for creating your own NoiseModelFactor with n variables.
Definition NonlinearFactor.h:400