navlie State Estimation Library

navlie is a state estimation package designed for both traditional vector-space estimation and Lie-group-based estimation problems.

It uses a unified abstraction layer that allows a single estimator implementation to operate on various state definitions. Users define their own State, Process Model, and Measurement Model, and can then apply various estimation algorithms to them.

Supported Algorithms:

• Extended Kalman Filter (EKF)
• Iterated Extended Kalman Filter (IEKF)
• Sigmapoint Kalman Filters (Unscented, Spherical Cubature, Gauss-Hermite)
• Interacting Multiple Model Filter
• Batch MAP Estimation

Key Features:

• Large collection of built-in process and measurement models.
• On-manifold numerical Jacobians via finite differencing.
• Utilities for plotting, error analysis, and consistency evaluation.
• Monte Carlo experiment executor with result aggregation.
• Preintegration module for linear, wheel odometry, and IMU process models.

The core of navlie is the abstract State class. To implement a custom state (e.g., for a vector space or a Lie group), you must subclass State and provide the following:

Required Attributes

• value: The underlying representation of the state.
• dof: The degrees of freedom associated with the state.

Required Methods

• plus(dx): A generic "addition" operation. It takes a dx vector (size equal to dof) and returns a new State instance.
• minus(other_state): A generic "subtraction" operation. It takes another State of the same type and returns a numpy array of error values.
• copy(): Returns a new object of the same type with the same attribute values.

Optional Attributes

• stamp: A timestamp for the state.
• state_id: A label to differentiate state instances.

from navlie.types import State 
import numpy as np

class VectorState(State):
    def __init__(self, value: np.ndarray, stamp: float = None, state_id=None):
        super(VectorState, self).__init__(
            value=value,
            dof=value.size,
            stamp=stamp,
            state_id=state_id,
        )

    def plus(self, dx: np.ndarray):
        new = self.copy()
        new.value = new.value + dx
        return new

    def minus(self, x: "VectorState") -> np.ndarray:
        return self.value - x.value

    def copy(self) -> "VectorState":
        return VectorState(self.value.copy(), self.stamp, self.state_id)

To define how a system evolves or how sensors observe it, you must implement the ProcessModel or MeasurementModel abstract classes. Both require the following methods:

• evaluate(...): Computes the state transition (for process models) or the expected observation (for measurement models).
• covariance(...): Returns the covariance of the process errors or measurement noise.
• jacobian(...) (Optional): Returns the Jacobian matrix. If not implemented, subclasses inherit jacobian_fd(), which uses finite-difference numerical differentiation.

ProcessModel Signature

evaluate(self, x: State, u: Input, dt: float) -> np.ndarray

MeasurementModel Signature

evaluate(self, x: State) -> np.ndarray

class SingleIntegrator(ProcessModel):
    def __init__(self, Q: np.ndarray):
        self._Q = Q

    def evaluate(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        x.value = x.value + dt * u.value
        return x

    def jacobian(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        return np.identity(x.dof)

    def covariance(self, x: VectorState, u: VectorInput, dt: float) -> np.ndarray:
        return dt**2 * self._Q

Navlie filters (like ExtendedKalmanFilter, UnscentedKalmanFilter, or IteratedKalmanFilter) are stateless. They do not store the current estimate internally. Instead, you must manage the state externally using a StateWithCovariance object.

Workflow:

1. Initialize the filter with the process_model.
2. Wrap your initial state in nav.StateWithCovariance(initial_state, initial_covariance).
3. In your loop:
   • Call filter.correct(x, measurement, input) to fuse measurements.
   • Call filter.predict(x, input, dt) to propagate the state forward.

This stateless design allows you to easily combine different filters (e.g., EKF for prediction and UKF for correction) in a single loop.

kalman_filter = nav.ExtendedKalmanFilter(process_model)

P0 = np.diag([0.1**2, 1**2, 1**2])
x = nav.StateWithCovariance(x, P0)

# Inside a loop:
# 1. Correction step
x = kalman_filter.correct(x, y, u)

# 2. Prediction step
dt = next_input_stamp - x.state.stamp
x = kalman_filter.predict(x, u, dt)

The MatrixLieGroupState class (and its specialized subclasses) represents states on a Lie Group. It handles perturbations using either "left" or "right" multiplication.

Subclasses

• SO2State: 2D rotations (2x2 matrix).
• SO3State: 3D rotations (3x3 matrix).
• SE2State: 2D rigid body transformations (3x3 matrix) with .attitude and .position properties.
• SE3State: 3D rigid body transformations (4x4 matrix) with .attitude and .position properties.
• SE23State: 2D rigid body transformations with velocity (5x5 matrix) with .attitude, .position, and .velocity properties.
• SL3State: Special linear group in 3D (3x3 matrix).

Key Concepts

• Direction: Defines how perturbations $\delta \mathbf{x}$ are applied:
  • "right" (default): $\mathbf{X} = \mathbf{X} \exp(\delta \mathbf{x}^\wedge)$
  • "left": $\mathbf{X} = \exp(\delta \mathbf{x}^\wedge) \mathbf{X}$
• Initialization: You can initialize with a vector of exponential coordinates (length equal to group DOF) or a direct matrix element.
• Arithmetic:
  • plus(dx): Applies a perturbation dx based on the direction.
  • minus(x): Computes the Lie algebra difference (log map) between two states.
  • dot(other): Performs group multiplication.

import numpy as np
from navlie.lib.states import SE3State

# Initialize with exponential coordinates (e.g., [roll, pitch, yaw, x, y, z])
val = np.zeros(6)
state = SE3State(value=val, direction="right")

# Access properties
pos = state.position
att = state.attitude

# Apply perturbation
dx = np.array([0.1, 0.0, 0.0, 0.0, 0.0, 0.0])
new_state = state.plus(dx)

The CompositeState class allows you to combine multiple states (potentially of different types) into a single state object. This is useful for representing complex system states, such as a robot's pose combined with sensor biases.

Key characteristics:

• The value attribute is a list of the constituent states (substates).
• The order of states in the list determines the order of elements in vectors during plus() and minus() operations.
• plus(), minus(), and copy() methods are automatically implemented based on the substates' implementations.
• Each substate should have a unique state_id to facilitate identification, especially when using CompositeMeasurementModel.

import navlie as nav 
import numpy as np

# Define individual states
T = nav.lib.SE2State(value=[0.1, 2.0, 3.0], stamp=0.0, state_id="pose")
b = nav.lib.VectorState(value=[0.1, 2.0], stamp=0.0, state_id="bias")

# Combine into a composite state
x = nav.CompositeState([T, b], stamp=0.0)

# Vector operations follow the order of substates
# If T has 3 DOF and b has 2 DOF, the vector is length 5
x_temp = x.plus(np.array([1, 2, 3, 4, 5]))
dx = x_temp.minus(x)