33 #ifndef ROBOT_LOCALIZATION_UKF_H 34 #define ROBOT_LOCALIZATION_UKF_H 69 explicit Ukf(std::vector<double>
args);
89 void predict(
const double referenceTime,
const double delta);
124 #endif // ROBOT_LOCALIZATION_UKF_H std::vector< Eigen::VectorXd > sigmaPoints_
The UKF sigma points.
Eigen::MatrixXd weightedCovarSqrt_
This matrix is used to generate the sigmaPoints_.
Unscented Kalman filter class.
void correct(const Measurement &measurement)
Carries out the correct step in the predict/update cycle.
Structure used for storing and comparing measurements (for priority queues)
std::vector< double > covarWeights_
The weights associated with each sigma point when calculating a predicted estimateErrorCovariance_.
~Ukf()
Destructor for the Ukf class.
void predict(const double referenceTime, const double delta)
Carries out the predict step in the predict/update cycle.
Ukf(std::vector< double > args)
Constructor for the Ukf class.
bool uncorrected_
Used to determine if we need to re-compute the sigma points when carrying out multiple corrections...
std::vector< double > stateWeights_
The weights associated with each sigma point when generating a new state.
double lambda_
Used in weight generation for the sigma points.