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

Itertative Extended Kalman Filter (IEKF). More...

#include <iekf.h>

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

Public Member Functions

 IEKF (DataHandler &data)
 IEKF class constructor.
 
 ~IEKF () override
 Default destructor.
 
- Public Member Functions inherited from EKF
 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 correction (EstimationParameters &, const EstimationParameters &) override
 Performs the Iterative Extended Kalman correct step.
 
void robustCorrection (EstimationParameters &, const EstimationParameters &)
 A robust version of the correction function that uses the Huber cost function to increase estimation error covariance of measurements that seem to be outliers.
 

Private Attributes

const unsigned short max_iterations = 1
 

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

Itertative Extended Kalman Filter (IEKF).

This is a child of the EKF since it has the same prediction step. The difference lies in the Gauss-Nwton optimisation approach to the measurement update.

Note
This filter does not work well in the precense of outliers and therefore it is recommended to use the robustCorrection. Even so, since the system does not exhibit strong non-linearities, the benifits of relinearising the Jacobians is minimial. In conclusion, using the standard EKF (or EKF with robust correction) results in better performance.

Constructor & Destructor Documentation

◆ IEKF()

IEKF::IEKF ( DataHandler &  data)
explicit

IEKF class constructor.

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

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

◆ ~IEKF()

IEKF::~IEKF ( )
override

Default destructor.

Member Function Documentation

◆ correction()

void IEKF::correction ( EstimationParameters ego_robot,
const EstimationParameters other_agent 
)
overrideprivatevirtual

Performs the Iterative Extended Kalman correct step.

Parameters
[in,out]ego_robotThe estimation parameters of the ego robot.
[in]other_agentThe estimation parameters of the obejct that was measured by the ego robot.

Reimplemented from EKF.

◆ robustCorrection()

void IEKF::robustCorrection ( EstimationParameters ego_robot,
const EstimationParameters other_agent 
)
private

A robust version of the correction function that uses the Huber cost function to increase estimation error covariance of measurements that seem to be outliers.

Parameters
[in,out]ego_robotThe estimation parameters of the ego robot.
[in]other_agentThe estimation parameters of the obejct that was measured by the ego robot.

Member Data Documentation

◆ max_iterations

const unsigned short IEKF::max_iterations = 1
private

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