nyx_space/od/kalman/
initializers.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 nalgebra::DimName;
20
21pub use crate::errors::NyxError;
22use crate::linalg::allocator::Allocator;
23use crate::linalg::{DefaultAllocator, U3};
24pub use crate::od::estimate::{Estimate, KfEstimate, Residual};
25pub use crate::od::snc::ProcessNoise;
26use crate::od::State;
27pub use crate::time::{Epoch, Unit};
28
29use super::{KalmanFilter, KalmanVariant};
30
31impl<T> KalmanFilter<T, U3>
32where
33    T: State,
34    DefaultAllocator: Allocator<<T as State>::Size>
35        + Allocator<<T as State>::VecLength>
36        + Allocator<U3>
37        + Allocator<<T as State>::Size, <T as State>::Size>
38        + Allocator<U3, U3>
39        + Allocator<<T as State>::Size, U3>
40        + Allocator<U3, <T as State>::Size>,
41    <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
42    <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
43{
44    /// Initializes this KF with an initial estimate, measurement noise, and one process noise
45    pub fn new(initial_estimate: KfEstimate<T>, variant: KalmanVariant) -> Self {
46        Self {
47            prev_estimate: initial_estimate,
48            process_noise: vec![],
49            variant,
50            prev_used_snc: 0,
51        }
52    }
53}
54
55impl<T, A> KalmanFilter<T, A>
56where
57    T: State,
58    A: DimName,
59    DefaultAllocator: Allocator<<T as State>::Size>
60        + Allocator<<T as State>::VecLength>
61        + Allocator<A>
62        + Allocator<<T as State>::Size, <T as State>::Size>
63        + Allocator<A, A>
64        + Allocator<<T as State>::Size, A>
65        + Allocator<A, <T as State>::Size>,
66    <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
67    <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
68{
69    /// Set (or replaces) the existing process noise configuration.
70    pub fn from_process_noise(
71        initial_estimate: KfEstimate<T>,
72        variant: KalmanVariant,
73        mut process_noise: ProcessNoise<A>,
74    ) -> Self {
75        process_noise.init_epoch = Some(initial_estimate.epoch());
76        Self {
77            prev_estimate: initial_estimate,
78            process_noise: vec![process_noise],
79            variant,
80            prev_used_snc: 0,
81        }
82    }
83
84    /// Set (or replaces) the existing process noise configuration.
85    pub fn with_process_noise(mut self, mut process_noise: ProcessNoise<A>) -> Self {
86        process_noise.init_epoch = Some(self.prev_estimate.epoch());
87        self.process_noise.clear();
88        self.process_noise.push(process_noise);
89        self
90    }
91
92    /// Pushes the provided process noise to the list the existing process noise configurations.
93    pub fn and_with_process_noise(mut self, mut process_noise: ProcessNoise<A>) -> Self {
94        process_noise.init_epoch = Some(self.prev_estimate.epoch());
95        self.process_noise.push(process_noise);
96        self
97    }
98
99    pub(crate) fn initialize_process_noises(&mut self) {
100        for process_noise in &mut self.process_noise {
101            process_noise.init_epoch = Some(self.prev_estimate.epoch());
102        }
103    }
104}
nyx_space/od/kalman/initializers.rs

nyx_space/od/kalman/
initializers.rs
