class mola::factors::FactorGnssMapEnuBias

Overview

GNSS position factor with a receiver bias state.

Measurement model (antenna position in ENU, from the fix):

z = T_enu2map * T_map2i * l + b

where l is the antenna (lever arm) in the vehicle frame and b the slowly varying position error of the receiver, in ENU axes, as a variable of the graph (e.g. a chain of GaussMarkovPoint3 variables). This is FactorGnssMapEnu plus b; FactorGnssMapEnuConstVelBias is the version for a fix stamped away from its keyframe.

#include <FactorGnssMapEnuBias.h>

class FactorGnssMapEnuBias: public gtsam::ExpressionFactorN< gtsam::Point3, gtsam::Pose3, gtsam::Pose3, gtsam::Point3 >
{
public:
    // construction

    FactorGnssMapEnuBias();

    FactorGnssMapEnuBias(
        gtsam::Key kT_enu2map,
        gtsam::Key kT_map2i,
        gtsam::Key kBias,
        const gtsam::Point3& sensorOnVehicle,
        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

FactorGnssMapEnuBias()

Default constructor for serialization.

FactorGnssMapEnuBias(
    gtsam::Key kT_enu2map,
    gtsam::Key kT_map2i,
    gtsam::Key kBias,
    const gtsam::Point3& sensorOnVehicle,
    const gtsam::Point3& observedENU,
    const gtsam::SharedNoiseModel& model
    )

Parameters:

kT_enu2map

Key for the ENU-to-map transform

kT_map2i

Key for the vehicle pose in map coordinates

kBias

Key for the receiver bias (Point3, ENU axes) [m]

sensorOnVehicle

GNSS antenna position in the vehicle frame

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