Struct IEKFLocalizer 

pub struct IEKFLocalizer {
    pub state: IEKFState,
    pub covariance: Matrix4<f64>,
    pub config: IEKFConfig,
}

Iterated Extended Kalman Filter for robot localization

Fields

state: IEKFState

Current state estimate [x, y, yaw, v]

covariance: Matrix4<f64>

State covariance matrix

config: IEKFConfig

Configuration

Implementations

impl IEKFLocalizer

pub fn new(config: IEKFConfig) -> Self

Create a new IEKF localizer; panics on invalid config.

pub fn try_new(config: IEKFConfig) -> RoboticsResult<Self>

Create a new validated IEKF localizer.

pub fn with_initial_state(initial_state: IEKFState, config: IEKFConfig) -> Self

Create with an explicit initial state; panics on invalid inputs.

pub fn state_2d(&self) -> State2D

Get the current estimate as a State2D.

pub fn estimate( &mut self, measurement: &IEKFMeasurement, control: &IEKFControl, dt: f64, ) -> Result<&IEKFState, RoboticsError>

Full IEKF estimation step (predict + iterated update).

Trait Implementations

impl StateEstimator for IEKFLocalizer

fn predict(&mut self, control: &Self::Control, dt: f64)

Prediction step — identical to EKF predict.

fn update(&mut self, measurement: &Self::Measurement)

Update step using iterated linearization.

Silently skips the update if the innovation covariance S is singular.

type State = Matrix<f64, Const<4>, Const<1>, ArrayStorage<f64, 4, 1>>

State type used by this estimator

type Measurement = Matrix<f64, Const<2>, Const<1>, ArrayStorage<f64, 2, 1>>

Measurement type used by this estimator

type Control = Matrix<f64, Const<2>, Const<1>, ArrayStorage<f64, 2, 1>>

Control input type

fn get_state(&self) -> &Self::State

Get current state estimate

fn get_covariance(&self) -> Option<&DMatrix<f64>>

Get current covariance estimate (if applicable)