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.