Class UKF

Inheritance Relationships

Base Type

Class Documentation

class UKF : public lupnt::Filter

Unscented Kalman Filter (UKF): predict/update via the unscented transform (sigma-point propagation) instead of linearization.

An alternative to EKF for nonlinear dynamics/measurement models where Jacobians are unavailable or linearization error is too large; constructed and driven the same way as other Filter subclasses (SetDynamicsFunction/SetProcessNoiseFunction/ SetMeasurementFunction, then Predict/Update).

Public Functions

UKF() = default
inline void Initialize(const Real t0, const State &x0, const MatXd &P0)

Initialize the UKF’s epoch, state, and covariance (including prior/posterior copies), set the state dimension n_x_ from x0, and (re)compute the unscented transform weights via InitializeUkfParams.

Called once by application setup code before the first Predict/Update call.

Parameters:

  • t0 – Initial epoch [s]

  • x0 – Initial state estimate, size [n_x]

  • P0 – Initial state covariance, size [n_x x n_x]

MatX ComputeSigmaPoints(const State &state, const MatXd &cov)

Generate the 2*n_x_ + 1 unscented-transform sigma points for a given mean state and covariance cov: the central point state, plus state +/- / chol((n_x_ + lambda_) * cov) columns. 

    Called by `Predict` (on the prior `x_`/`P_`, propagated through `f_dyn_`) and by
    `Update` (on the predicted `x_`/`P_`, passed through `f_meas_`).

    @param state  Mean state about which to spread the sigma points, size [n_x]
    @param cov    Covariance matrix, size [n_x x n_x]
    @return       Sigma points, size [n_x x n_sigma_] (n_sigma_ = 2*n_x_ + 1)
State UnscentedTransform(const MatX &sigma_points, MatXd &cov_out, const MatXd *Q = nullptr)

Recombine a set of (already-propagated) sigma points into a mean and covariance using the unscented-transform weights w_m_/w_c_, optionally adding a process noise covariance Q.

Called by Predict to recombine the propagated dynamics sigma points (with Q = the process noise from f_proc_) into the predicted state mean/covariance.

Parameters:

  • sigma_points – Propagated sigma points, size [n_x_ x n_sigma_]

  • cov_out – Output covariance of the recombined state, size [n_x_ x n_x_]

  • Q – Optional process noise covariance to add, size [n_x_ x n_x_]

Returns:

Mean of the sigma points, size [n_x_]

inline void SetAlpha(double alpha)

Set the unscented-transform spread parameter alpha (typically small, e.g. 1e-3) and recompute the sigma-point weights.

inline void SetBeta(double beta)

Set the unscented-transform secondary scaling parameter beta (2 is optimal for Gaussian distributions) and recompute the sigma-point weights.

inline void SetKappa(double kappa)

Set the unscented-transform secondary scaling parameter kappa (typically 0 or 3 - n_x) and recompute the sigma-point weights.

virtual void Predict(Real t, const State *u = nullptr) override

UKF predict step: generate sigma points from (x_, P_) via ComputeSigmaPoints, propagate each through f_dyn_ (no Jacobian needed), then recombine via UnscentedTransform (adding process noise Q_ from f_proc_) to form the predicted x_prior_/P_prior_, which also become the new x_/P_.

virtual void Update(const VecX &z_true) override

UKF update step: generate sigma points from the predicted (x_, P_), transform each through f_meas_ to form the measurement sigma points, recombine them (with weights w_m_/w_c_) into the predicted measurement mean and innovation covariance S_, compute the state-measurement cross-covariance, form the Kalman gain K_ = cross_cov * S_^-1, and apply the correction dx_ = K_ * dy_ to update x_post_/P_post_ (which also become the new x_/P_).

inline VecXd GetMeasurementResidual()

Get the measurement residual dy = z_true - meas_mean from the last Update call, size [n_z].

inline MatXd GetKalmanGain()

Get the Kalman gain K_ = cross_cov * S_^-1 from the last Update call, size [n_x x n_z].

inline MatXd GetMeasurementNoiseCov()

Get the measurement noise covariance R_ (from the first sigma point’s f_meas_ evaluation) from the last Update call, size [n_z x n_z].

inline MatXd GetMeasurementJacobian()

Get H_ (unused by the UKF’s sigma-point update; retained for interface compatibility with KalmanFilter).

inline int GetMeasurementSize()

Get the current measurement dimension (number of rows of H_; see GetMeasurementJacobian).

inline MatXd GetProcessNoise()

Get Q_ (unused by the UKF’s sigma-point predict step, which adds process noise directly in UnscentedTransform; retained for interface compatibility with KalmanFilter).

inline MatXd GetStateJacobian()

Get F_ (unused by the UKF’s sigma-point predict step, which has no explicit state-transition Jacobian; retained for interface compatibility with KalmanFilter).

inline MatXd GetInnovationCov()

Get the innovation covariance S_ from the last Update call, size [n_z x n_z].

inline MatXd GetMeasurementCov()

Same as GetMeasurementNoiseCov; get the measurement noise covariance R_.

inline VecXd GetStateCorrection()

Get the state correction dx_ = K_ * dy_ from the last Update call, size [n_x].

inline VecXd GetTrueMeasurement()

Get the observed measurement vector from the last Update call, size [n_z].

inline VecXd GetPredictedMeasurement()

Get the predicted measurement mean (recombined from the measurement sigma points) from the last Update call, size [n_z].

Protected Attributes

MatXd F_
MatXd H_
MatXd Q_
MatXd R_
VecX dy_
VecX dx_
VecX z_true_
VecX z_prior_
MatXd S_
MatXd K_