Merge pull request #425 from nyx-space/feat/85-batch-least-squares

Add batch least squares estimators. Fix #85.

Author: ChristopherRabotin

src/cosmic/mod.rs

@@ -65,7 +65,7 @@ where
         )
     }
 
-    /// Return this state as a vector for the propagation/estimation
+    /// Return the state's _step_ state transition matrix.
     /// By default, this is not implemented. This function must be implemented when filtering on this state.
     fn stm(&self) -> Result<OMatrix<f64, Self::Size, Self::Size>, DynamicsError> {
         Err(DynamicsError::StateTransitionMatrixUnset)
@@ -74,8 +74,7 @@ where
     /// Copies the current state but sets the STM to identity
     fn with_stm(self) -> Self;
 
-    /// Return this state as a vector for the propagation/estimation
-    /// By default, this is not implemented. This function must be implemented when filtering on this state.
+    /// Resets the STM, unimplemented by default.
     fn reset_stm(&mut self) {
         unimplemented!()
     }
@@ -102,6 +101,7 @@ where
 
     /// Retrieve the Epoch
     fn epoch(&self) -> Epoch;
+
     /// Set the Epoch
     fn set_epoch(&mut self, epoch: Epoch);
 

src/od/blse/mod.rs

@@ -0,0 +1,93 @@
+/*
+    Nyx, blazing fast astrodynamics
+    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
+
+    This program is free software: you can redistribute it and/or modify
+    it under the terms of the GNU Affero General Public License as published
+    by the Free Software Foundation, either version 3 of the License, or
+    (at your option) any later version.
+
+    This program is distributed in the hope that it will be useful,
+    but WITHOUT ANY WARRANTY; without even the implied warranty of
+    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+    GNU Affero General Public License for more details.
+
+    You should have received a copy of the GNU Affero General Public License
+    along with this program.  If not, see <https://www.gnu.org/licenses/>.
+*/
+
+#![allow(clippy::type_complexity)] // Allow complex types for generics
+#![allow(unused_imports)] // Keep imports for context even if slightly unused in snippet
+
+use crate::linalg::allocator::Allocator;
+use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector, U1}; // Use U1 for MsrSize
+use crate::md::trajectory::{Interpolatable, Traj}; // May not need Traj if we propagate point-to-point
+pub use crate::od::estimate::*;
+pub use crate::od::ground_station::*;
+pub use crate::od::snc::*; // SNC not typically used in BLS, but keep context
+pub use crate::od::*;
+use crate::propagators::Propagator;
+pub use crate::time::{Duration, Epoch, Unit};
+use anise::prelude::Almanac;
+use indexmap::IndexSet;
+use log::{debug, info, trace, warn};
+use msr::sensitivity::TrackerSensitivity; // Assuming this is the correct path
+use snafu::prelude::*;
+use std::collections::BTreeMap;
+use std::fmt;
+use std::marker::PhantomData;
+use std::ops::Add;
+use std::sync::Arc;
+use typed_builder::TypedBuilder;
+
+// Define a simple BLS solution struct
+#[derive(Debug, Clone)]
+pub struct BLSSolution<StateType: State>
+where
+    DefaultAllocator: Allocator<<StateType as State>::Size>
+        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
+        + Allocator<<StateType as State>::VecLength>,
+{
+    pub estimated_state: StateType,
+    pub covariance: OMatrix<f64, <StateType as State>::Size, <StateType as State>::Size>,
+    pub num_iterations: usize,
+    pub final_rms: f64,
+    pub final_corr_pos_km: f64,
+    pub converged: bool,
+}
+
+impl<StateType> fmt::Display for BLSSolution<StateType>
+where
+    StateType: State,
+    DefaultAllocator: Allocator<<StateType as State>::Size>
+        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
+        + Allocator<<StateType as State>::VecLength>,
+{
+    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
+        writeln!(f, "Converged: {}", self.converged)?;
+        writeln!(f, "Iterations: {}", self.num_iterations)?;
+        writeln!(f, "Final RMS: {}", self.final_rms)?;
+        writeln!(f, "Final State: {}", self.estimated_state.orbit())?;
+        write!(f, "Final Covariance:\n{:.3e}", self.covariance)
+    }
+}
+
+impl<StateType: State> From<BLSSolution<StateType>> for KfEstimate<StateType>
+where
+    DefaultAllocator: Allocator<<StateType as State>::Size>
+        + Allocator<<StateType as State>::Size, <StateType as State>::Size>
+        + Allocator<<StateType as State>::VecLength>,
+    <DefaultAllocator as Allocator<<StateType as State>::Size>>::Buffer<f64>: Copy,
+    <DefaultAllocator as Allocator<<StateType as State>::Size, <StateType as State>::Size>>::Buffer<
+        f64,
+    >: Copy,
+{
+    fn from(mut bls: BLSSolution<StateType>) -> Self {
+        // Set the uncertainty on Cr, Cd, mass to zero
+        bls.covariance[(6, 6)] = 0.0;
+        bls.covariance[(7, 7)] = 0.0;
+        bls.covariance[(8, 8)] = 0.0;
+
+        Self::from_covar(bls.estimated_state, bls.covariance)
+    }
+}

src/od/blse/solution.rs

@@ -58,6 +58,9 @@ pub use simulator::TrackingDevice;
 /// Provides all state noise compensation functionality
 pub mod snc;
 
+/// Provides the Batch least squares initial state solver.
+pub mod blse;
+
 /// A helper type for spacecraft orbit determination.
 pub type SpacecraftKalmanOD = self::process::KalmanODProcess<
     SpacecraftDynamics,
@@ -110,6 +113,8 @@ pub enum ODError {
     InvalidMeasurement { epoch: Epoch, val: f64 },
     #[snafu(display("Kalman gain is singular"))]
     SingularKalmanGain,
+    #[snafu(display("Information matrix is singular"))]
+    SingularInformationMatrix,
     #[snafu(display("noise matrix is singular"))]
     SingularNoiseRk,
     #[snafu(display("{kind} noise not configured"))]
@@ -142,6 +147,8 @@ pub enum ODError {
         source: Box<StateError>,
         action: &'static str,
     },
+    #[snafu(display("Maximum iterations ({max_iter}) reached without convergence"))]
+    ODMaxIterations { max_iter: usize },
     #[snafu(display("Nyx orbit determination limitation: {action}"))]
     ODLimitation { action: &'static str },
 }

src/od/filter/mod.rs

@@ -213,7 +213,7 @@ where
             loop {
                 let delta_t = next_msr_epoch - epoch;
 
-                // Propagator for the minimum time between the maximum step size, the next step size, and the duration to the next measurement.
+                // Propagate for the minimum time between the maximum step size, the next step size, and the duration to the next measurement.
                 let next_step_size = delta_t.min(prop_instance.step_size).min(self.max_step);
 
                 // Remove old states from the trajectory
@@ -361,7 +361,7 @@ where
                                     msr_accepted_cnt += 1;
                                 }
                             } else {
-                                debug!("ignoring observation @ {epoch} because simulated {} does not expect it", msr.tracker);
+                                debug!("Device {} does not expect measurement at {epoch}, skipping", msr.tracker);
                             }
                         }
                         None => {

src/od/mod.rs

@@ -63,5 +63,3 @@ where
         }
     }
 }
-
-// TODO: Allow importing from parquet and initialization of a process, thereby allowing this to serve as a record!!

src/od/process/mod.rs

@@ -12,7 +12,7 @@ use nyx::io::gravity::*;
 use nyx::time::{Epoch, Unit};
 use nyx::State;
 use nyx::{propagators::*, Spacecraft};
-use rand::thread_rng;
+use rand::rng;
 use rand_distr::{Distribution, Normal};
 use rayon::prelude::*;
 use std::sync::Arc;
@@ -59,7 +59,7 @@ fn multi_thread_monte_carlo_demo(almanac: Arc<Almanac>) {
 
     // Generate all 100 initial states
     let init_states: Vec<Spacecraft> = sma_dist
-        .sample_iter(&mut thread_rng())
+        .sample_iter(&mut rng())
         .take(100)
         .map(|delta_sma| {
             Spacecraft::from(