nyx-space 2.4.0

Flight-proven, blazing fast astrodynamics from preliminary design to operations
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104

/*
    Nyx, blazing fast astrodynamics
    Copyright (C) 2018-onwards Christopher Rabotin <christopher.rabotin@gmail.com>

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU Affero General Public License as published
    by the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Affero General Public License for more details.

    You should have received a copy of the GNU Affero General Public License
    along with this program.  If not, see <https://www.gnu.org/licenses/>.
*/

use nalgebra::DimName;

pub use crate::errors::NyxError;
use crate::linalg::allocator::Allocator;
use crate::linalg::{DefaultAllocator, U3};
use crate::od::State;
pub use crate::od::estimate::{Estimate, KfEstimate, Residual};
pub use crate::od::snc::ProcessNoise;
pub use crate::time::{Epoch, Unit};

use super::{KalmanFilter, KalmanVariant};

impl<T> KalmanFilter<T, U3>
where
    T: State,
    DefaultAllocator: Allocator<<T as State>::Size>
        + Allocator<<T as State>::VecLength>
        + Allocator<U3>
        + Allocator<<T as State>::Size, <T as State>::Size>
        + Allocator<U3, U3>
        + Allocator<<T as State>::Size, U3>
        + Allocator<U3, <T as State>::Size>,
    <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
    <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
{
    /// Initializes this KF with an initial estimate, measurement noise, and one process noise
    pub fn new(initial_estimate: KfEstimate<T>, variant: KalmanVariant) -> Self {
        Self {
            prev_estimate: initial_estimate,
            process_noise: vec![],
            variant,
            prev_used_snc: 0,
        }
    }
}

impl<T, A> KalmanFilter<T, A>
where
    T: State,
    A: DimName,
    DefaultAllocator: Allocator<<T as State>::Size>
        + Allocator<<T as State>::VecLength>
        + Allocator<A>
        + Allocator<<T as State>::Size, <T as State>::Size>
        + Allocator<A, A>
        + Allocator<<T as State>::Size, A>
        + Allocator<A, <T as State>::Size>,
    <DefaultAllocator as Allocator<<T as State>::Size>>::Buffer<f64>: Copy,
    <DefaultAllocator as Allocator<<T as State>::Size, <T as State>::Size>>::Buffer<f64>: Copy,
{
    /// Set (or replaces) the existing process noise configuration.
    pub fn from_process_noise(
        initial_estimate: KfEstimate<T>,
        variant: KalmanVariant,
        mut process_noise: ProcessNoise<A>,
    ) -> Self {
        process_noise.init_epoch = Some(initial_estimate.epoch());
        Self {
            prev_estimate: initial_estimate,
            process_noise: vec![process_noise],
            variant,
            prev_used_snc: 0,
        }
    }

    /// Set (or replaces) the existing process noise configuration.
    pub fn with_process_noise(mut self, mut process_noise: ProcessNoise<A>) -> Self {
        process_noise.init_epoch = Some(self.prev_estimate.epoch());
        self.process_noise.clear();
        self.process_noise.push(process_noise);
        self
    }

    /// Pushes the provided process noise to the list the existing process noise configurations.
    pub fn and_with_process_noise(mut self, mut process_noise: ProcessNoise<A>) -> Self {
        process_noise.init_epoch = Some(self.prev_estimate.epoch());
        self.process_noise.push(process_noise);
        self
    }

    pub(crate) fn initialize_process_noises(&mut self) {
        for process_noise in &mut self.process_noise {
            process_noise.init_epoch = Some(self.prev_estimate.epoch());
        }
    }
}