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