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

#include <repredictor.h>

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.

Public Member Functions
template<bool check = disable_reprediction>
std::enable_if_t<!check, statecov_t >	predictTo (const rclcpp::Time &to_stamp)
	Estimates the system state and covariance matrix at the specified time.

template<bool check = disable_reprediction>
std::enable_if_t< check, statecov_t >	predictTo (const rclcpp::Time &to_stamp)
	Estimates the system state and covariance matrix at the specified time.

template<bool check = disable_reprediction>
std::enable_if_t<!check >	addInputChangeWithNoise (const u_t &u, const Q_t &Q, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t< check >	addInputChangeWithNoise (const u_t &u, const Q_t &Q, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t<!check >	addInputChange (const u_t &u, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t< check >	addInputChange (const u_t &u, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t<!check >	addProcessNoiseChange (const Q_t &Q, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t< check >	addProcessNoiseChange (const Q_t &Q, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t<!check >	addMeasurement (const z_t &z, const R_t &R, const rclcpp::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.

template<bool check = disable_reprediction>
std::enable_if_t< check >	addMeasurement (const z_t &z, const R_t &R, const rclcpp::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.

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

	Repredictor ()
	Empty constructor.

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

Protected Types
using	history_t = boost::circular_buffer< info_t >

Protected Member Functions
template<typename T >
bool	checkMonotonicity (const T &buf)

template<typename T >
void	printBuffer (const T &buf)

Protected Attributes
statecov_t	m_sc

ModelPtr	m_default_model

history_t	m_history

Detailed Description

template<class Model, bool disable_reprediction = false>
class mrs_lib::Repredictor< Model, disable_reprediction >

Implementation of the Repredictor 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 Repredictor 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 Repredictor 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 Repredictor to work correctly (see eg. the varstepLKF class).

Template Parameters

Model	the prediction and correction model (eg. a Kalman Filter).
disable_reprediction	if true, reprediction is disabled and the class will act like a dumb LKF (for evaluation purposes).

Constructor & Destructor Documentation

◆ Repredictor() [1/2]

template<class Model , bool disable_reprediction = false>

mrs_lib::Repredictor< Model, disable_reprediction >::Repredictor	(	const x_t &	x0,
		const P_t &	P0,
		const u_t &	u0,
		const Q_t &	Q0,
		const rclcpp::Time &	t0,
		const ModelPtr &	model,
		const unsigned	hist_len
	)

inline

The main constructor.

Initializes the Repredictor 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.

◆ Repredictor() [2/2]

template<class Model , bool disable_reprediction = false>

mrs_lib::Repredictor< Model, disable_reprediction >::Repredictor	(	const x_t &	x0,
		const P_t &	P0,
		const Q_t &	Q0,
		const rclcpp::Time &	t0,
		const ModelPtr &	model,
		const unsigned	hist_len
	)

inline

Variation of the constructor for cases without a system input.

Initializes the Repredictor with the necessary initial and default values. Assumes the system input is zero at t0.

Parameters

x0	Initial state.
P0	Covariance matrix of the initial state uncertainty.
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.

Member Function Documentation

◆ addInputChange() [1/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t<!check > mrs_lib::Repredictor< Model, disable_reprediction >::addInputChange	(	const u_t &	u,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`
	)

inline

Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

u	The system input vector to be added.
stamp	Time stamp of the input vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: The system input vector will not be added if it is older than the oldest element in the history buffer.

◆ addInputChange() [2/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t< check > mrs_lib::Repredictor< Model, disable_reprediction >::addInputChange	(	const u_t &	u,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`
	)

inline

Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

u	The system input vector to be added.
stamp	Time stamp of the input vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: This is the variant of the method when reprediction is disabled and will function like a dumb LKF.

◆ addInputChangeWithNoise() [1/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t<!check > mrs_lib::Repredictor< Model, disable_reprediction >::addInputChangeWithNoise	(	const u_t &	u,
		const Q_t &	Q,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`
	)

inline

Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

u	The system input vector to be added.
Q	The process noise covariance matrix.
stamp	Time stamp of the input vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: The system input vector will not be added if it is older than the oldest element in the history buffer.

◆ addInputChangeWithNoise() [2/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t< check > mrs_lib::Repredictor< Model, disable_reprediction >::addInputChangeWithNoise	(	const u_t &	u,
		const Q_t &	Q,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`
	)

inline

Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

u	The system input vector to be added.
Q	The process noise covariance matrix.
stamp	Time stamp of the input vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this input (eg. mapping it to different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: This is the variant of the method when reprediction is disabled and will function like a dumb LKF.

◆ addMeasurement() [1/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t<!check > mrs_lib::Repredictor< Model, disable_reprediction >::addMeasurement	(	const z_t &	z,
		const R_t &	R,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`,
		const double &	meas_id = `-1`
	)

inline

Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

z	The measurement vector to be added.
R	The measurement noise covariance matrix, corresponding to the measurement vector.
stamp	Time stamp of the measurement vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this measurement (eg. mapping it from different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: The measurement vector will not be added if it is older than the oldest element in the history buffer.

◆ addMeasurement() [2/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t< check > mrs_lib::Repredictor< Model, disable_reprediction >::addMeasurement	(	const z_t &	z,
		const R_t &	R,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`,
		const double &	meas_id = `-1`
	)

inline

Adds one measurement to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

z	The measurement vector to be added.
R	The measurement noise covariance matrix, corresponding to the measurement vector.
stamp	Time stamp of the measurement vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this measurement (eg. mapping it from different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: This is the variant of the method when reprediction is disabled and will function like a dumb LKF.

◆ addProcessNoiseChange() [1/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t<!check > mrs_lib::Repredictor< Model, disable_reprediction >::addProcessNoiseChange	(	const Q_t &	Q,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`
	)

inline

Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

Q	The process noise covariance matrix.
stamp	Time stamp of the input vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this covariance matrix (eg. mapping it to different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: The new element will not be added if it is older than the oldest element in the history buffer.

◆ addProcessNoiseChange() [2/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t< check > mrs_lib::Repredictor< Model, disable_reprediction >::addProcessNoiseChange	(	const Q_t &	Q,
		const rclcpp::Time &	stamp,
		const ModelPtr &	model = `nullptr`
	)

inline

Adds one system input to the history buffer, removing the oldest element in the buffer if it is full.

Parameters

Q	The process noise covariance matrix.
stamp	Time stamp of the input vector and covariance matrix.
model	Optional pointer to a specific Model to be used with this covariance matrix (eg. mapping it to different states). If it equals to nullptr, the default model specified in the constructor will be used.

Note: This is the variant of the method when reprediction is disabled and will function like a dumb LKF.

◆ predictTo() [1/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t<!check, statecov_t > mrs_lib::Repredictor< Model, disable_reprediction >::predictTo ( const rclcpp::Time & to_stamp )

inline

Estimates the system state and covariance matrix at the specified time.

The measurement and system input histories are used to estimate the state vector and covariance matrix values at the specified time, which are returned.

Parameters

to_stamp The desired time at which the state vector and covariance matrix should be estimated.

Returns: Returns the estimated state vector and covariance matrix in a single struct.

◆ predictTo() [2/2]

template<class Model , bool disable_reprediction = false>

template<bool check = disable_reprediction>

std::enable_if_t< check, statecov_t > mrs_lib::Repredictor< Model, disable_reprediction >::predictTo ( const rclcpp::Time & to_stamp )

inline

Estimates the system state and covariance matrix at the specified time.

The measurement and system input histories are used to estimate the state vector and covariance matrix values at the specified time, which are returned.

Parameters

to_stamp The desired time at which the state vector and covariance matrix should be estimated.

Returns: Returns the estimated state vector and covariance matrix in a single struct.

Note: This is the variant of the method when reprediction is disabled and will function like a dumb LKF.

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

include/mrs_lib/repredictor.h

Public Types

Public Member Functions

Protected Types

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ Repredictor() [1/2]

◆ Repredictor() [2/2]

Member Function Documentation

◆ addInputChange() [1/2]

◆ addInputChange() [2/2]

◆ addInputChangeWithNoise() [1/2]

◆ addInputChangeWithNoise() [2/2]

◆ addMeasurement() [1/2]

◆ addMeasurement() [2/2]

◆ addProcessNoiseChange() [1/2]

◆ addProcessNoiseChange() [2/2]

◆ predictTo() [1/2]

◆ predictTo() [2/2]