Measurement Models¶

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

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

    // Define a LEO circular orbit
    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]);

    // Truth propagator for generating simulated GNSS observations
    let mut truth_prop = bh::propagators::DNumericalOrbitPropagator::builder(
        epoch,
        true_state.clone(),
        bh::propagators::force_model_config::ForceModelConfig::two_body_gravity(),
    )
    .build()
    .unwrap();

    // Perturbed initial state: 1 km position error
    let mut initial_state = true_state.clone();
    initial_state[0] += 1000.0;
    initial_state[4] += 1.0;
    let p0 = DMatrix::from_diagonal(&DVector::from_vec(vec![
        1e6, 1e6, 1e6, 1e2, 1e2, 1e2,
    ]));

    // ECEF position model with typical GNSS accuracy (5 m noise)
    let ecef_model = bh::estimation::ECEFPositionMeasurementModel::new(5.0);
    let models: Vec<Box<dyn bh::estimation::MeasurementModel>> = vec![
        Box::new(ecef_model),
    ];

    let mut ekf = bh::estimation::ExtendedKalmanFilter::builder(
        epoch,
        initial_state,
        p0,
        bh::propagators::force_model_config::ForceModelConfig::two_body_gravity(),
        bh::estimation::EKFConfig::default(),
    )
    .measurement_models(models)
    .build()
    .unwrap();

    // Simulate GNSS observations: get truth ECI state, convert to ECEF
    let dt = 60.0;
    for i in 1..=20 {
        let obs_epoch = epoch + dt * i as f64;
        truth_prop.propagate_to(obs_epoch).unwrap();
        let truth_eci = truth_prop.current_state();

        // Simulate GNSS: convert truth position to ECEF
        let truth_eci_pos = nalgebra::Vector3::new(truth_eci[0], truth_eci[1], truth_eci[2]);
        let truth_ecef_pos = bh::frames::position_eci_to_ecef(obs_epoch, truth_eci_pos);
        let z = DVector::from_vec(vec![truth_ecef_pos[0], truth_ecef_pos[1], truth_ecef_pos[2]]);

        let obs = bh::estimation::Observation::new(obs_epoch, z, 0);
        ekf.process_observation(&obs).unwrap();
    }

    // Compare final state to truth
    use bh::propagators::traits::DStatePropagator;
    truth_prop.propagate_to(ekf.current_epoch()).unwrap();
    let truth_final = truth_prop.current_state();
    let final_state = ekf.current_state();
    let pos_error = (final_state.rows(0, 3) - truth_final.rows(0, 3)).norm();
    let vel_error = (final_state.rows(3, 3) - truth_final.rows(3, 3)).norm();

    println!("ECEF GNSS tracking with ECEFPositionMeasurementModel:");
    println!("  Initial position error: 1000.0 m");
    println!("  Final position error:   {:.2} m", pos_error);
    println!("  Final velocity error:   {:.4} m/s", vel_error);
    println!("  Observations processed: {}", ekf.records().len());
}

//! simulates az/el/range measurements during passes, and processes them with
//! an Extended Kalman Filter, propagating through gaps between passes.

#[allow(unused_imports)]
use brahe as bh;
use bh::access::{ElevationConstraint, location_accesses};
use bh::datasets::ssn_sensors::load_ssn_sensors;
use bh::estimation::{EKFConfig, ExtendedKalmanFilter, MeasurementModel, Observation, SimpleSSNSensor};
use bh::traits::{DStatePropagator, InterpolatableTrajectory};
use nalgebra::{DMatrix, DVector, SVector};

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

    // Configuration
    let meas_interval = 15.0; // seconds between measurements during a pass
    let duration = 2.0 * 3600.0; // tracking duration (seconds)
    let seed = 42u64;

    // Truth orbit: LEO at 700 km, 72 degree inclination
    let epoch = bh::Epoch::from_datetime(2024, 1, 1, 0, 0, 0.0, 0.0, bh::TimeSystem::UTC);
    let oe = SVector::<f64, 6>::new(bh::R_EARTH + 700e3, 0.001, 72.0, 30.0, 0.0, 0.0);
    let true_state = bh::state_koe_to_eci(oe, bh::AngleFormat::Degrees);

    // Hermite-cubic interpolation (the propagator default) lets the
    // trajectory, stored at the ~60 s adaptive-step cadence, be sampled
    // accurately at the much finer measurement cadence used for measurement
    // simulation.
    let truth_config = bh::NumericalPropagationConfig::default();
    let mut truth_prop = bh::DNumericalOrbitPropagator::new(
        epoch,
        DVector::from_column_slice(true_state.as_slice()),
        truth_config,
        bh::ForceModelConfig::two_body_gravity(),
        None, None, None, None,
    ).unwrap();
    let epoch_end = epoch + duration;
    truth_prop.propagate_to(epoch_end).unwrap();
    let truth_traj = truth_prop.trajectory().clone();

    // Build sensors from the Vallado SSN dataset (calibrated radar and optical
    // sites; radar measures az/el/range, optical measures angles-only az/el)
    let sites = load_ssn_sensors().unwrap();
    let mut sensors = SimpleSSNSensor::from_locations_calibrated(&sites, Some(seed));
    println!(
        "Loaded {} SSN sites, {} calibrated sensors",
        sites.len(),
        sensors.len()
    );

    // Find passes and simulate measurements only inside them
    let mut observations: Vec<Observation> = Vec::new();
    let mut pass_count = 0;
    for (i, sensor) in sensors.iter_mut().enumerate() {
        let constraint = ElevationConstraint::new(Some(sensor.el_min().max(1.0)), None).unwrap();
        let windows = location_accesses(
            sensor.location(),
            &truth_prop,
            epoch,
            epoch_end,
            &constraint,
            None,
            None,
        ).unwrap();
        for w in windows {
            let obs = sensor
                .simulate_observations(&truth_traj, w.start(), w.end(), meas_interval, i)
                .unwrap();
            if !obs.is_empty() {
                pass_count += 1;
            }
            observations.extend(obs);
        }
    }

    observations.sort_by(|a, b| a.epoch.partial_cmp(&b.epoch).unwrap());
    println!(
        "Simulated {} measurements over {} passes",
        observations.len(),
        pass_count
    );

    // EKF from a perturbed initial state, using each sensor's matching model
    let mut initial_state = DVector::from_column_slice(true_state.as_slice());
    initial_state[0] += 1000.0;
    initial_state[4] += 1.0;
    let p0 = DMatrix::from_diagonal(&DVector::from_vec(vec![
        1e6, 1e6, 1e6, 1e2, 1e2, 1e2,
    ]));

    let models: Vec<Box<dyn MeasurementModel>> =
        sensors.iter().map(|s| s.measurement_model()).collect();

    let mut ekf = ExtendedKalmanFilter::new(
        epoch,
        initial_state,
        p0,
        bh::NumericalPropagationConfig::default(),
        bh::ForceModelConfig::two_body_gravity(),
        None,
        None,
        None,
        models,
        EKFConfig::default(),
    ).unwrap();

    // Process observations in order; propagate through gaps between passes
    let gap_split = 600.0; // start a new arc when consecutive obs are > 10 min apart
    let mut prev_epoch = epoch;
    for obs in &observations {
        if obs.epoch - prev_epoch > gap_split {
            // advance through the gap in 60 s steps to record covariance growth
            let mut t = prev_epoch + 60.0;
            while t < obs.epoch {
                ekf.propagate_to(t).unwrap();
                t += 60.0;
            }
        }
        ekf.process_observation(obs).unwrap();
        prev_epoch = obs.epoch;
    }

    // Compare final estimate to truth
    let truth_final = truth_traj.interpolate(&ekf.current_epoch()).unwrap();
    let final_state = ekf.current_state();
    let err = (final_state.rows(0, 3) - truth_final.rows(0, 3)).norm();
    println!("Final position error: {:.1} m", err);

    let cov = ekf.current_covariance();
    println!(
        "Final position 1-sigma: [{:.1}, {:.1}, {:.1}] m",
        cov[(0, 0)].sqrt(),
        cov[(1, 1)].sqrt(),
        cov[(2, 2)].sqrt()
    );

    assert!(err < 500.0, "EKF should converge to a small position error");
    println!("Example validated successfully!");
}

use brahe as bh;
use brahe::estimation::MeasurementModel;
use nalgebra::DMatrix;

fn main() {
    // --- Scalar sigma: same noise on all axes ---
    let model = bh::estimation::ECEFPositionMeasurementModel::new(5.0);
    let r = model.noise_covariance();
    println!("Scalar (5 m isotropic):");
    println!("  diag = [{:.1}, {:.1}, {:.1}]", r[(0,0)], r[(1,1)], r[(2,2)]);

    // --- Per-axis sigma: different noise per component ---
    let model = bh::estimation::ECEFPositionMeasurementModel::new_per_axis(3.0, 3.0, 8.0);
    let r = model.noise_covariance();
    println!("\nPer-axis (3, 3, 8 m):");
    println!("  diag = [{:.1}, {:.1}, {:.1}]", r[(0,0)], r[(1,1)], r[(2,2)]);

    // --- Full covariance: captures cross-axis correlations ---
    let mut cov = DMatrix::zeros(3, 3);
    cov[(0,0)] = 9.0; cov[(0,1)] = 1.0;
    cov[(1,0)] = 1.0; cov[(1,1)] = 9.0;
    cov[(2,2)] = 64.0;
    let model = bh::estimation::ECEFPositionMeasurementModel::from_covariance(cov).unwrap();
    let r = model.noise_covariance();
    println!("\nFull covariance (with correlations):");
    println!("  (0,0)={:.1} (0,1)={:.1} (2,2)={:.1}", r[(0,0)], r[(0,1)], r[(2,2)]);

    // --- Upper-triangular: compact packed form ---
    // Elements: [c00, c01, c02, c11, c12, c22]
    let model = bh::estimation::ECEFPositionMeasurementModel::from_upper_triangular(
        &[9.0, 1.0, 0.0, 9.0, 0.0, 64.0]
    ).unwrap();
    let r = model.noise_covariance();
    println!("\nUpper-triangular packed:");
    println!("  (0,0)={:.1} (0,1)={:.1} (2,2)={:.1}", r[(0,0)], r[(0,1)], r[(2,2)]);

    // --- Standalone covariance helpers ---
    let r = bh::math::covariance::isotropic_covariance(3, 10.0);
    println!("\nisotropic_covariance(3, 10.0) diag: [{:.0}, {:.0}, {:.0}]",
        r[(0,0)], r[(1,1)], r[(2,2)]);

    let r = bh::math::covariance::diagonal_covariance(&[5.0, 10.0, 15.0]);
    println!("diagonal_covariance([5, 10, 15]) diag: [{:.0}, {:.0}, {:.0}]",
        r[(0,0)], r[(1,1)], r[(2,2)]);
}