Extended Kalman Filter¶

The Extended Kalman Filter (EKF) is a sequential estimator that linearizes dynamics and measurement models around the current state estimate. At each observation, it propagates state and covariance forward in time (predict step), then incorporates the measurement (update step). Brahe's EKF leverages the propagator's built-in State Transition Matrix (STM) for covariance prediction, so no separate covariance propagation is needed.

Setting Up¶

ExtendedKalmanFilter.builder() is the primary way to construct an EKF: it takes the five required inputs -- epoch, state, initial_covariance, force_config, and config -- directly as arguments, and measurement models and remaining optional inputs are set through chained setters. It internally builds a numerical orbit propagator with STM enabled.

PythonRust

import numpy as np

import brahe as bh

bh.initialize_eop()

epoch = bh.Epoch(2024, 1, 1, 0, 0, 0.0)
state = np.array([bh.R_EARTH + 500e3, 0.0, 0.0, 0.0, 7612.0, 0.0])
p0 = np.diag([1e6, 1e6, 1e6, 1e2, 1e2, 1e2])

ekf = (
    bh.ExtendedKalmanFilter.builder(
        epoch, state, p0, bh.ForceModelConfig.two_body(), bh.EKFConfig()
    )
    .measurement_model(bh.InertialPositionMeasurementModel(10.0))
    .build()
)

print(f"EKF state dimension: {ekf.current_state().shape[0]}")
print(f"EKF current epoch: {ekf.current_epoch()}")

use brahe::estimation::*;
use brahe::propagators::*;
use brahe::time::{Epoch, TimeSystem};
use nalgebra::{DMatrix, DVector};

fn main() {
    brahe::initialize_eop().unwrap();

    let epoch = Epoch::from_datetime(2024, 1, 1, 0, 0, 0.0, 0.0, TimeSystem::UTC);
    let state = DVector::from_vec(vec![brahe::R_EARTH + 500e3, 0.0, 0.0, 0.0, 7612.0, 0.0]);
    let p0 = DMatrix::from_diagonal(&DVector::from_vec(vec![1e6, 1e6, 1e6, 1e2, 1e2, 1e2]));

    let ekf = ExtendedKalmanFilter::builder(
        epoch,
        state,
        p0,
        ForceModelConfig::two_body_gravity(),
        EKFConfig::default(),
    )
    .measurement_model(Box::new(InertialPositionMeasurementModel::new(10.0)))
    .build()
    .unwrap();

    println!("EKF state dimension: {}", ekf.current_state().len());
    println!("EKF current epoch: {}", ekf.current_epoch());
}

Output

PythonRust

1 2	`EKF state dimension: 6 EKF current epoch: 2024-01-01 00:00:00.000 UTC`

1 2	`EKF state dimension: 6 EKF current epoch: 2024-01-01 00:00:00.000 UTC`

The flat constructor remains available as an alternative, taking every field as a keyword argument in Python and positionally in Rust:

PythonRust

import numpy as np

import brahe as bh

bh.initialize_eop()

epoch = bh.Epoch(2024, 1, 1, 0, 0, 0.0)
state = np.array([bh.R_EARTH + 500e3, 0.0, 0.0, 0.0, 7612.0, 0.0])
p0 = np.diag([1e6, 1e6, 1e6, 1e2, 1e2, 1e2])

ekf = bh.ExtendedKalmanFilter(
    epoch,
    state,
    p0,
    measurement_models=[bh.InertialPositionMeasurementModel(10.0)],
    propagation_config=bh.NumericalPropagationConfig.default(),
    force_config=bh.ForceModelConfig.two_body(),
)

print(f"EKF state dimension: {ekf.current_state().shape[0]}")
print(f"EKF current epoch: {ekf.current_epoch()}")

use brahe::estimation::*;
use brahe::propagators::*;
use brahe::time::{Epoch, TimeSystem};
use nalgebra::{DMatrix, DVector};

fn main() {
    brahe::initialize_eop().unwrap();

    let epoch = Epoch::from_datetime(2024, 1, 1, 0, 0, 0.0, 0.0, TimeSystem::UTC);
    let state = DVector::from_vec(vec![brahe::R_EARTH + 500e3, 0.0, 0.0, 0.0, 7612.0, 0.0]);
    let p0 = DMatrix::from_diagonal(&DVector::from_vec(vec![1e6, 1e6, 1e6, 1e2, 1e2, 1e2]));

    let ekf = ExtendedKalmanFilter::new(
        epoch,
        state,
        p0,
        NumericalPropagationConfig::default(),
        ForceModelConfig::two_body_gravity(),
        None,
        None,
        None,
        vec![Box::new(InertialPositionMeasurementModel::new(10.0))],
        EKFConfig::default(),
    )
    .unwrap();

    println!("EKF state dimension: {}", ekf.current_state().len());
    println!("EKF current epoch: {}", ekf.current_epoch());
}

Output

PythonRust

1 2	`EKF state dimension: 6 EKF current epoch: 2024-01-01 00:00:00.000 UTC`

1 2	`EKF state dimension: 6 EKF current epoch: 2024-01-01 00:00:00.000 UTC`

Both paths ensure STM propagation is enabled regardless of the propagation config passed in. The initial covariance matrix dimensions must match the state vector length.

Processing Observations¶

Each observation pairs a measurement vector with a timestamp and a model index indicating which measurement model to use.

One at a time with process_observation(), which enforces strict chronological ordering and returns a FilterRecord with full diagnostics:

obs = bh.Observation(epoch + 60.0, np.array([6878e3, 100.0, 50.0]), model_index=0)
record = ekf.process_observation(obs)

In batch with process_observations(), which auto-sorts by epoch before processing:

observations = [
    bh.Observation(epoch + 60.0 * i, truth_pos, model_index=0)
    for i in range(1, 31)
]
ekf.process_observations(observations)

Different measurement types can be interleaved by setting different model_index values. For example, position observations at index 0 and range observations at index 1 can arrive in any order when using process_observations().

Accessing Results¶

After processing, the filter state is available through pass-through methods:

state = ekf.current_state()         # numpy 1D array
cov = ekf.current_covariance()      # numpy 2D array
epoch = ekf.current_epoch()         # Epoch
records = ekf.records()             # list of FilterRecord

Each FilterRecord captures the complete diagnostic state of a single update:

Field	Description
`state_predicted`	State after propagation, before measurement update
`covariance_predicted`	Covariance after propagation, before update
`state_updated`	State after measurement incorporation
`covariance_updated`	Covariance after measurement incorporation
`prefit_residual`	\(\mathbf{z} - h(\mathbf{x}_{pred})\) -- innovation
`postfit_residual`	\(\mathbf{z} - h(\mathbf{x}_{upd})\) -- should be small
`kalman_gain`	\(K\) matrix used for the update
`measurement_name`	Name of the measurement model used

Pre-fit residuals indicate how well the predicted state matches the observation. Post-fit residuals should be smaller, confirming the update improved the estimate. Monitoring these over time is the primary tool for assessing filter health.

Process Noise¶

Process noise accounts for unmodeled dynamics (drag variations, gravity model truncation, etc.) by inflating the predicted covariance at each step. Without process noise, the covariance monotonically decreases and the filter eventually ignores new measurements.

q = np.diag([1e-6, 1e-6, 1e-6, 1e-8, 1e-8, 1e-8])
pn = bh.ProcessNoiseConfig(q, scale_with_dt=True)
config = bh.EKFConfig(process_noise=pn)

ekf = bh.ExtendedKalmanFilter(
    epoch, state, p0,
    measurement_models=[bh.InertialPositionMeasurementModel(10.0)],
    propagation_config=bh.NumericalPropagationConfig.default(),
    force_config=bh.ForceModelConfig.two_body(),
    config=config,
)

When scale_with_dt=True, the effective process noise is \(Q \cdot \Delta t\) (continuous-time model). When False, \(Q\) is applied as-is at each step (discrete-time model).

Sub-stepping Over Long Gaps¶

By default the process noise for a predict interval is added once, scaled by the full \(\Delta t\). Over a long coast arc this injects all of the noise at the start of the interval, which understates the covariance growth that the nonlinear dynamics produce as earlier noise shears along the trajectory. The max_noise_dt argument caps the time step over which continuous noise is accumulated per chunk:

pn = bh.ProcessNoiseConfig(q, scale_with_dt=True, max_noise_dt=60.0)

With max_noise_dt=h, a predict interval longer than h is split into chunks of at most h seconds; the covariance is propagated through each chunk and \(Q \cdot \delta t_{chunk}\) is added per chunk, injecting the noise along the trajectory rather than all at once. A single sub-stepped predict is by construction equivalent to a sequence of single-shot predicts at cadence h. Sub-stepping applies only when scale_with_dt=True (a continuous noise rate); a discrete per-update \(Q\) (scale_with_dt=False) is applied once per interval regardless of max_noise_dt. max_noise_dt=None (the default) preserves the single-shot behavior. This is an approximation using a constant rate per chunk; the exact variational discrete-time \(Q_d\) integration is tracked in issue #408.

Using Custom Dynamics¶

For systems beyond standard orbital mechanics, you can supply custom dynamics. In both languages the EKF constructor accepts an additional_dynamics function (along with force model params and a control_input) that passes through to the internal orbit propagator. Alternatively, in Rust, build a DNumericalPropagator with a fully custom dynamics function and pass it to from_propagator():

PythonRust

import numpy as np

import brahe as bh

bh.initialize_eop()

# Define a LEO circular orbit
epoch = bh.Epoch(2024, 1, 1, 0, 0, 0.0)
r = bh.R_EARTH + 500e3
v = (bh.GM_EARTH / r) ** 0.5
state = np.array([r, 0.0, 0.0, 0.0, v, 0.0])

# Initial covariance
p0 = np.diag([1e6, 1e6, 1e6, 1e2, 1e2, 1e2])


# Define custom two-body dynamics as additional acceleration
def two_body_dynamics(t, state, params):
    """Custom two-body gravitational acceleration."""
    pos = state[:3]
    r_mag = np.linalg.norm(pos)
    accel = -bh.GM_EARTH / r_mag**3 * pos
    deriv = np.zeros(6)
    deriv[:3] = state[3:6]  # velocity
    deriv[3:6] = accel  # acceleration
    return deriv


# Create EKF with custom dynamics via additional_dynamics parameter
ekf = bh.ExtendedKalmanFilter(
    epoch,
    state,
    p0,
    measurement_models=[bh.InertialPositionMeasurementModel(10.0)],
    propagation_config=bh.NumericalPropagationConfig.default(),
    force_config=bh.ForceModelConfig.two_body(),
    additional_dynamics=two_body_dynamics,
)

# Process a single observation using truth position
obs = bh.Observation(epoch + 60.0, state[:3], model_index=0)
record = ekf.process_observation(obs)

print("Custom dynamics EKF:")
print(f"  Prefit residual norm: {np.linalg.norm(record.prefit_residual):.3f} m")
print(f"  Postfit residual norm: {np.linalg.norm(record.postfit_residual):.6f} m")
print(f"  State dim: {len(ekf.current_state())}")

use brahe as bh;
use nalgebra::{DMatrix, DVector};

fn main() {
    bh::initialize_eop().unwrap();

    let epoch = bh::time::Epoch::from_datetime(2024, 1, 1, 0, 0, 0.0, 0.0, bh::time::TimeSystem::UTC);
    let r = bh::constants::physical::R_EARTH + 500e3;
    let v = (bh::constants::physical::GM_EARTH / r).sqrt();
    let state = DVector::from_vec(vec![r, 0.0, 0.0, 0.0, v, 0.0]);
    let p0 = DMatrix::from_diagonal(&DVector::from_vec(vec![1e6, 1e6, 1e6, 1e2, 1e2, 1e2]));

    // Define custom two-body dynamics
    let dynamics: bh::integrators::traits::DStateDynamics = Box::new(
        |_t, state: &DVector<f64>, _params| {
            let r_vec = state.rows(0, 3);
            let v_vec = state.rows(3, 3);
            let r_mag = r_vec.norm();
            let mu = bh::constants::physical::GM_EARTH;
            let a = -mu / r_mag.powi(3) * &r_vec;
            let mut dx = DVector::zeros(6);
            dx.rows_mut(0, 3).copy_from(&v_vec);
            dx.rows_mut(3, 3).copy_from(&a);
            Ok(dx)
        },
    );

    // Build a generic propagator with STM enabled
    let mut prop_config = bh::propagators::NumericalPropagationConfig::default();
    prop_config.variational.enable_stm = true;

    let prop = bh::propagators::DNumericalPropagator::builder(epoch, state.clone(), dynamics)
        .propagation_config(prop_config)
        .initial_covariance(p0)
        .build()
        .unwrap();

    // Create EKF from the pre-built propagator (converts into a DynamicsSource
    // automatically; the filter starts from the propagator's initial covariance)
    let models: Vec<Box<dyn bh::estimation::MeasurementModel>> = vec![
        Box::new(bh::estimation::InertialPositionMeasurementModel::new(10.0)),
    ];

    let mut ekf = bh::estimation::ExtendedKalmanFilter::from_propagator(
        prop,
        models,
        bh::estimation::EKFConfig::default(),
    ).unwrap();

    // Process a single observation
    let obs = bh::estimation::Observation::new(epoch + 60.0, state.rows(0, 3).into_owned(), 0);
    let record = ekf.process_observation(&obs).unwrap();

    println!("Custom dynamics EKF:");
    println!("  Prefit residual norm: {:.3} m", record.prefit_residual.norm());
    println!("  Postfit residual norm: {:.6} m", record.postfit_residual.norm());
    println!("  State dim: {}", ekf.current_state().len());
}

Output

PythonRust

Custom dynamics EKF:
  Prefit residual norm: 456672.591 m
  Postfit residual norm: 33.698475 m
  State dim: 6

Custom dynamics EKF:
  Prefit residual norm: 456672.591 m
  Postfit residual norm: 33.698475 m
  State dim: 6

See General Dynamics Propagation for details on the dynamics function signature.

Filter Equations¶

The EKF alternates between a predict step (propagate to the next observation) and an update step (incorporate the measurement). Below are the equations implemented by Brahe's EKF.

Predict¶

The state is propagated from \(t_{k-1}\) to \(t_k\) using the nonlinear dynamics \(f(\mathbf{x}, t)\), and the covariance is propagated using the State Transition Matrix \(\Phi\):

\[ \mathbf{x}_{k}^{-} = f(\mathbf{x}_{k-1}^{+},\; t_{k-1} \to t_k) \]

\[ P_{k}^{-} = \Phi_k \, P_{k-1}^{+} \, \Phi_k^T + Q_k \]

where \(\Phi_k\) is the STM integrated alongside the state, and \(Q_k\) is the process noise matrix (optionally scaled by \(\Delta t\)).

Update¶

Given observation \(\mathbf{z}_k\) and measurement model \(h(\mathbf{x})\) with noise covariance \(R\):

Innovation (pre-fit residual):

\[ \mathbf{y}_k = \mathbf{z}_k - h(\mathbf{x}_{k}^{-}) \]

Measurement Jacobian:

\[ H_k = \frac{\partial h}{\partial \mathbf{x}} \bigg|_{\mathbf{x}_{k}^{-}} \]

Innovation covariance:

\[ S_k = H_k \, P_{k}^{-} \, H_k^T + R \]

Kalman gain:

\[ K_k = P_{k}^{-} \, H_k^T \, S_k^{-1} \]

State update:

\[ \mathbf{x}_{k}^{+} = \mathbf{x}_{k}^{-} + K_k \, \mathbf{y}_k \]

Covariance update (Joseph form):

\[ P_{k}^{+} = (I - K_k H_k) \, P_{k}^{-} \, (I - K_k H_k)^T + K_k \, R \, K_k^T \]

The Joseph form is numerically more stable than the simpler \(P^{+} = (I - KH) P^{-}\) and guarantees the updated covariance remains symmetric positive semi-definite.

Extended Kalman Filter¶

Setting Up¶

Processing Observations¶

Accessing Results¶

Process Noise¶

Sub-stepping Over Long Gaps¶

Using Custom Dynamics¶

Filter Equations¶

Predict¶

Update¶

See Also¶