|
| 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 |
| |
|
| virtual void | prediction (const Robot::Odometry &, EstimationParameters &)=0 |
| |
| virtual void | correction (EstimationParameters &, const EstimationParameters &)=0 |
| |
| 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 &) |
| |
Jacobian of the measurement model evaluated in terms of the systems states: x,y, and heading.
- Parameters
-
| [in,out] | ego_robot | The estimation parameters of the ego robot. |
| [in] | other_agent | The estimation parameters of the agent that was measured by the ego robot. |
The formula used for the calculation of the Jacobian of the measurement matrix between ego vehicle \(i\) and measured agent \(j\) take the form
\[ H = \begin{bmatrix} \frac{-\Delta x}{d} & \frac{-\Delta y}{d} & 0 &
\frac{\Delta x}{d} & \frac{\Delta y}{d} & 0\\ \frac{\Delta y}{d^2} &
\frac{-\Delta x}{d^2} & -1 & \frac{-\Delta y}{d^2} & \frac{\Delta x}{d^2}
& 0\end{bmatrix} \]
where \(\Delta x = x_j - x_i\); \(\Delta y = y_j
- y_i\); and \(\Delta d = \sqrt{\Delta x^2 + \Delta y^2}\).
| void Filter::calculateMotionJacobian |
( |
const Robot::Odometry & |
odometry, |
|
|
EstimationParameters & |
estimation_parameters, |
|
|
const double |
sample_period |
|
) |
| |
|
protected |
Calculates the Jacobian matrix of the unicycle motion model in terms of the systems states (x,y,orientation).
- Parameters
-
| [in] | odometry | The prior inputs into the system comprising a forward and angular velocity. |
| [in,out] | estimation_parameters | The estimation parameters of the ego robot. |
| [in] | sample_period | The period between odometry measurements. |
The formula used for the calculation of the motion model Jacobian takes the form:
\[ F = \begin{bmatrix} 1 & 0 & -\tilde{v}\Delta t \sin(\theta) \\ 0 & 1
& \tilde{v} \Delta t \cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}, \]
where \(\theta\) denotes the heading (orientation) of the ego vehicle; and \(\tilde{v}\) denotes the forward velocity. The forward velocity is a random variable with Gaussian distributed noise \(\mathcal{N}(0,w)\) , where \(w\) is defined by the covariance matrix Filter::EstimationParameters.measurement_noise. See Filter::motionModel for information on the motion model from which this was derived.
| void Filter::calculateProcessJacobian |
( |
EstimationParameters & |
estimation_parameters, |
|
|
const double |
sample_period |
|
) |
| |
|
protected |
Calculates the Jacobian matrix of the motion model evaluated in terms of the process inputs.
- Parameters
-
| [in,out] | estimation_parameters | The estimation parameters of the ego robot. |
| [in] | sample_period | The period between odometry measurements. |
The formula used for the calculation of the process noise Jacobian takes the form
\[L = \begin{bmatrix}\Delta t \cos(\theta) & 0 \\ \Delta t \sin(\theta)
& 0 \\ 0 & \Delta t \end{bmatrix}, \]
where \(\Delta t\) denotes the sample period; and \(\theta\) denotes the heading (orientation) of the ego robot. measurement_noise. See EKF::prediction for information on the motion model from which this was derived.
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.
- Parameters
-
| [in] | ego_robot | the structure containing the estimation parameters of the ego vehicle. |
| [in] | other_agent | the structure containing the estimation parameters of the agent measured by the ego vehicle. |
Cooperative Localisation (Positioning) involves robots that share thier state and estimation error covariances / precision when one robot measures the other. As a result, the estimation error covariance/precision needs to be augmented from a 3x3 to a 6x6 matrix to house the error covariance of both the ego vehicle ( \(i\)) and the measured agent ( \(j\)):
\[\mathbf{P} = \begin{bmatrix} \mathbf{P}_i & \mathbf{0} \\ \mathbf{0} &
\mathbf{P}_j \end{bmatrix}, \]
where \(\mathbf{P}_i\) and \(\mathbf{P}_j\) are the estimation error covariance of the ego robot \(i\) and the observed agent \(j\) respectively.
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.
- Parameters
-
| [in,out] | ego_robot | The estimation parameters of the ego robot. |
| [in] | other_agent | The estimation parameters of the agent that was measured by the ego robot. |
The measusurement model for the measurement taken from ego vehicle \(i\) to agent \(j\) used for the correction step takes the form
\[ \begin{bmatrix} r_{ij}^{(t)} \\ \phi_{ij}^{(t)}\end{bmatrix} =
\begin{bmatrix}\sqrt{(x_j^{(t)} - x_i^{(t)})^2 + (y_j^{(t)} - y_i^{(t)})^2} +
q_r \\ \text{atan2}\left(\frac{y_j^{(t)}-y_i^{(t)}}{x_j^{(t)}-x_i^{(t)}
}\right) - \theta_i^{(t)} + q_\phi\end{bmatrix}, \]
where \(x\) and \(y\) denote the robots coordinates; \(\theta\) denotes the ego robots orientation (heading); and \(q_r\) and \(q_\omega\) denote the Gaussian distributed measurement noise (See Filter::EstimationParameters.measurement_noise).
| void Filter::motionModel |
( |
const Robot::Odometry & |
odometry, |
|
|
EstimationParameters & |
estimation_parameters, |
|
|
const double |
sample_period |
|
) |
| |
|
protected |
The unicycle motion model used to perform motion predictions.
- Parameters
-
| [in] | odometry | The prior inputs into the system comprising a forward and angular velocity. |
| [in,out] | estimation_parameters | The estimation parameters of the ego robot. |
| [in] | sample_period | The period between odometry measurements. |
The motion model used for robot takes the form:
\[\begin{bmatrix} x_i^{(t+1)} \\ y_i^{(t+1)}
\\ \theta_i^{(t+1)}\end{bmatrix} = \begin{bmatrix} x_i^{(t)} +
\tilde{v}_i^{(t)}\Delta t\cos(\theta_i^{(t)}) \\ y_i^{(t)} +
\tilde{v}_i^{(t)}\Delta t\sin(\theta_i^{(t)})\\ \theta_i^{(t)} +
\tilde{\omega}_i^{(t)}\Delta t. \end{bmatrix}, \]
where \(i\) denotes the robots ID; \(t\) denotes the current timestep; \(\Delta t\) denotes the sample period; \(x\) and \(y\) are the robots coordinates; \(\theta\) denotes the robots heading (orientation); \(\tilde{v}_i\) denotes the forward velocity input; and \(\tilde{\omega}\) denotes the angular velocity input. Both \(\tilde{v}_i\) and \(\tilde{\omega}_i\) are normally distributed random variables \(\mathcal{N}(0,w)\) (see Filter::EstimationParameters::process_noise).
