nyx_space/od/estimate/
mod.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
19use super::State;
20use crate::cosmic::Orbit;
21use crate::linalg::allocator::Allocator;
22use crate::linalg::{DefaultAllocator, OMatrix, OVector};
23use crate::Spacecraft;
24use hifitime::Epoch;
25use std::cmp::PartialEq;
26use std::fmt;
27
28pub mod residual;
29pub use residual::Residual;
30pub mod kfestimate;
31pub use kfestimate::KfEstimate;
32mod sc_uncertainty;
33pub use sc_uncertainty::SpacecraftUncertainty;
34
35/// Stores an Estimate, as the result of a `time_update` or `measurement_update`.
36pub trait Estimate<T: State>
37where
38    Self: Clone + PartialEq + Sized + fmt::Display,
39    DefaultAllocator: Allocator<<T as State>::Size>
40        + Allocator<<T as State>::Size, <T as State>::Size>
41        + Allocator<<T as State>::VecLength>,
42{
43    /// An empty estimate. This is useful if wanting to store an estimate outside the scope of a filtering loop.
44    fn zeros(state: T) -> Self;
45    /// Epoch of this Estimate
46    fn epoch(&self) -> Epoch {
47        self.state().epoch()
48    }
49    // Sets the epoch
50    fn set_epoch(&mut self, dt: Epoch) {
51        self.state().set_epoch(dt);
52    }
53    /// The estimated state
54    fn state(&self) -> T {
55        self.nominal_state().add(self.state_deviation())
56    }
57    /// The state deviation as computed by the filter.
58    fn state_deviation(&self) -> OVector<f64, <T as State>::Size>;
59    /// The nominal state as reported by the filter dynamics
60    fn nominal_state(&self) -> T;
61    /// The Covariance of this estimate. Will return the predicted covariance if this is a time update/prediction.
62    fn covar(&self) -> OMatrix<f64, <T as State>::Size, <T as State>::Size>;
63    /// The predicted covariance of this estimate from the time update
64    fn predicted_covar(&self) -> OMatrix<f64, <T as State>::Size, <T as State>::Size>;
65    /// Sets the state deviation.
66    fn set_state_deviation(&mut self, new_state: OVector<f64, <T as State>::Size>);
67    /// Sets the Covariance of this estimate
68    fn set_covar(&mut self, new_covar: OMatrix<f64, <T as State>::Size, <T as State>::Size>);
69    /// Whether or not this is a predicted estimate from a time update, or an estimate from a measurement
70    fn predicted(&self) -> bool;
71    /// The STM used to compute this Estimate
72    fn stm(&self) -> &OMatrix<f64, <T as State>::Size, <T as State>::Size>;
73    /// Returns whether this estimate is within some bound
74    /// The 68-95-99.7 rule is a good way to assess whether the filter is operating normally
75    fn within_sigma(&self, sigma: f64) -> bool {
76        let state = self.state_deviation();
77        let covar = self.covar();
78        for i in 0..state.len() {
79            let bound = covar[(i, i)].sqrt() * sigma;
80            if state[i] > bound || state[i] < -bound {
81                return false;
82            }
83        }
84        true
85    }
86    /// Returns whether this estimate is within 3 sigma, which represent 99.7% for a Normal distribution
87    fn within_3sigma(&self) -> bool {
88        self.within_sigma(3.0)
89    }
90}
91
92/// A trait to store a navigation solution, can be used in conjunction with KfEstimate
93pub trait NavSolution<T>: Estimate<Spacecraft>
94where
95    T: State,
96    DefaultAllocator: Allocator<<T as State>::Size>
97        + Allocator<<T as State>::Size, <T as State>::Size>
98        + Allocator<<T as State>::VecLength>,
99{
100    fn orbital_state(&self) -> Orbit;
101    /// Returns the nominal state as computed by the dynamics
102    fn expected_state(&self) -> Orbit;
103}
104
105impl NavSolution<Spacecraft> for KfEstimate<Spacecraft> {
106    fn orbital_state(&self) -> Orbit {
107        self.state().orbit
108    }
109    fn expected_state(&self) -> Orbit {
110        self.nominal_state().orbit
111    }
112}
nyx_space/od/estimate/mod.rs

nyx_space/od/estimate/
mod.rs
