Class EKF

Inheritance Relationships

Base Type

Derived Types

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::SRIF, lupnt::SchmidtEKF, 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 SetConsiderStateCount(int n_consider)

Configure the trailing n_consider elements of the state vector as Schmidt “consider” states, turning this EKF into a Schmidt (consider- parameter) Extended Kalman Filter (also available pre-configured as SchmidtEKF, see schmidt_ekf.h).

Consider states are still propagated (Predict) and contribute to the measurement Jacobian H_ and hence to the Kalman gain and covariance bookkeeping in Update, but are never corrected: Update zeros the trailing n_consider rows of the Kalman gain K_ before applying the state correction, so their mean is left unchanged while their (co)variance — including cross-covariance with the estimated states — still updates consistently (the Joseph-form covariance update is valid for any gain, optimal or not). This is the standard “gain zeroing” implementation of the Schmidt-Kalman filter.

Parameters:

n_consider – Number of trailing state elements to treat as consider states (0 disables consider-state handling)

inline int GetConsiderStateCount() const

Get the number of trailing consider states configured via SetConsiderStateCount (0 if this is a plain EKF).

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
int n_consider_ = 0

Number of trailing state elements treated as Schmidt “consider” states (0 disables consider-state handling, i.e. a plain EKF).

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_