nyx_space/od/process/
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 crate::linalg::allocator::Allocator;
20use crate::linalg::{DefaultAllocator, DimName};
21use crate::md::trajectory::{Interpolatable, Traj};
22pub use crate::od::estimate::*;
23pub use crate::od::ground_station::*;
24pub use crate::od::snc::*;
25pub use crate::od::*;
26use crate::propagators::Propagator;
27pub use crate::time::{Duration, Unit};
28use anise::prelude::Almanac;
29use indexmap::IndexSet;
30use log::{debug, error, info, warn};
31use msr::sensitivity::TrackerSensitivity;
32use snafu::prelude::*;
33use solution::kalman::KalmanVariant;
34use std::collections::BTreeMap;
35use std::marker::PhantomData;
36use std::ops::Add;
37use typed_builder::TypedBuilder;
38
39mod rejectcrit;
40use self::kalman::KalmanFilter;
41use self::msr::TrackingDataArc;
42pub use self::rejectcrit::ResidRejectCrit;
43mod solution;
44pub use solution::ODSolution;
45mod initializers;
46
47/// An orbit determination process (ODP) which filters OD measurements through a Kalman filter.
48#[derive(Clone, TypedBuilder)]
49#[builder(doc)]
50#[allow(clippy::upper_case_acronyms)]
51pub struct KalmanODProcess<
52    D: Dynamics,
53    MsrSize: DimName,
54    Accel: DimName,
55    Trk: TrackerSensitivity<D::StateType, D::StateType>,
56> where
57    D::StateType:
58        Interpolatable + Add<OVector<f64, <D::StateType as State>::Size>, Output = D::StateType>,
59    <DefaultAllocator as Allocator<<D::StateType as State>::VecLength>>::Buffer<f64>: Send,
60    <DefaultAllocator as Allocator<<D::StateType as State>::Size>>::Buffer<f64>: Copy,
61    <DefaultAllocator as Allocator<<D::StateType as State>::Size, <D::StateType as State>::Size>>::Buffer<f64>: Copy,
62    DefaultAllocator: Allocator<<D::StateType as State>::Size>
63        + Allocator<<D::StateType as State>::VecLength>
64        + Allocator<MsrSize>
65        + Allocator<MsrSize, <D::StateType as State>::Size>
66        + Allocator<<D::StateType as State>::Size, MsrSize>
67        + Allocator<MsrSize, MsrSize>
68        + Allocator<<D::StateType as State>::Size, <D::StateType as State>::Size>
69        + Allocator<Accel>
70        + Allocator<Accel, Accel>
71        + Allocator<<D::StateType as State>::Size, Accel>
72        + Allocator<Accel, <D::StateType as State>::Size>,
73{
74    /// Propagator used for the estimation
75    pub prop: Propagator<D>,
76    /// Kalman filter variant
77    #[builder(default)]
78    pub kf_variant: KalmanVariant,
79    /// Residual rejection criteria allows preventing bad measurements from affecting the estimation.
80    #[builder(default, setter(strip_option))]
81    pub resid_crit: Option<ResidRejectCrit>,
82    /// Tracking devices
83    #[builder(default_code = "BTreeMap::new()")]
84    pub devices: BTreeMap<String, Trk>,
85    /// A sets of process noise (usually noted Q), must be ordered chronologically
86    #[builder(default_code = "vec![]")]
87    pub process_noise: Vec<ProcessNoise<Accel>>,
88    /// Maximum step size where the STM linearization is assumed correct (1 minute is usually fine)
89    #[builder(default_code = "1 * Unit::Minute")]
90    pub max_step: Duration,
91    /// Precision of the measurement epoch when processing measurements.
92    #[builder(default_code = "1 * Unit::Microsecond")]
93    pub epoch_precision: Duration,
94    pub almanac: Arc<Almanac>,
95    #[builder(default_code = "PhantomData::<MsrSize>")]
96    _msr_size: PhantomData<MsrSize>,
97}
98
99impl<
100        D: Dynamics,
101        MsrSize: DimName,
102        Accel: DimName,
103        Trk: TrackerSensitivity<D::StateType, D::StateType>,
104    > KalmanODProcess<D, MsrSize, Accel, Trk>
105where
106    D::StateType:
107        Interpolatable + Add<OVector<f64, <D::StateType as State>::Size>, Output = D::StateType>,
108    <DefaultAllocator as Allocator<<D::StateType as State>::VecLength>>::Buffer<f64>: Send,
109    <DefaultAllocator as Allocator<<D::StateType as State>::Size>>::Buffer<f64>: Copy,
110    <DefaultAllocator as Allocator<<D::StateType as State>::Size, <D::StateType as State>::Size>>::Buffer<f64>: Copy,
111    DefaultAllocator: Allocator<<D::StateType as State>::Size>
112        + Allocator<<D::StateType as State>::VecLength>
113        + Allocator<MsrSize>
114        + Allocator<MsrSize, <D::StateType as State>::Size>
115        + Allocator<<D::StateType as State>::Size, MsrSize>
116        + Allocator<MsrSize, MsrSize>
117        + Allocator<<D::StateType as State>::Size, <D::StateType as State>::Size>
118        + Allocator<Accel>
119        + Allocator<Accel, Accel>
120        + Allocator<<D::StateType as State>::Size, Accel>
121        + Allocator<Accel, <D::StateType as State>::Size>
122        + Allocator<nalgebra::Const<1>, MsrSize>,
123{
124    /// Process the provided tracking arc for this orbit determination process.
125    #[allow(clippy::erasing_op)]
126    pub fn process_arc(
127        &self,
128        initial_estimate: KfEstimate<D::StateType>,
129        arc: &TrackingDataArc,
130    ) -> Result<ODSolution<D::StateType, KfEstimate<D::StateType>, MsrSize, Trk>, ODError> {
131        // Initialize the solution.
132        let mut od_sol = ODSolution::new(self.devices.clone(), arc.unique_types());
133
134        let measurements = &arc.measurements;
135        ensure!(
136            measurements.len() >= 2,
137            TooFewMeasurementsSnafu {
138                need: 2_usize,
139                action: "running a Kalman filter"
140            }
141        );
142
143        ensure!(
144            !self.max_step.is_negative() && self.max_step != Duration::ZERO,
145            StepSizeSnafu { step: self.max_step }
146        );
147
148        // Check proper configuration.
149        if MsrSize::DIM > arc.unique_types().len() {
150            error!("Filter misconfigured: expect high rejection count!");
151            error!(
152                "Arc only contains {} measurement types, but filter configured for {}.",
153                arc.unique_types().len(),
154                MsrSize::DIM
155            );
156            error!("Filter should be configured for these numbers to match.");
157            error!("Consider running subsequent arcs if ground stations provide different measurements.")
158        }
159
160        // Start by propagating the estimator.
161        let num_msrs = measurements.len();
162
163        // Set up the propagator instance.
164        let prop = self.prop.clone();
165        let mut prop_instance = prop.with(initial_estimate.nominal_state().with_stm(), self.almanac.clone()).quiet();
166
167        // Update the step size of the navigation propagator if it isn't already fixed step
168        if !prop_instance.fixed_step {
169            prop_instance.set_step(self.max_step, false);
170        }
171
172        let prop_time = arc.end_epoch().unwrap() - initial_estimate.epoch();
173        info!("Navigation propagating for a total of {prop_time} with step size {}", self.max_step);
174
175        let resid_crit = if arc.force_reject {
176            warn!("Rejecting all measurements from {arc} as requested");
177            Some(ResidRejectCrit { num_sigmas: 0.0 })
178        } else {
179            self.resid_crit
180        };
181
182        let mut epoch = prop_instance.state.epoch();
183
184        let mut reported = [false; 11];
185        reported[0] = true; // Prevent showing "0% done"
186        info!(
187            "Processing {num_msrs} measurements from {:?}",
188            arc.unique_aliases()
189        );
190
191        // Set up the Kalman filter.
192        let mut kf = KalmanFilter::<D::StateType, Accel> {
193            prev_estimate: initial_estimate,
194            process_noise: self.process_noise.clone(),
195            variant: self.kf_variant,
196            prev_used_snc: 0,
197        };
198
199        kf.initialize_process_noises();
200
201        let mut devices = self.devices.clone();
202
203        // We'll build a trajectory of the estimated states. This will be used to compute the measurements.
204        let mut traj: Traj<D::StateType> = Traj::new();
205
206        let mut msr_accepted_cnt: usize = 0;
207        let mut msr_rejected_cnt: usize = 0;
208        let mut unknown_trackers = IndexSet::new();
209        let tick = Epoch::now().unwrap();
210
211        for (msr_cnt, (epoch_ref, msr)) in measurements.iter().enumerate() {
212            let next_msr_epoch = *epoch_ref;
213
214            // Advance the propagator
215            loop {
216                let delta_t = next_msr_epoch - epoch;
217
218                // Propagate for the minimum time between the maximum step size, the next step size, and the duration to the next measurement.
219                let next_step_size = delta_t.min(prop_instance.step_size).min(self.max_step);
220
221                // Remove old states from the trajectory
222                // This is a manual implementation of `retaint` because we know it's a sorted vec, so no need to resort every time
223                let mut index = traj.states.len();
224                while index > 0 {
225                    index -= 1;
226                    if traj.states[index].epoch() >= epoch {
227                        break;
228                    }
229                }
230                traj.states.truncate(index);
231
232                debug!("propagate for {next_step_size} (Δt to next msr: {delta_t})");
233                let (_, traj_covar) = prop_instance
234                    .for_duration_with_traj(next_step_size)
235                    .context(ODPropSnafu)?;
236
237                for state in traj_covar.states {
238                    // NOTE: At the time being, only spacecraft estimation is possible, and the trajectory will always be the exact state
239                    // that was propagated. Even once ground station biases are estimated, these won't go through the propagator.
240                    traj.states.push(state);
241                }
242
243                // Now that we've advanced the propagator, let's see whether we're at the time of the next measurement.
244
245                // Extract the state and update the STM in the filter.
246                let nominal_state = prop_instance.state;
247                // Get the datetime and info needed to compute the theoretical measurement according to the model
248                epoch = nominal_state.epoch();
249
250                // Perform a measurement update, accounting for possible errors in measurement timestamps
251                if (nominal_state.epoch() - next_msr_epoch).abs() < self.epoch_precision {
252                    // Get the computed observations
253                    match devices.get_mut(&msr.tracker) {
254                        Some(device) => {
255                            if let Some(computed_meas) =
256                                device.measure(epoch, &traj, None, self.almanac.clone())?
257                            {
258                                let msr_types = device.measurement_types();
259
260                                // Perform several measurement updates to ensure the desired dimensionality.
261                                let windows = msr_types.len() / MsrSize::DIM;
262                                let mut msr_rejected = false;
263                                for wno in 0..=windows {
264                                    let mut cur_msr_types = IndexSet::new();
265                                    for msr_type in msr_types
266                                        .iter()
267                                        .copied()
268                                        .skip(wno * MsrSize::DIM)
269                                        .take(MsrSize::DIM)
270                                    {
271                                        cur_msr_types.insert(msr_type);
272                                    }
273
274                                    if cur_msr_types.is_empty() {
275                                        // We've processed all measurements.
276                                        break;
277                                    }
278
279                                    // If this measurement type is unavailable, continue to the next one.
280                                    if !msr.availability(&cur_msr_types).iter().any(|avail| *avail)
281                                    {
282                                        continue;
283                                    }
284
285                                    // Grab the un-modulo'd real observation
286                                    let mut real_obs: OVector<f64, MsrSize> =
287                                        msr.observation(&cur_msr_types);
288
289                                    // Check that the observation is valid.
290                                    for val in real_obs.iter().copied() {
291                                        ensure!(
292                                            val.is_finite(),
293                                            InvalidMeasurementSnafu {
294                                                epoch: *epoch_ref,
295                                                val
296                                            }
297                                        );
298                                    }
299
300                                    // Compute device specific matrices
301                                    let h_tilde = device.h_tilde::<MsrSize>(
302                                        msr,
303                                        &cur_msr_types,
304                                        &nominal_state,
305                                        self.almanac.clone(),
306                                    )?;
307
308                                    let measurement_covar =
309                                        device.measurement_covar_matrix(&cur_msr_types, epoch)?;
310
311                                    // Apply any biases on the computed observation
312                                    let computed_obs = computed_meas
313                                        .observation::<MsrSize>(&cur_msr_types)
314                                        - device.measurement_bias_vector::<MsrSize>(
315                                            &cur_msr_types,
316                                            epoch,
317                                        )?;
318
319                                    // Apply the modulo to the real obs
320                                    if let Some(moduli) = &arc.moduli {
321                                        let mut obs_ambiguity = OVector::<f64, MsrSize>::zeros();
322
323                                        for (i, msr_type) in cur_msr_types.iter().enumerate() {
324                                            if let Some(modulus) = moduli.get(msr_type) {
325                                                let k = computed_obs[i].div_euclid(*modulus);
326                                                // real_obs = measured_obs + k * modulus
327                                                obs_ambiguity[i] = k * *modulus;
328                                            }
329                                        }
330                                        real_obs += obs_ambiguity;
331                                    }
332
333                                    let (estimate, mut residual, gain) = kf.measurement_update(
334                                        nominal_state,
335                                        real_obs,
336                                        computed_obs,
337                                        measurement_covar,
338                                        h_tilde,
339                                        resid_crit,
340                                    )?;
341
342                                    debug!("processed measurement #{msr_cnt} for {cur_msr_types:?} @ {epoch} from {}", device.name());
343
344                                    residual.tracker = Some(device.name());
345                                    residual.msr_types = cur_msr_types;
346
347                                    if residual.rejected {
348                                        msr_rejected = true;
349                                    }
350
351                                    if kf.replace_state() {
352                                        prop_instance.state = estimate.state();
353                                    }
354
355                                    prop_instance.state.reset_stm();
356
357                                    od_sol
358                                        .push_measurement_update(estimate, residual, gain);
359                                }
360                                if msr_rejected {
361                                    msr_rejected_cnt += 1;
362                                } else {
363                                    msr_accepted_cnt += 1;
364                                }
365                            } else {
366                                debug!("Device {} does not expect measurement at {epoch}, skipping", msr.tracker);
367                            }
368                        }
369                        None => {
370                            if !unknown_trackers.contains(&msr.tracker) {
371                                error!(
372                                    "Tracker {} is not in the list of configured devices",
373                                    msr.tracker
374                                );
375                            }
376                            unknown_trackers.insert(msr.tracker.clone());
377                        }
378                    }
379
380                    let msr_prct = (10.0 * (msr_cnt as f64) / (num_msrs as f64)) as usize;
381                    if !reported[msr_prct] {
382                        let msg = format!(
383                            "{:>3}% done - {msr_accepted_cnt:.0} measurements accepted, {:.0} rejected",
384                            10 * msr_prct, msr_rejected_cnt
385                        );
386                        if msr_accepted_cnt < msr_rejected_cnt {
387                            warn!("{msg}");
388                        } else {
389                            info!("{msg}");
390                        }
391                        reported[msr_prct] = true;
392                    }
393
394                    break;
395                } else {
396                    // No measurement can be used here, let's just do a time update and continue advancing the propagator.
397                    debug!("time update {epoch:?}, next msr {next_msr_epoch:?}");
398                    match kf.time_update(nominal_state) {
399                        Ok(est) => {
400                            // State deviation is always zero for an EKF time update so we don't do anything different than for a CKF.
401                            od_sol.push_time_update(est);
402                        }
403                        Err(e) => return Err(e),
404                    }
405                    prop_instance.state.reset_stm();
406                }
407            }
408        }
409
410        // Always report the 100% mark
411        if !reported[10] {
412            let tock_time = Epoch::now().unwrap() - tick;
413            info!(
414                "100% done - {msr_accepted_cnt} measurements accepted, {msr_rejected_cnt} rejected (done in {tock_time})",
415            );
416        }
417
418        Ok(od_sol)
419    }
420
421    /// Perform a time update. Continuously predicts the trajectory until the provided end epoch, with covariance mapping at each step.
422    pub fn predict_until(
423        &self,
424        initial_estimate: KfEstimate<D::StateType>,
425        end_epoch: Epoch,
426    ) -> Result<ODSolution<D::StateType, KfEstimate<D::StateType>, MsrSize, Trk>, ODError> {
427        // Initialize the solution with no measurement types.
428        let mut od_sol = ODSolution::new(self.devices.clone(), IndexSet::new());
429
430        od_sol.push_time_update(initial_estimate);
431
432        // Set up the propagator instance.
433        let prop = self.prop.clone();
434        let mut prop_instance = prop.with(initial_estimate.nominal_state().with_stm(), self.almanac.clone()).quiet();
435
436
437        // Set up the Kalman filter.
438        let mut kf = KalmanFilter::<D::StateType, Accel> {
439            prev_estimate: initial_estimate,
440            process_noise: self.process_noise.clone(),
441            variant: self.kf_variant,
442            prev_used_snc: 0,
443        };
444
445        let prop_time = end_epoch - kf.previous_estimate().epoch();
446        info!("Mapping covariance for {prop_time} every {} until {end_epoch}", self.max_step);
447
448        loop {
449            let nominal_state = prop_instance.for_duration(self.max_step).context(ODPropSnafu)?;
450            // Extract the state and update the STM in the filter.
451            // Get the datetime and info needed to compute the theoretical measurement according to the model
452            let epoch = nominal_state.epoch();
453            // No measurement can be used here, let's just do a time update
454            debug!("time update {epoch}");
455            match kf.time_update(nominal_state) {
456                Ok(est) => {
457                    od_sol.push_time_update(est);
458                }
459                Err(e) => return Err(e),
460            }
461            prop_instance.state.reset_stm();
462            if epoch >= end_epoch {
463                break;
464            }
465        }
466
467        Ok(od_sol)
468    }
469
470    /// Perform a time update. Continuously predicts the trajectory for the provided duration, with covariance mapping at each step. In other words, this performs a time update.
471    pub fn predict_for(
472        &self,
473        initial_estimate: KfEstimate<D::StateType>,
474        duration: Duration,
475    ) -> Result<ODSolution<D::StateType, KfEstimate<D::StateType>, MsrSize, Trk>, ODError> {
476        let end_epoch = initial_estimate.nominal_state().epoch() + duration;
477        self.predict_until(initial_estimate, end_epoch)
478    }
479}