nyx_space/od/process/
trigger.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 hifitime::Epoch;
20
21use super::estimate::Estimate;
22use crate::linalg::allocator::Allocator;
23use crate::linalg::DefaultAllocator;
24use crate::time::Duration;
25use crate::State;
26use serde::{Deserialize, Serialize};
27
28#[derive(Copy, Clone, Debug, Serialize, Default, Deserialize)]
29/// An EkfTrigger on the number of measurements processed and a time between measurements.
30pub struct EkfTrigger {
31    pub num_msrs: usize,
32    pub disable_time: Duration,
33    /// Set to the sigma number needed to switch to the EKF (cf. 68–95–99.7 rule). If number is negative, this is ignored.
34    pub within_sigma: f64,
35    prev_msr_dt: Option<Epoch>,
36    cur_msrs: usize,
37    inhibit: bool,
38}
39
40impl EkfTrigger {
41    pub fn new(num_msrs: usize, disable_time: Duration) -> Self {
42        Self {
43            num_msrs,
44            disable_time,
45            within_sigma: -1.0,
46            prev_msr_dt: None,
47            cur_msrs: 0,
48            inhibit: false,
49        }
50    }
51
52    pub fn enable_ekf<T: State, E>(&mut self, est: &E) -> bool
53    where
54        E: Estimate<T>,
55        DefaultAllocator: Allocator<<T as State>::Size>
56            + Allocator<<T as State>::VecLength>
57            + Allocator<<T as State>::Size, <T as State>::Size>,
58    {
59        if self.inhibit {
60            return false;
61        }
62
63        if !est.predicted() {
64            // If this isn't a prediction, let's update the previous measurement time
65            self.prev_msr_dt = Some(est.epoch());
66        }
67        self.cur_msrs += 1;
68        self.cur_msrs >= self.num_msrs
69            && ((self.within_sigma > 0.0 && est.within_sigma(self.within_sigma))
70                || self.within_sigma <= 0.0)
71    }
72
73    pub fn disable_ekf(&mut self, epoch: Epoch) -> bool {
74        if self.inhibit {
75            return true;
76        }
77        // Return true if there is a prev msr dt, and the next measurement time is more than the disable time seconds away
78        match self.prev_msr_dt {
79            Some(prev_dt) => {
80                if (epoch - prev_dt).abs() > self.disable_time {
81                    self.cur_msrs = 0;
82                    true
83                } else {
84                    false
85                }
86            }
87            None => false,
88        }
89    }
90
91    pub fn set_inhibit(&mut self, inhibit: bool) {
92        self.inhibit = inhibit;
93    }
94
95    pub fn reset(&mut self) {
96        self.cur_msrs = 0;
97        self.prev_msr_dt = None;
98    }
99}
nyx_space/od/process/trigger.rs

nyx_space/od/process/
trigger.rs
