#include <information_filter.h>

Inheritance diagram for InformationFilter:

Collaboration diagram for InformationFilter:

Public Member Functions
	InformationFilter (DataHandler &data)
	InformationFilter class constructor.

	~InformationFilter () override
	Default destructor.

Public Member Functions inherited from Filter
	Filter (DataHandler &data)
	Assigns fields data based on datahandler input.

virtual	~Filter ()

void	performInference ()
	Performs robot state inference using the EKF bayesian inference framework for all robots provided.

Private Member Functions
void	prediction (const Robot::Odometry &, EstimationParameters &) override
	performs the prediction step of the Information filter.

void	correction (EstimationParameters &, const EstimationParameters &) override

Additional Inherited Members
Protected Types inherited from Filter
enum	{ FORWARD_VELOCITY = 0 , ANGULAR_VELOCITY = 1 }

enum	{ RANGE = 0 , BEARING = 1 }

enum	{ X = 0 , Y = 1 , ORIENTATION = 2 }

typedef Eigen::Matrix< double, 2, 1 >	vector2D_t

typedef Eigen::Matrix< double, total_states, 1 >	vector3D_t

typedef Eigen::Matrix< double, 2 *total_states, 1 >	vector6D_t

typedef Eigen::Matrix< double, 2, 2 >	matrix2D_t

typedef Eigen::Matrix< double, total_states, total_states >	matrix3D_t

typedef Eigen::Matrix< double, 2 total_states, 2 total_states >	matrix6D_t

typedef vector3D_t	state_t

typedef vector6D_t	augmentedState_t

typedef matrix3D_t	covariance_t

typedef matrix6D_t	augmentedCovariance_t

typedef vector3D_t	information_t

typedef vector6D_t	augmentedInformation_t

typedef matrix3D_t	precision_t

typedef matrix6D_t	augmentedPrecision_t

typedef vector2D_t	measurement_t

typedef matrix2D_t	measurementCovariance_t

typedef Eigen::Matrix< double, 2 *total_states, total_measurements >	kalmanGain_t

typedef Eigen::Matrix< double, total_states, total_states >	motionJacobian_t

typedef Eigen::Matrix< double, total_states, total_inputs >	processJacobian_t

typedef Eigen::Matrix< double, total_measurements, 2 *total_states >	measurementJacobian_t

typedef Eigen::Matrix< double, total_inputs, total_inputs >	processCovariance_t

typedef Eigen::Matrix< double, total_measurements, 1 >	huberMeasurementThresholds_t

typedef Eigen::Matrix< double, total_measurements, total_measurements >	huberMeasurementWeights_t

typedef Eigen::Matrix< double, 2 *total_states, 1 >	huberStateThresholds_t

typedef Eigen::Matrix< double, 2 total_states, 2 total_states >	huberStateWeights_t

Protected Member Functions inherited from Filter
void	motionModel (const Robot::Odometry &, EstimationParameters &, const double)
	The unicycle motion model used to perform motion predictions.

void	calculateMotionJacobian (const Robot::Odometry &, EstimationParameters &, const double)
	Calculates the Jacobian matrix of the unicycle motion model in terms of the systems states (x,y,orientation).

void	calculateProcessJacobian (EstimationParameters &, const double)
	Calculates the Jacobian matrix of the motion model evaluated in terms of the process inputs.

measurement_t	measurementModel (EstimationParameters &, const EstimationParameters &)
	Uses the non-linear measurement model to predict what the measurement from the system would be given the state estimates of the ego robot and measured agent.

void	calculateMeasurementJacobian (EstimationParameters &, const EstimationParameters &)
	Jacobian of the measurement model evaluated in terms of the systems states: x,y, and heading.

matrix3D_t	marginalise (const matrix6D_t &)
	Schur complement-based marginalisation that marginalises a 6x6 matrix into a 3x3 matrix.

state_t	marginalise (const vector6D_t &, const matrix6D_t &)
	Schur complement-based marginalisation that marginalises a 6x1 vector and 6x6 matrix into a 3x1 vector.

augmentedInformation_t	createAugmentedVector (const state_t &, const state_t &)
	Combines the 3 states of the ego vehicle (x,y,orientation), with the state of the agent measured to create a augmented state vector.

augmentedCovariance_t	createAugmentedMatrix (const covariance_t &, const covariance_t &)
	Combines the covariance/precision matrix of the 3 states of the ego vehicle (x,y,orientation), with the covariance/precision of the of the agent measured.

void	normaliseAngle (double &)
	Normalise an angle between \((-\pi, \pi]\).

measurement_t	calculateNormalisedInnovation (const EstimationParameters &)
	Calculates the normalised residual of the value produced by the sensor measurement and the prior estimate passed through the non-linear measurement model.

augmentedState_t	calculateNormalisedEstimationResidual (const EstimationParameters &)
	Calculates the normalised residual of the initial estimate and updated estimate.

huberMeasurementWeights_t	HuberMeasurement (const measurement_t &, const huberMeasurementThresholds_t &)
	Huber Measurement cost function,.

huberStateWeights_t	HuberState (const augmentedState_t &, const huberStateThresholds_t &)
	Huber State cost function,.

matrix3D_t	computePseudoInverse (const matrix3D_t &)

matrix6D_t	computePseudoInverse (const matrix6D_t &)

Protected Attributes inherited from Filter
DataHandler &	data_
	Data class housing all the data pertaining to cooperative localisation (positioning).

std::vector< EstimationParameters >	robot_parameters
	Houses all estimation parameters for all robots.

std::vector< EstimationParameters >	landmark_parameters
	Houses all estimation parameters for all landmarks.

const huberMeasurementThresholds_t	measurement_thresholds
	The thresholds for the huber measurement cost function.

const huberStateThresholds_t	state_thresholds
	The thresholds for the huber state cost function.

Static Protected Attributes inherited from Filter
static const unsigned short	total_states = 3
	The total number of system states: x coordinate [m], y-coordinate [m], and orientation (heading) [rad].

static const unsigned short	total_inputs = 2
	The total number of system inputs: forward velocity [m/s] and angular velocity [rad/s].

static const unsigned short	total_measurements = 2
	The total number of system measurements: range [m] and bearing [rad].

Constructor & Destructor Documentation

◆ InformationFilter()

InformationFilter::InformationFilter ( DataHandler & data )

InformationFilter class constructor.

This constructor sets up the prior states and parameters to perform Extended Information filtering.

Parameters

[in] data Class containing all robot and landmark data.

◆ ~InformationFilter()

InformationFilter::~InformationFilter ( )

override

Default destructor.

Member Function Documentation

◆ correction()

void InformationFilter::correction	(	EstimationParameters &	,
		const EstimationParameters &
	)

overrideprivatevirtual

Implements Filter.

◆ prediction()

void InformationFilter::prediction	(	const Robot::Odometry &	odometry,
		EstimationParameters &	ego_robot
	)

overrideprivatevirtual

performs the prediction step of the Information filter.

Parameters

[in]	odometry	The prior inputs into the system comprising a forward and angular velocity.
[in,out]	ego_robot	The parameters required by the Information filter to perform the prediction step.

Implements Filter.

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

/home/dan/Code/UTIAS-Multi-Robot/Filters/include/information_filter.h
/home/dan/Code/UTIAS-Multi-Robot/Filters/src/information_filter.cpp

Public Member Functions