Skip to content

Commit 24d27fe

Browse files
Merge pull request #425 from nyx-space/feat/85-batch-least-squares
Add batch least squares estimators. Fix #85.
2 parents 8d71d73 + 0baaa77 commit 24d27fe

File tree

10 files changed

+718
-111
lines changed

10 files changed

+718
-111
lines changed

src/cosmic/mod.rs

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -65,7 +65,7 @@ where
6565
)
6666
}
6767

68-
/// Return this state as a vector for the propagation/estimation
68+
/// Return the state's _step_ state transition matrix.
6969
/// By default, this is not implemented. This function must be implemented when filtering on this state.
7070
fn stm(&self) -> Result<OMatrix<f64, Self::Size, Self::Size>, DynamicsError> {
7171
Err(DynamicsError::StateTransitionMatrixUnset)
@@ -74,8 +74,7 @@ where
7474
/// Copies the current state but sets the STM to identity
7575
fn with_stm(self) -> Self;
7676

77-
/// Return this state as a vector for the propagation/estimation
78-
/// By default, this is not implemented. This function must be implemented when filtering on this state.
77+
/// Resets the STM, unimplemented by default.
7978
fn reset_stm(&mut self) {
8079
unimplemented!()
8180
}
@@ -102,6 +101,7 @@ where
102101

103102
/// Retrieve the Epoch
104103
fn epoch(&self) -> Epoch;
104+
105105
/// Set the Epoch
106106
fn set_epoch(&mut self, epoch: Epoch);
107107

src/od/blse/mod.rs

Lines changed: 426 additions & 0 deletions
Large diffs are not rendered by default.

src/od/blse/solution.rs

Lines changed: 93 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,93 @@
1+
/*
2+
Nyx, blazing fast astrodynamics
3+
Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>
4+
5+
This program is free software: you can redistribute it and/or modify
6+
it under the terms of the GNU Affero General Public License as published
7+
by the Free Software Foundation, either version 3 of the License, or
8+
(at your option) any later version.
9+
10+
This program is distributed in the hope that it will be useful,
11+
but WITHOUT ANY WARRANTY; without even the implied warranty of
12+
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13+
GNU Affero General Public License for more details.
14+
15+
You should have received a copy of the GNU Affero General Public License
16+
along with this program. If not, see <https://www.gnu.org/licenses/>.
17+
*/
18+
19+
#![allow(clippy::type_complexity)] // Allow complex types for generics
20+
#![allow(unused_imports)] // Keep imports for context even if slightly unused in snippet
21+
22+
use crate::linalg::allocator::Allocator;
23+
use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector, U1}; // Use U1 for MsrSize
24+
use crate::md::trajectory::{Interpolatable, Traj}; // May not need Traj if we propagate point-to-point
25+
pub use crate::od::estimate::*;
26+
pub use crate::od::ground_station::*;
27+
pub use crate::od::snc::*; // SNC not typically used in BLS, but keep context
28+
pub use crate::od::*;
29+
use crate::propagators::Propagator;
30+
pub use crate::time::{Duration, Epoch, Unit};
31+
use anise::prelude::Almanac;
32+
use indexmap::IndexSet;
33+
use log::{debug, info, trace, warn};
34+
use msr::sensitivity::TrackerSensitivity; // Assuming this is the correct path
35+
use snafu::prelude::*;
36+
use std::collections::BTreeMap;
37+
use std::fmt;
38+
use std::marker::PhantomData;
39+
use std::ops::Add;
40+
use std::sync::Arc;
41+
use typed_builder::TypedBuilder;
42+
43+
// Define a simple BLS solution struct
44+
#[derive(Debug, Clone)]
45+
pub struct BLSSolution<StateType: State>
46+
where
47+
DefaultAllocator: Allocator<<StateType as State>::Size>
48+
+ Allocator<<StateType as State>::Size, <StateType as State>::Size>
49+
+ Allocator<<StateType as State>::VecLength>,
50+
{
51+
pub estimated_state: StateType,
52+
pub covariance: OMatrix<f64, <StateType as State>::Size, <StateType as State>::Size>,
53+
pub num_iterations: usize,
54+
pub final_rms: f64,
55+
pub final_corr_pos_km: f64,
56+
pub converged: bool,
57+
}
58+
59+
impl<StateType> fmt::Display for BLSSolution<StateType>
60+
where
61+
StateType: State,
62+
DefaultAllocator: Allocator<<StateType as State>::Size>
63+
+ Allocator<<StateType as State>::Size, <StateType as State>::Size>
64+
+ Allocator<<StateType as State>::VecLength>,
65+
{
66+
fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
67+
writeln!(f, "Converged: {}", self.converged)?;
68+
writeln!(f, "Iterations: {}", self.num_iterations)?;
69+
writeln!(f, "Final RMS: {}", self.final_rms)?;
70+
writeln!(f, "Final State: {}", self.estimated_state.orbit())?;
71+
write!(f, "Final Covariance:\n{:.3e}", self.covariance)
72+
}
73+
}
74+
75+
impl<StateType: State> From<BLSSolution<StateType>> for KfEstimate<StateType>
76+
where
77+
DefaultAllocator: Allocator<<StateType as State>::Size>
78+
+ Allocator<<StateType as State>::Size, <StateType as State>::Size>
79+
+ Allocator<<StateType as State>::VecLength>,
80+
<DefaultAllocator as Allocator<<StateType as State>::Size>>::Buffer<f64>: Copy,
81+
<DefaultAllocator as Allocator<<StateType as State>::Size, <StateType as State>::Size>>::Buffer<
82+
f64,
83+
>: Copy,
84+
{
85+
fn from(mut bls: BLSSolution<StateType>) -> Self {
86+
// Set the uncertainty on Cr, Cd, mass to zero
87+
bls.covariance[(6, 6)] = 0.0;
88+
bls.covariance[(7, 7)] = 0.0;
89+
bls.covariance[(8, 8)] = 0.0;
90+
91+
Self::from_covar(bls.estimated_state, bls.covariance)
92+
}
93+
}

src/od/filter/mod.rs

Lines changed: 0 additions & 102 deletions
This file was deleted.

src/od/mod.rs

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,9 @@ pub use simulator::TrackingDevice;
5858
/// Provides all state noise compensation functionality
5959
pub mod snc;
6060

61+
/// Provides the Batch least squares initial state solver.
62+
pub mod blse;
63+
6164
/// A helper type for spacecraft orbit determination.
6265
pub type SpacecraftKalmanOD = self::process::KalmanODProcess<
6366
SpacecraftDynamics,
@@ -110,6 +113,8 @@ pub enum ODError {
110113
InvalidMeasurement { epoch: Epoch, val: f64 },
111114
#[snafu(display("Kalman gain is singular"))]
112115
SingularKalmanGain,
116+
#[snafu(display("Information matrix is singular"))]
117+
SingularInformationMatrix,
113118
#[snafu(display("noise matrix is singular"))]
114119
SingularNoiseRk,
115120
#[snafu(display("{kind} noise not configured"))]
@@ -142,6 +147,8 @@ pub enum ODError {
142147
source: Box<StateError>,
143148
action: &'static str,
144149
},
150+
#[snafu(display("Maximum iterations ({max_iter}) reached without convergence"))]
151+
ODMaxIterations { max_iter: usize },
145152
#[snafu(display("Nyx orbit determination limitation: {action}"))]
146153
ODLimitation { action: &'static str },
147154
}

src/od/process/mod.rs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -213,7 +213,7 @@ where
213213
loop {
214214
let delta_t = next_msr_epoch - epoch;
215215

216-
// Propagator for the minimum time between the maximum step size, the next step size, and the duration to the next measurement.
216+
// Propagate for the minimum time between the maximum step size, the next step size, and the duration to the next measurement.
217217
let next_step_size = delta_t.min(prop_instance.step_size).min(self.max_step);
218218

219219
// Remove old states from the trajectory
@@ -361,7 +361,7 @@ where
361361
msr_accepted_cnt += 1;
362362
}
363363
} else {
364-
debug!("ignoring observation @ {epoch} because simulated {} does not expect it", msr.tracker);
364+
debug!("Device {} does not expect measurement at {epoch}, skipping", msr.tracker);
365365
}
366366
}
367367
None => {

src/od/process/solution/display.rs

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -63,5 +63,3 @@ where
6363
}
6464
}
6565
}
66-
67-
// TODO: Allow importing from parquet and initialization of a process, thereby allowing this to serve as a record!!

tests/monte_carlo/manual_montecarlo.rs

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@ use nyx::io::gravity::*;
1212
use nyx::time::{Epoch, Unit};
1313
use nyx::State;
1414
use nyx::{propagators::*, Spacecraft};
15-
use rand::thread_rng;
15+
use rand::rng;
1616
use rand_distr::{Distribution, Normal};
1717
use rayon::prelude::*;
1818
use std::sync::Arc;
@@ -59,7 +59,7 @@ fn multi_thread_monte_carlo_demo(almanac: Arc<Almanac>) {
5959

6060
// Generate all 100 initial states
6161
let init_states: Vec<Spacecraft> = sma_dist
62-
.sample_iter(&mut thread_rng())
62+
.sample_iter(&mut rng())
6363
.take(100)
6464
.map(|delta_sma| {
6565
Spacecraft::from(

0 commit comments

Comments
 (0)