class mola::factors::GaussMarkovPoint3

Overview

A first-order Gauss-Markov process on a Point3, sampled at irregular times, one graph variable per sample (e.g. the position bias of a GNSS receiver, see FactorGnssMapEnuBias):

b(k) = Phi b(k-1) + w(k), Phi = diag(phi_i), phi_i = exp(-dt / tau), w_i(k) ~ N(0, s_i^2 (1 - phi_i^2)),

started from its stationary distribution N(0, diag(s_i^2)), the prior of the first variable of a chain. Its marginal variance never exceeds s_i^2; the random walk is the limit tau -> inf with s_i sqrt(2 / tau) per sqrt(s) fixed.

An axis with s_i = 0 is not a process: it is held at zero, each variable with its own tight prior (HELD_SIGMA) and no link to the previous one.

Usage: prior_noise() for the first variable (mean zero), link() between consecutive ones, transition() and stationary_cov() to predict a later value from an estimate.

#include <GaussMarkovPoint3.h>

class GaussMarkovPoint3
{
public:
    // fields

    static constexpr double HELD_SIGMA = 1e-3;
    static constexpr double MAX_PRIOR_TO_LINK_SIGMA_RATIO = 1000.0;

    // construction

    GaussMarkovPoint3(const gtsam::Vector3& stationarySigmas, double timeConstant);

    // methods

    const gtsam::Vector3& stationary_sigmas() const;
    double time_constant() const;
    double phi(double dt) const;
    gtsam::Matrix3 transition(double dt) const;
    gtsam::Matrix3 stationary_cov() const;
    gtsam::SharedNoiseModel prior_noise() const;
    gtsam::NonlinearFactor::shared_ptr link(gtsam::Key prev, gtsam::Key next, double dt) const;
};

Fields

static constexpr double HELD_SIGMA = 1e-3

Sigma [same units] holding at zero an axis that is not a process.

static constexpr double MAX_PRIOR_TO_LINK_SIGMA_RATIO = 1000.0

Links of (almost) simultaneous samples are floored at s_i divided by this, or the chain becomes too stiff with respect to its prior to be factorized (GTSAM rejects badly conditioned Cholesky pivots).

Construction

GaussMarkovPoint3(const gtsam::Vector3& stationarySigmas, double timeConstant)

Parameters:

stationarySigmas

s_i per axis; 0 holds that axis at zero

timeConstant

tau [s], > 0

Methods

const gtsam::Vector3& stationary_sigmas() const

The stationary sigmas s_i.

double time_constant() const

The time constant tau [s].

double phi(double dt) const

phi = exp(-dt / tau) of the axes that are processes

gtsam::Matrix3 transition(double dt) const

Phi(dt): phi on the process axes, 0 on the held ones.

gtsam::Matrix3 stationary_cov() const

diag(s_i^2): the variance a value tends to, with nothing observing it

gtsam::SharedNoiseModel prior_noise() const

Noise of the prior N(0, .) of the first variable of a chain.

gtsam::NonlinearFactor::shared_ptr link(
    gtsam::Key prev,
    gtsam::Key next,
    double dt
    ) const

The factor linking variable next to prev, sampled dt [s] later.