Implementation of the RepredictorAloamgarm for fusing measurements with variable delays. More...

#include <repredictor_aloamgarm.h>

Inheritance diagram for mrs_lib::RepredictorAloamgarm< Model >:

Collaboration diagram for mrs_lib::RepredictorAloamgarm< Model >:

Public Types
using	x_t = typename Model::x_t
	State vector type $n \times 1$ .

using	u_t = typename Model::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Model::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Model::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Model::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Model::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Model::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

using	ModelPtr = typename std::shared_ptr< Model >
	Shorthand type for a shared pointer-to-Model.

using	history_t = typename Repredictor< Model >::history_t

Public Types inherited from mrs_lib::Repredictor< Model >
using	x_t = typename Model::x_t
	State vector type $n \times 1$ .

using	u_t = typename Model::u_t
	Input vector type $m \times 1$ .

using	z_t = typename Model::z_t
	Measurement vector type $p \times 1$ .

using	P_t = typename Model::P_t
	State uncertainty covariance matrix type $n \times n$ .

using	R_t = typename Model::R_t
	Measurement noise covariance matrix type $p \times p$ .

using	Q_t = typename Model::Q_t
	Process noise covariance matrix type $n \times n$ .

using	statecov_t = typename Model::statecov_t
	Helper struct for passing around the state and its covariance in one variable.

using	ModelPtr = typename std::shared_ptr< Model >
	Shorthand type for a shared pointer-to-Model.

Public Member Functions
	RepredictorAloamgarm (const x_t &x0, const P_t &P0, const u_t &u0, const Q_t &Q0, const ros::Time &t0, const ModelPtr &model, const unsigned hist_len, const std::shared_ptr< boost::circular_buffer< double >> &nis_buffer)
	Variation of the constructor for kalman filter using nis_buffer (ALOAMGARM) More...

Public Member Functions inherited from mrs_lib::Repredictor< Model >
std::enable_if_t<!check, statecov_t >	predictTo (const ros::Time &to_stamp)
	Estimates the system state and covariance matrix at the specified time. More...

std::enable_if_t< check, statecov_t >	predictTo (const ros::Time &to_stamp)
	Estimates the system state and covariance matrix at the specified time. More...

std::enable_if_t<!check >	addInputChangeWithNoise (const u_t &u, const Q_t &Q, const ros::Time &stamp, const ModelPtr &model=nullptr)
	Adds one system input to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t< check >	addInputChangeWithNoise (const u_t &u, const Q_t &Q,[[maybe_unused]] const ros::Time &stamp, const ModelPtr &model=nullptr)
	Adds one system input to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t<!check >	addInputChange (const u_t &u, const ros::Time &stamp, const ModelPtr &model=nullptr)
	Adds one system input to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t< check >	addInputChange (const u_t &u,[[maybe_unused]] const ros::Time &stamp, const ModelPtr &model=nullptr)
	Adds one system input to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t<!check >	addProcessNoiseChange (const Q_t &Q, const ros::Time &stamp, const ModelPtr &model=nullptr)
	Adds one system input to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t< check >	addProcessNoiseChange (const Q_t &Q,[[maybe_unused]] const ros::Time &stamp, const ModelPtr &model=nullptr)
	Adds one system input to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t<!check >	addMeasurement (const z_t &z, const R_t &R, const ros::Time &stamp, const ModelPtr &model=nullptr, const double &meas_id=-1)
	Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full. More...

std::enable_if_t< check >	addMeasurement (const z_t &z, const R_t &R, const ros::Time &stamp, const ModelPtr &model=nullptr, const double &meas_id=-1)
	Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full. More...

	Repredictor (const x_t &x0, const P_t &P0, const u_t &u0, const Q_t &Q0, const ros::Time &t0, const ModelPtr &model, const unsigned hist_len)
	The main constructor. More...

	Repredictor ()
	Empty constructor. More...

	Repredictor (const x_t &x0, const P_t &P0, const Q_t &Q0, const ros::Time &t0, const ModelPtr &model, const unsigned hist_len)
	Variation of the constructor for cases without a system input. More...

Additional Inherited Members
Protected Types inherited from mrs_lib::Repredictor< Model >
using	history_t = boost::circular_buffer< info_t >

Protected Member Functions inherited from mrs_lib::Repredictor< Model >
bool	checkMonotonicity (const T &buf)

void	printBuffer (const T &buf)

Protected Attributes inherited from mrs_lib::Repredictor< Model >
statecov_t	m_sc

ModelPtr	m_default_model

history_t	m_history

Detailed Description

template<class Model>
class mrs_lib::RepredictorAloamgarm< Model >

Implementation of the RepredictorAloamgarm for fusing measurements with variable delays.

A standard state-space system model is assumed for the repredictor with system inputs and measurements, generated from the system state vector. The inputs and measurements may be delayed with varying durations (an older measurement may become available after a newer one). A typical use-case scenario is when you have one "fast" but imprecise sensor and one "slow" but precise sensor and you want to use them both to get a good prediction (eg. height from the Garmin LiDAR, which comes at 100Hz, and position from a SLAM, which comes at 10Hz with a long delay). If the slow sensor is significantly slower than the fast one, fusing its measurement at the time it arrives without taking into account the sensor's delay may significantly bias your latest estimate.

To accomodate this, the RepredictorAloamgarm keeps a buffer of N last inputs and measurements (N is specified in the constructor). This buffer is then used to re-predict the desired state to a specific time, as requested by the user. Note that the re-prediction is evaluated in a lazy manner only when the user requests it, so it goes through the whole history buffer every time a prediction is requested.

The RepredictorAloamgarm utilizes a fusion Model (specified as the template parameter), which should implement the predict() and correct() methods. This Model is used for fusing the system inputs and measurements as well as for predictions. Typically, this Model will be some kind of a Kalman Filter (LKF, UKF etc.).

Note: The Model should be able to accomodate predictions with varying time steps in order for the RepredictorAloamgarm to work correctly (see eg. the varstepLKF class).

Template Parameters

Model the prediction and correction model (eg. a Kalman Filter).

Constructor & Destructor Documentation

◆ RepredictorAloamgarm()

template<class Model >

mrs_lib::RepredictorAloamgarm< Model >::RepredictorAloamgarm	(	const x_t &	x0,
		const P_t &	P0,
		const u_t &	u0,
		const Q_t &	Q0,
		const ros::Time &	t0,
		const ModelPtr &	model,
		const unsigned	hist_len,
		const std::shared_ptr< boost::circular_buffer< double >> &	nis_buffer
	)

inline

Variation of the constructor for kalman filter using nis_buffer (ALOAMGARM)

Initializes the RepredictorAloamgarm with the necessary initial and default values.

Parameters

x0	Initial state.
P0	Covariance matrix of the initial state uncertainty.
u0	Initial system input.
Q0	Default covariance matrix of the process noise.
t0	Time stamp of the initial state.
model	Default prediction and correction model.
hist_len	Length of the history buffer for system inputs and measurements.
nis_buffer	Circular buffer for NIS values.

The documentation for this class was generated from the following file:

include/mrs_lib/repredictor_aloamgarm.h

Public Types

Public Member Functions

Additional Inherited Members

Detailed Description

template<class Model> class mrs_lib::RepredictorAloamgarm< Model >

Constructor & Destructor Documentation

◆ RepredictorAloamgarm()

template<class Model>
class mrs_lib::RepredictorAloamgarm< Model >