Cooperative Localisation Filters 0.0.1
Loading...
Searching...
No Matches
Public Member Functions | Private Member Functions | Private Attributes | List of all members
EKF Class Reference

Extended Kalman Filter. More...

#include <ekf.h>

Inheritance diagram for EKF:
Inheritance graph
[legend]
Collaboration diagram for EKF:
Collaboration graph
[legend]

Public Member Functions

 EKF (DataHandler &data)
 EKF class constructor.
 
 ~EKF () 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 Extended Kalman filter.
 
void correction (EstimationParameters &, const EstimationParameters &) override
 
void robustCorrection (EstimationParameters &, const EstimationParameters &)
 
Eigen::Matrix< double, total_measurements, 1 > computeMeasurementTau (const EstimationParameters &)
 
Eigen::Matrix< double, 2+total_states, 1 > computeStateTau (const EstimationParameters &)
 

Private Attributes

Eigen::Matrix< double, total_measurements, 1 > accumulative_innovation
 
Eigen::Matrix< double, total_measurements, 1 > accumulative_innovation_covariance
 
Eigen::Matrix< double, 2+total_states, 1 > accumulative_estimation_error
 
Eigen::Matrix< double, 2+total_states, 1 > accummulative_estimation_covariance
 

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_statesmatrix3D_t
 
typedef Eigen::Matrix< double, 2 *total_states, 2 *total_statesmatrix6D_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_measurementskalmanGain_t
 
typedef Eigen::Matrix< double, total_states, total_statesmotionJacobian_t
 
typedef Eigen::Matrix< double, total_states, total_inputsprocessJacobian_t
 
typedef Eigen::Matrix< double, total_measurements, 2 *total_statesmeasurementJacobian_t
 
typedef Eigen::Matrix< double, total_inputs, total_inputsprocessCovariance_t
 
typedef Eigen::Matrix< double, total_measurements, 1 > huberMeasurementThresholds_t
 
typedef Eigen::Matrix< double, total_measurements, total_measurementshuberMeasurementWeights_t
 
typedef Eigen::Matrix< double, 2 *total_states, 1 > huberStateThresholds_t
 
typedef Eigen::Matrix< double, 2 *total_states, 2 *total_stateshuberStateWeights_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< EstimationParametersrobot_parameters
 Houses all estimation parameters for all robots.
 
std::vector< EstimationParameterslandmark_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].
 

Detailed Description

Extended Kalman Filter.

The data contained in the class includes:

Constructor & Destructor Documentation

◆ EKF()

EKF::EKF ( DataHandler &  data)
explicit

EKF class constructor.

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

Parameters
[in]dataClass containing all robot and landmark data.

◆ ~EKF()

EKF::~EKF ( )
override

Default destructor.

Member Function Documentation

◆ computeMeasurementTau()

Eigen::Matrix< double, total_measurements, 1 > EKF::computeMeasurementTau ( const EstimationParameters )
private

◆ computeStateTau()

Eigen::Matrix< double, 2+total_states, 1 > EKF::computeStateTau ( const EstimationParameters )
private

◆ correction()

void EKF::correction ( EstimationParameters ,
const EstimationParameters  
)
overrideprivatevirtual

Implements Filter.

Reimplemented in IEKF.

◆ prediction()

void EKF::prediction ( const Robot::Odometry &  odometry,
EstimationParameters estimation_parameters 
)
overrideprivatevirtual

performs the prediction step of the Extended Kalman filter.

Parameters
[in]odometryThe prior inputs into the system comprising a forward and angular velocity.
[in,out]estimation_parametersThe parameters required by the Extended Kalman filter to perform the prediction step.

Implements Filter.

◆ robustCorrection()

void EKF::robustCorrection ( EstimationParameters ,
const EstimationParameters  
)
private

Member Data Documentation

◆ accummulative_estimation_covariance

Eigen::Matrix<double, 2 + total_states, 1> EKF::accummulative_estimation_covariance
private
Initial value:
=
Eigen::Matrix<double, 2 + total_states, 1>::Zero()

◆ accumulative_estimation_error

Eigen::Matrix<double, 2 + total_states, 1> EKF::accumulative_estimation_error
private
Initial value:
=
Eigen::Matrix<double, 2 + total_states, 1>::Zero()

◆ accumulative_innovation

Eigen::Matrix<double, total_measurements, 1> EKF::accumulative_innovation
private
Initial value:
=
Eigen::Matrix<double, total_measurements, 1>::Zero()

◆ accumulative_innovation_covariance

Eigen::Matrix<double, total_measurements, 1> EKF::accumulative_innovation_covariance
private
Initial value:
=
Eigen::Matrix<double, total_measurements, 1>::Zero()

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