nyx_space/od/blse/
solution.rs

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
22use crate::linalg::allocator::Allocator;
23use crate::linalg::{DefaultAllocator, DimName, OMatrix, OVector, U1}; // Use U1 for MsrSize
24use crate::md::trajectory::{Interpolatable, Traj}; // May not need Traj if we propagate point-to-point
25pub use crate::od::estimate::*;
26pub use crate::od::ground_station::*;
27pub use crate::od::snc::*; // SNC not typically used in BLS, but keep context
28pub use crate::od::*;
29use crate::propagators::Propagator;
30pub use crate::time::{Duration, Epoch, Unit};
31use anise::prelude::Almanac;
32use indexmap::IndexSet;
33use log::{debug, info, trace, warn};
34use msr::sensitivity::TrackerSensitivity; // Assuming this is the correct path
35use snafu::prelude::*;
36use std::collections::BTreeMap;
37use std::fmt;
38use std::marker::PhantomData;
39use std::ops::Add;
40use std::sync::Arc;
41use typed_builder::TypedBuilder;
42
43// Define a simple BLS solution struct
44#[derive(Debug, Clone)]
45pub struct BLSSolution<StateType: State>
46where
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
59impl<StateType> fmt::Display for BLSSolution<StateType>
60where
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
75impl<StateType: State> From<BLSSolution<StateType>> for KfEstimate<StateType>
76where
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}
nyx_space/od/blse/solution.rs

nyx_space/od/blse/
solution.rs
