|
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 |
|
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 &) |
|
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 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].
|
|
Extended Kalman Filter.
The data contained in the class includes:
- All robots states, odometry readings, and measurements.
- All landmarks positions.
- All robot sensors errors statistics.