Class UDUEKF

Inheritance Relationships

Base Type

Derived Type

Class Documentation

class UDUEKF : public lupnt::EKF

Extended Kalman filter that stores and updates the covariance as a UDU (Bierman-Thornton / square-root) factorization P = U diag(D) U^T instead of a dense matrix.

A drop-in EKF replacement (registered with the Filter asset factory as "UDUEKF") for applications that need the improved numerical conditioning of a square-root filter; reuses EKF’s outlier rejection, fault detection, and smoother infrastructure, but overrides Predict/Update/SetCovariance to maintain U_/ D_diag_ via UDUDecomposition/UDUReconstruct (see udu_utils.h).

Subclassed by lupnt::UDUStochasticCloningEKF

Public Functions

UDUEKF() = default
inline explicit UDUEKF(Config &config)

Construct a UDU EKF from a YAML config node (forwarded to EKF).

~UDUEKF() override = default
virtual void SetCovariance(const MatXd &P) override

Set the initial state covariance P_ (via Filter::SetCovariance) and immediately factor it into U_/D_diag_ via SetUd.

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

UDU-filter predict step: same propagation as EKF::Predict (state via f_dyn_/F_, process noise Q_ via f_proc_), but reconstructs P_ from U_/D_diag_ before propagation and refactors the result back into U_/D_diag_ via SetUd afterward.

virtual void Update(const VecX &z_true) override

UDU-filter update step: same outlier handling as EKF::Update, but applies the correction via the sequential Carlson (UD/Bierman-Thornton) update (CarlsonUpdate) instead of a batch Joseph-form Kalman update.

inline MatXd GetUFactor() const

Get the unit-upper-triangular U factor of the covariance, P_ = U diag(D) / U^T, size [n_x x n_x].

inline VecXd GetDFactor() const

Get the diagonal D factor of the covariance, P_ = U diag(D) U^T, size [n_x].

Protected Functions

void SetUd()

Refactor the current dense covariance P_ into U_/D_diag_ via UDUDecomposition.

Called by SetCovariance (on initialization) and at the end of Predict (after P_ has been propagated and process noise added).

void CarlsonUpdate()

Sequentially process each scalar component of the measurement residual dz_ using the Carlson-form (rank-1 UD/Bierman-Thornton) measurement update, refactoring U_/D_diag_ after each component and accumulating the total state correction dx_.

Called by Update once outlier rejection has finalized dz_/H_/R_. Implements the algorithmic core that distinguishes UDUEKF::Update from EKF::Update’s batch Joseph-form update.

Protected Attributes

MatXd U_
VecXd D_diag_