Class KalmanFilter¶

Defined in File ekf.h

Inheritance Relationships¶

Base Type¶

public lupnt::Filter (Class Filter)

Derived Type¶

public lupnt::EKF (Class EKF)

Class Documentation¶

class KalmanFilter : public lupnt::Filter ¶

Base class for linear/linearized Kalman-type filters (EKF, UDUEKF), adding Kalman-gain bookkeeping, fault detection, and a fixed-interval smoother on top of Filter.

Stores the per-step Kalman quantities (K_, S_, dz_, dx_, H_, F_, …) produced by Predict/Update so they can be inspected (e.g. by filter_print.h helpers or estimation-error utilities in filter_utils.h) and provides hooks (InitializeLogger/LogFilterEstimate/UpdateSmoother) for a Rauch-Tung-Striebel (RTS) smoother pass implemented by derived classes (e.g. EKF).

Subclassed by lupnt::EKF

Public Functions

KalmanFilter() = default¶

KalmanFilter(Config &config)¶: Construct a Kalman filter from a YAML config node (forwarded to Filter).

virtual ~KalmanFilter() = default¶

inline VecXd GetStateCorrection()¶: Get the last state correction dx = K * dz applied by Update, 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 (from f_meas_) from the last Update call, size [n_z].

inline VecXd GetMeasurementResidual()¶: Get the measurement residual dz = z_true - z_prior from the last Update call (after outlier removal), size [n_z].

inline MatXd GetProcessNoise()¶: Get the process noise covariance Q_ used in the last Predict call, size [n_x x n_x].

inline MatXd GetInnovationCov()¶: Get the innovation covariance S = H P H^T + R from the last Update call, size [n_z x n_z].

inline MatXd GetMeasurementCov()¶: Get the measurement noise covariance R_ from the last Update call, size [n_z x n_z].

inline MatXd GetKalmanGain()¶: Get the Kalman gain K = P H^T S^-1 from the last Update call, size [n_x x n_z].

inline MatXd GetProcessNoiseMappingMatrix()¶: Get the process noise mapping matrix G_ (maps a reduced-dimension process noise onto the full state, used when use_process_noise_mapping_ is enabled via SetProcessNoiseMappingMatrix).

inline MatXd GetMeasurementNoiseCov()¶: Same as GetMeasurementCov; get the measurement noise covariance R_.

inline MatXd GetStateCorrectionCov()¶: Get the state-correction covariance Sigma_dx = K S K^T from the last Update call, size [n_x x n_x].

inline MatXd GetStateJacobian()¶: Get the state-transition (Jacobian) matrix F_ from the last Predict call, size [n_x x n_x].

inline MatXd GetMeasurementJacobian()¶: Get the measurement Jacobian H_ from the last Update call, size [n_z x n_x].

inline VecXd GetSmoothedState(int tidx)¶: Get the smoothed state estimate at time index tidx, produced by UpdateSmoother.

inline MatXd GetSmoothedCovariance(int tidx)¶: Get the smoothed state covariance at time index tidx, produced by UpdateSmoother.

inline void SetMeasurementJacobian(const MatXd &H)¶: Override the measurement Jacobian H_ (used by a custom fault-detection function to drop/reweight measurements before Update’s gain computation).

inline void SetMeasurementNoiseCov(const MatXd &R)¶: Override the measurement noise covariance R_ (used by a custom fault-detection function).

inline void SetInnovationCov(const MatXd &S)¶: Override the innovation covariance S_ (used by a custom fault-detection function).

inline void SetMeasurementResidual(const VecXd &dz)¶: Override the measurement residual dz_ (used by a custom fault-detection function, e.g. to remove outlier components before the gain computation).

inline int GetMeasurementSize()¶: Get the current measurement dimension (number of rows of dz_).

inline void SetProcessNoiseMappingMatrix(const MatXd &G)¶

Set the process noise mapping matrix G_ and enable process-noise mapping, so that Predict maps Q_ via Q_ = G_ * Q_ * G_^T before adding it to the propagated covariance.

Parameters:: G – Process noise mapping matrix, size [n_x x n_q]

inline void SetFaultDetectionFunction(std::function<void(KalmanFilter*)> f_fault_det)¶

Register a custom fault-detection/outlier-rejection callback, invoked by Update (in place of the default RemoveOutliers) after the measurement residual dz_ and innovation covariance S_ have been computed but before the Kalman gain is applied.

The callback typically calls SetMeasurementResidual/SetMeasurementJacobian/ SetMeasurementNoiseCov/SetInnovationCov to remove or reweight outlier components.

virtual void InitializeLogger(int max_tidx) = 0¶

Allocate per-time-step smoother history buffers for max_tidx_ epochs.

Called once before a forward filtering + backward smoothing run to size the x_prior_log_/P_prior_log_/x_pos_log_/P_pos_log_/stm_log_/x_sm_/P_sm_ buffers used by LogFilterEstimate and UpdateSmoother.

Parameters:: max_tidx – Number of time steps to allocate for

virtual void InitializeSmootherState() = 0¶

Seed the backward smoother pass with the final posterior state/covariance (x_sm_[N-1] = x_post_, P_sm_[N-1] = P_post_).

Called once, after the forward filtering pass completes, before the first call to UpdateSmoother.

virtual void LogFilterEstimate(int tidx) = 0¶

Record the prior/posterior state, covariance, and state-transition matrix at time index tidx for later use by UpdateSmoother.

Called once per epoch during the forward filtering pass, after each Predict/ Update pair.

Parameters:: tidx – Time index at which to record the current filter state

virtual void UpdateSmoother(int tidx) = 0¶

Perform one backward Rauch-Tung-Striebel (RTS) smoother step, computing the smoothed state/covariance at time index tidx from the smoothed values at tidx+1 and the logged filter history.

Called once per epoch, in decreasing time-index order, during the backward smoothing pass after the forward filtering pass and InitializeSmootherState have completed.