nyx_space/od/filter/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
19pub use crate::errors::NyxError;
20use crate::linalg::allocator::Allocator;
21use crate::linalg::{DefaultAllocator, DimName, U3};
22pub use crate::od::estimate::{Estimate, KfEstimate, Residual};
23pub use crate::od::snc::ProcessNoise;
24use crate::od::State;
25pub use crate::time::{Epoch, Unit};
26
27use super::KF;
28
29impl<T, A> KF<T, A>
30where
31    A: DimName,
32    T: State,
33    DefaultAllocator: Allocator<<T as State>::Size>
34        + Allocator<<T as State>::VecLength>
35        + Allocator<A>
36        + Allocator<<T as State>::Size, <T as State>::Size>
37        + Allocator<A, A>
38        + Allocator<<T as State>::Size, A>
39        + Allocator<A, <T as State>::Size>,
40    <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
41    <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
42{
43    /// Initializes this KF with an initial estimate, measurement noise, and one process noise
44    pub fn new(initial_estimate: KfEstimate<T>, process_noise: ProcessNoise<A>) -> Self {
45        assert_eq!(
46            A::dim() % 3,
47            0,
48            "SNC can only be applied to accelerations multiple of 3"
49        );
50
51        // Set the initial epoch of the SNC
52        let mut process_noise = process_noise;
53        process_noise.init_epoch = Some(initial_estimate.epoch());
54
55        Self {
56            prev_estimate: initial_estimate,
57            process_noise: vec![process_noise],
58            ekf: false,
59            prev_used_snc: 0,
60        }
61    }
62
63    /// Initializes this KF with an initial estimate, measurement noise, and several process noise
64    /// WARNING: SNCs MUST be ordered chronologically! They will be selected automatically by walking
65    /// the list of SNCs backward until one can be applied!
66    pub fn with_sncs(
67        initial_estimate: KfEstimate<T>,
68        process_noises: Vec<ProcessNoise<A>>,
69    ) -> Self {
70        assert_eq!(
71            A::dim() % 3,
72            0,
73            "SNC can only be applied to accelerations multiple of 3"
74        );
75        let mut process_noises = process_noises;
76        // Set the initial epoch of the SNC
77        for snc in &mut process_noises {
78            snc.init_epoch = Some(initial_estimate.epoch());
79        }
80
81        Self {
82            prev_estimate: initial_estimate,
83            process_noise: process_noises,
84            ekf: false,
85            prev_used_snc: 0,
86        }
87    }
88}
89
90impl<T> KF<T, U3>
91where
92    T: State,
93    DefaultAllocator: Allocator<<T as State>::Size>
94        + Allocator<<T as State>::VecLength>
95        + Allocator<<T as State>::Size, <T as State>::Size>
96        + Allocator<U3, U3>
97        + Allocator<<T as State>::Size, U3>
98        + Allocator<U3, <T as State>::Size>,
99    <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
100    <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
101{
102    /// Initializes this KF without SNC
103    pub fn no_snc(initial_estimate: KfEstimate<T>) -> Self {
104        Self {
105            prev_estimate: initial_estimate,
106            process_noise: Vec::new(),
107            ekf: false,
108            prev_used_snc: 0,
109        }
110    }
111}
nyx_space/od/filter/kalman/initializers.rs

nyx_space/od/filter/kalman/
initializers.rs
