1use 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#[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 pub prop: Propagator<D>,
76 #[builder(default)]
78 pub kf_variant: KalmanVariant,
79 #[builder(default, setter(strip_option))]
81 pub resid_crit: Option<ResidRejectCrit>,
82 #[builder(default_code = "BTreeMap::new()")]
84 pub devices: BTreeMap<String, Trk>,
85 #[builder(default_code = "vec![]")]
87 pub process_noise: Vec<ProcessNoise<Accel>>,
88 #[builder(default_code = "1 * Unit::Minute")]
90 pub max_step: Duration,
91 #[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 #[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 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 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 let num_msrs = measurements.len();
162
163 let prop = self.prop.clone();
165 let mut prop_instance = prop.with(initial_estimate.nominal_state().with_stm(), self.almanac.clone()).quiet();
166
167 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; info!(
187 "Processing {num_msrs} measurements from {:?}",
188 arc.unique_aliases()
189 );
190
191 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 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 loop {
216 let delta_t = next_msr_epoch - epoch;
217
218 let next_step_size = delta_t.min(prop_instance.step_size).min(self.max_step);
220
221 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 traj.states.push(state);
241 }
242
243 let nominal_state = prop_instance.state;
247 epoch = nominal_state.epoch();
249
250 if (nominal_state.epoch() - next_msr_epoch).abs() < self.epoch_precision {
252 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 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 break;
277 }
278
279 if !msr.availability(&cur_msr_types).iter().any(|avail| *avail)
281 {
282 continue;
283 }
284
285 let mut real_obs: OVector<f64, MsrSize> =
287 msr.observation(&cur_msr_types);
288
289 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 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 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 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 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 debug!("time update {epoch:?}, next msr {next_msr_epoch:?}");
398 match kf.time_update(nominal_state) {
399 Ok(est) => {
400 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 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 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 let mut od_sol = ODSolution::new(self.devices.clone(), IndexSet::new());
429
430 od_sol.push_time_update(initial_estimate);
431
432 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 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 let epoch = nominal_state.epoch();
453 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 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}