Class Ukf

Inheritance Relationships

Base Type

Class Documentation

class Ukf : public robot_localization::FilterBase

Unscented Kalman filter class.

Implementation of an unscenter Kalman filter (UKF). This class derives from FilterBase and overrides the predict() and correct() methods in keeping with the discrete time UKF algorithm. The algorithm was derived from the UKF Wikipedia article at http://en.wikipedia.org/wiki/Kalman_filter#Unscented_Kalman_filter as well as this paper: J. J. LaViola, Jr., “A comparison of unscented and extended Kalman filtering for estimating quaternion motion,” in Proc. American Control Conf., Denver, CO, June 4–6, 2003, pp. 2435–2440 Obtained here: http://www.cs.ucf.edu/~jjl/pubs/laviola_acc2003.pdf

Public Functions

Ukf()

Constructor for the Ukf class.

Parameters:

args – [in] - Generic argument container. It is assumed that args[0] constains the alpha parameter, args[1] contains the kappa parameter, and args[2] contains the beta parameter.

~Ukf()

Destructor for the Ukf class.

void setConstants(double alpha, double kappa, double beta)
virtual void correct(const Measurement &measurement) override

Carries out the correct step in the predict/update cycle.

Parameters:

measurement – [in] - The measurement to fuse with our estimate

virtual void predict(const rclcpp::Time &reference_time, const rclcpp::Duration &delta) override

Carries out the predict step in the predict/update cycle.

Projects the state and error matrices forward using a model of the vehicle’s motion.

Parameters:

  • reference_time – [in] - The time at which the prediction is being made

  • delta – [in] - The time step over which to predict.

Protected Functions

void generateSigmaPoints()

Computes the weighted covariance and sigma points.

void projectSigmaPoint(Eigen::VectorXd &sigma_point, const rclcpp::Duration &delta)

Carries out the predict step for the posteriori state of a sigma point.

Parameters:

  • sigma_point – [inout] - The sigma point (state vector) to project

  • delta – [in] - The time step over which to project

Protected Attributes

std::vector<Eigen::VectorXd> sigma_points_

The UKF sigma points.

Used to sample possible next states during prediction.

Eigen::MatrixXd weighted_covar_sqrt_

This matrix is used to generate the sigmaPoints_.

std::vector<double> state_weights_

The weights associated with each sigma point when generating a new state.

std::vector<double> covar_weights_

The weights associated with each sigma point when calculating a predicted estimateErrorCovariance_.

double lambda_

Used in weight generation for the sigma points.

bool uncorrected_

Used to determine if we need to re-compute the sigma points when carrying out multiple corrections.