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 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#[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 pub prop: Propagator<D>,
75 #[builder(default)]
77 pub kf_variant: KalmanVariant,
78 #[builder(default, setter(strip_option))]
80 pub resid_crit: Option<ResidRejectCrit>,
81 #[builder(default_code = "BTreeMap::new()")]
83 pub devices: BTreeMap<String, Trk>,
84 #[builder(default_code = "vec![]")]
86 pub process_noise: Vec<ProcessNoise<Accel>>,
87 #[builder(default_code = "1 * Unit::Minute")]
89 pub max_step: Duration,
90 #[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 #[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 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 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 let num_msrs = measurements.len();
161
162 let prop = self.prop.clone();
164 let mut prop_instance = prop.with(initial_estimate.nominal_state().with_stm(), self.almanac.clone()).quiet();
165
166 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; info!(
186 "Processing {num_msrs} measurements from {:?}",
187 arc.unique_aliases()
188 );
189
190 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 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 loop {
215 let delta_t = next_msr_epoch - epoch;
216
217 let next_step_size = delta_t.min(prop_instance.step_size).min(self.max_step);
219
220 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 traj.states.push(state);
240 }
241
242 let nominal_state = prop_instance.state;
246 epoch = nominal_state.epoch();
248
249 if (nominal_state.epoch() - next_msr_epoch).abs() < self.epoch_precision {
251 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 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 break;
276 }
277
278 if !msr.availability(&cur_msr_types).iter().any(|avail| *avail)
280 {
281 continue;
282 }
283
284 let mut real_obs: OVector<f64, MsrSize> =
286 msr.observation(&cur_msr_types);
287
288 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 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 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 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 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 debug!("time update {epoch:?}, next msr {next_msr_epoch:?}");
397 match kf.time_update(nominal_state) {
398 Ok(est) => {
399 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 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 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 let mut od_sol = ODSolution::new(self.devices.clone(), IndexSet::new());
428
429 od_sol.push_time_update(initial_estimate);
430
431 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 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 let epoch = nominal_state.epoch();
452 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 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}