Class EKF

Inheritance Relationships

Base Type

Derived Type

Class Documentation

class EKF : public lupnt::KalmanFilter

Extended Kalman Filter (EKF): linearized predict/update with Joseph-form covariance update, residual-based outlier rejection, and RTS smoothing.

The standard recursive estimator used throughout LuPNT’s navigation applications (e.g. LNSS receiver/satellite state estimation) — registered with the Filter asset factory under the name "EKF" so it can be constructed from YAML config.

Subclassed by lupnt::UDUEKF

Public Functions

EKF() = default
EKF(Config &config)

Construct an EKF from a YAML config node, reading the optional outlier_threshold field (default 3.0 sigma).

virtual ~EKF() = default
void SetOutlierThreshold(double outlier_threshold)

Set the outlier-rejection threshold (in standard deviations of the normalized residual dz_i / sqrt(S_ii)) used by RemoveOutliers.

Parameters:

outlier_threshold – Threshold in sigma; must be non-negative

void RemoveOutliers()

Drop measurement components whose normalized residual |dz_i| / sqrt(S_ii) exceeds outlier_threshold_, shrinking dz_, H_, R_, and recomputing S_ accordingly.

Called by Update as the default fault-detection step (when no custom f_fault_det_ is registered via SetFaultDetectionFunction), after the residual dz_ and innovation covariance S_ have been computed but before the Kalman gain is applied.

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

EKF predict step: propagate x_/P_ via f_dyn_ (linearized by the returned STM F_) and add the process noise Q_ from f_proc_, i.e. P_ = F_ P_ F_^T + Q_.

virtual void Update(const VecX &z_true) override

EKF update step: linearized measurement update with Joseph-form covariance propagation P_ = (I - K H) P (I - K H)^T + K R K^T, including outlier rejection via RemoveOutliers (or a registered fault-detection function) before the Kalman gain K_ = P H^T S^-1 is applied.

virtual void InitializeLogger(int max_tidx) override

EKF implementation of KalmanFilter::InitializeLogger: allocates the x_prior_log_/P_prior_log_/x_pos_log_/P_pos_log_/stm_log_/x_sm_/P_sm_ buffers for max_tidx epochs.

virtual void InitializeSmootherState() override

EKF implementation of KalmanFilter::InitializeSmootherState: seeds x_sm_/P_sm_ at the final time index with the current x_post_/P_post_.

virtual void LogFilterEstimate(int tidx) override

EKF implementation of KalmanFilter::LogFilterEstimate: records the current prior/posterior state, covariance, and state-transition matrix F_ at index tidx.

virtual void UpdateSmoother(int tidx) override

EKF implementation of KalmanFilter::UpdateSmoother: standard RTS smoother recursion using the logged prior/posterior states, covariances, and state-transition matrices.

Protected Attributes

double outlier_threshold_ = 3.0
std::vector<VecXd> x_prior_log_
std::vector<MatXd> P_prior_log_
std::vector<VecXd> x_pos_log_
std::vector<MatXd> P_pos_log_
std::vector<MatXd> stm_log_