class mola::factors::FactorGnssMapEnuConstVelBias¶
Overview¶
GNSS position factor with a receiver bias state, for a fix stamped dt seconds after (or before, if negative) its keyframe.
Measurement model (antenna position in ENU, from the fix):
z = T_enu2map * T_map2i * l(dt) + b
where b is the slowly varying position error of the receiver, in ENU axes, as a variable of the graph, and l(dt) the antenna (lever arm l in the vehicle frame) moved to the fix time with the keyframe’s body-frame twist, to first order (as in FactorGnssMapEnuConstVel):
l(dt) = l + dt * (v + w x l)
With dt = 0 this is FactorGnssMapEnuBias, with zero Jacobians for v and w : use that one instead.
#include <FactorGnssMapEnuConstVelBias.h> class FactorGnssMapEnuConstVelBias: public gtsam::ExpressionFactorN< gtsam::Point3, gtsam::Pose3, gtsam::Pose3, gtsam::Point3, gtsam::Point3, gtsam::Point3 > { public: // construction FactorGnssMapEnuConstVelBias(); FactorGnssMapEnuConstVelBias( gtsam::Key kT_enu2map, gtsam::Key kT_map2i, gtsam::Key kV, gtsam::Key kW, gtsam::Key kBias, const gtsam::Point3& sensorOnVehicle, double dt, const gtsam::Point3& observedENU, const gtsam::SharedNoiseModel& model ); // methods gtsam::NonlinearFactor::shared_ptr clone() const; gtsam::Expression<gtsam::Point3> expression(const std::array<gtsam::Key, NARY_EXPRESSION_SIZE>& keys) const; void print( const std::string& s, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter ) const; bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const; };
Construction¶
FactorGnssMapEnuConstVelBias()
Default constructor for serialization.
FactorGnssMapEnuConstVelBias( gtsam::Key kT_enu2map, gtsam::Key kT_map2i, gtsam::Key kV, gtsam::Key kW, gtsam::Key kBias, const gtsam::Point3& sensorOnVehicle, double dt, const gtsam::Point3& observedENU, const gtsam::SharedNoiseModel& model )
Parameters:
kT_enu2map |
Key for the ENU-to-map transform |
kT_map2i |
Key for the keyframe pose in map coordinates |
kV |
Key for the keyframe linear velocity (body frame) |
kW |
Key for the keyframe angular velocity (body frame) |
kBias |
Key for the receiver bias (Point3, ENU axes) [m] |
sensorOnVehicle |
GNSS antenna position in the vehicle frame |
dt |
Fix time minus keyframe time [s] |
observedENU |
Measured GNSS position in ENU |
model |
Noise model (3D position uncertainty, white part only) |
Methods¶
gtsam::NonlinearFactor::shared_ptr clone() const
Returns:
Deep copy of this factor
void print( const std::string& s, const gtsam::KeyFormatter& keyFormatter = gtsam::DefaultKeyFormatter ) const
Print factor details
bool equals(const gtsam::NonlinearFactor& expected, double tol = 1e-9) const
equals