Batch Least Squares¶

Batch Least Squares (BLS) processes an entire arc of observations simultaneously, iterating to minimize the weighted sum of squared residuals. Unlike the EKF and UKF which update sequentially, BLS re-linearizes the full problem at each iteration, making it the standard approach for offline orbit determination when the complete observation set is available.

Golden-Path Example¶

PythonRust

import numpy as np

import brahe as bh

bh.initialize_eop()

# Define truth orbit: LEO circular at ~500km altitude
epoch = bh.Epoch(2024, 1, 1, 0, 0, 0.0)
r = bh.R_EARTH + 500e3
v = (bh.GM_EARTH / r) ** 0.5
true_state = np.array([r, 0.0, 0.0, 0.0, v, 0.0])

# Generate truth trajectory and noise-free position observations
truth_prop = bh.NumericalOrbitPropagator(
    epoch,
    true_state,
    bh.NumericalPropagationConfig.default(),
    bh.ForceModelConfig.two_body(),
)

observations = []
for i in range(1, 21):
    t = epoch + i * 30.0
    truth_prop.propagate_to(t)
    pos = truth_prop.current_state()[:3]
    observations.append(bh.Observation(t, pos, 0))

# Initial guess: perturbed by 1km in x-position
initial_state = true_state.copy()
initial_state[0] += 1000.0

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

# Create and run BLS
bls = bh.BatchLeastSquares(
    epoch,
    initial_state,
    p0,
    propagation_config=bh.NumericalPropagationConfig.default(),
    force_config=bh.ForceModelConfig.two_body(),
    measurement_models=[bh.InertialPositionMeasurementModel(10.0)],
)

bls.solve(observations)

# Results
print(f"Converged: {bls.converged()}")
print(f"Iterations: {bls.iterations_completed()}")
print(f"Final cost: {bls.final_cost():.6e}")

pos_error = np.linalg.norm(bls.current_state()[:3] - true_state[:3])
print(f"Position error: {pos_error:.6f} m")

# Iteration history
for rec in bls.iteration_records():
    print(
        f"  Iter {rec.iteration}: cost={rec.cost:.6e}, ||dx||={rec.state_correction_norm:.6e}"
    )

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

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

    // Define truth orbit: LEO circular at ~500km altitude
    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 true_state = DVector::from_vec(vec![r, 0.0, 0.0, 0.0, v, 0.0]);

    // Create a truth propagator for generating observations
    let mut truth_prop = bh::propagators::DNumericalOrbitPropagator::new(
        epoch,
        true_state.clone(),
        bh::propagators::NumericalPropagationConfig::default(),
        bh::propagators::force_model_config::ForceModelConfig::two_body_gravity(),
        None, None, None, None,
    ).unwrap();

    // Generate 20 noise-free position observations at 30-second intervals
    let mut observations = Vec::new();
    for i in 1..=20 {
        let obs_epoch = epoch + 30.0 * i as f64;
        truth_prop.propagate_to(obs_epoch);
        let truth_pos = truth_prop.current_state().rows(0, 3).into_owned();
        observations.push(bh::estimation::Observation::new(obs_epoch, truth_pos, 0));
    }

    // Initial guess: perturbed by 1km in x-position
    let mut initial_state = true_state.clone();
    initial_state[0] += 1000.0;

    // A priori covariance
    let p0 = DMatrix::from_diagonal(&DVector::from_vec(vec![
        1e6, 1e6, 1e6, 1e2, 1e2, 1e2,
    ]));

    // Create and run BLS
    let models: Vec<Box<dyn bh::estimation::MeasurementModel>> = vec![
        Box::new(bh::estimation::InertialPositionMeasurementModel::new(10.0)),
    ];

    let mut bls = bh::estimation::BatchLeastSquares::new(
        epoch,
        initial_state,
        p0,
        bh::propagators::NumericalPropagationConfig::default(),
        bh::propagators::force_model_config::ForceModelConfig::two_body_gravity(),
        None, None, None,
        models,
        bh::estimation::BLSConfig::default(),
    ).unwrap();

    bls.solve(&observations).unwrap();

    // Results
    println!("Converged: {}", bls.converged());
    println!("Iterations: {}", bls.iterations_completed());
    println!("Final cost: {:.6e}", bls.final_cost());

    let pos_error = (bls.current_state().rows(0, 3) - true_state.rows(0, 3)).norm();
    println!("Position error: {:.6} m", pos_error);

    // Iteration history
    for rec in bls.iteration_records() {
        println!("  Iter {}: cost={:.6e}, ||dx||={:.6e}",
            rec.iteration, rec.cost, rec.state_correction_norm);
    }
}

Output

PythonRust

Converged: True
Iterations: 3
Final cost: 9.999770e-01
Position error: 0.022992 m
  Iter 0: cost=2.706892e+05, ||dx||=9.999610e+02
  Iter 1: cost=1.000445e+00, ||dx||=1.907885e-02
  Iter 2: cost=9.999770e-01, ||dx||=5.337234e-10

Converged: true
Iterations: 3
Final cost: 9.999770e-1
Position error: 0.022992 m
  Iter 0: cost=2.706892e5, ||dx||=9.999610e2
  Iter 1: cost=1.000445e0, ||dx||=1.907885e-2
  Iter 2: cost=9.999770e-1, ||dx||=5.337234e-10

How It Works¶

BLS uses Gauss-Newton iteration to refine the state estimate. Starting from an initial guess \(\mathbf{x}_0\), the algorithm propagates the state to each observation epoch, computes predicted measurements \(h(\mathbf{x}, t_i)\), and forms the residual vector \(\mathbf{y}_i = \mathbf{z}_i - h(\mathbf{x}, t_i)\). The State Transition Matrix \(\Phi\) maps state perturbations from the reference epoch to each observation time, allowing the measurement Jacobians to be expressed in terms of the state correction at the reference epoch.

At each iteration, the algorithm solves a linear least squares problem for the state correction \(\delta\mathbf{x}\) that minimizes the weighted residual cost function:

\[ J = \frac{1}{2} \left[ \delta\mathbf{x}^T P_0^{-1} \delta\mathbf{x} + \sum_i (\mathbf{y}_i - \tilde{H}_i \, \delta\mathbf{x})^T R_i^{-1} (\mathbf{y}_i - \tilde{H}_i \, \delta\mathbf{x}) \right] \]

where \(\tilde{H}_i = H_i \Phi(t_i, t_0)\) maps the state correction at the reference epoch to the observation space at time \(t_i\), and \(P_0\) is the a priori covariance. The state is updated, the trajectory is re-propagated from the corrected initial conditions, and the process repeats until convergence.

Solver Formulations¶

Brahe provides two equivalent formulations for solving the linear system at each iteration.

Normal Equations (default) accumulates the information matrix \(\Lambda = P_0^{-1} + \sum_i \tilde{H}_i^T R_i^{-1} \tilde{H}_i\) and normal vector \(\mathbf{N} = -P_0^{-1} \delta\mathbf{x}_0 + \sum_i \tilde{H}_i^T R_i^{-1} \mathbf{y}_i\), then solves \(\Lambda \, \delta\mathbf{x} = \mathbf{N}\) via Cholesky decomposition. This requires only \(O(n^2)\) memory where \(n\) is the state dimension, making it efficient for large observation sets.

Stacked Observation Matrix builds the full \(\tilde{H}\) matrix and residual vector across all observations, then solves via QR decomposition. This uses \(O(m \times n)\) memory where \(m\) is the total measurement dimension, but provides better numerical conditioning when the problem is poorly scaled or near-singular.

Consider Parameters¶

When some state elements are uncertain but should not be estimated (e.g., drag coefficient, sensor biases), the consider parameter formulation accounts for their effect on the solution covariance without including them in the solve. The state vector is partitioned into solve-for parameters (first \(n_s\) elements) and consider parameters (remaining \(n_c\) elements). The solve only corrects the solve-for parameters, but the final covariance reflects the additional uncertainty from the consider parameters through the sensitivity matrix \(S_c\).

Configure consider parameters via ConsiderParameterConfig, specifying n_solve (the number of solve-for parameters) and consider_covariance (the \(n_c \times n_c\) a priori covariance for the consider parameters).

Convergence Criteria¶

BLS supports two convergence criteria, configured via BLSConfig:

State correction norm (state_correction_threshold) declares convergence when \(\|\delta\mathbf{x}\|\) falls below the threshold. This is enabled by default with a threshold of \(10^{-8}\). It directly measures whether the state estimate has stabilized.

Relative cost change (cost_convergence_threshold) declares convergence when \(|\Delta J| / |J|\) falls below the threshold. This is useful when the absolute state correction is large but the cost function is no longer improving meaningfully.

If both are set, the solver converges when either criterion is satisfied. The solver also terminates when max_iterations is reached, regardless of convergence.

Diagnostics¶

After calling solve(), iteration-level diagnostics are available through iteration_records(), which returns a BLSIterationRecord for each iteration containing the cost function value, state correction norm, and RMS residuals. Monitoring the cost across iterations confirms that the solver is converging (decreasing cost) and has not stalled.

Per-observation residuals are available through observation_residuals(), returning BLSObservationResidual entries with pre-fit and post-fit residuals for each measurement. Large post-fit residuals can indicate outlier observations or an incorrect measurement model. Systematic patterns in residuals across time may reveal unmodeled dynamics.

The converged(), iterations_completed(), and final_cost() accessors provide summary status without inspecting individual records.