nyx_space/od/position/
sensitivity.rs

1use crate::linalg::allocator::Allocator;
2use crate::linalg::DefaultAllocator;
3use crate::od::ODError;
4use crate::{Spacecraft, State};
5use anise::prelude::Almanac;
6use indexmap::IndexSet;
7use nalgebra::{DimName, OMatrix, U1};
8use std::marker::PhantomData;
9use std::sync::Arc;
10
11use super::PositionDevice;
12use crate::od::msr::measurement::Measurement;
13use crate::od::msr::sensitivity::{ScalarSensitivity, ScalarSensitivityT, TrackerSensitivity};
14use crate::od::msr::MeasurementType;
15
16impl TrackerSensitivity<Spacecraft, Spacecraft> for PositionDevice
17where
18    DefaultAllocator: Allocator<<Spacecraft as State>::Size>
19        + Allocator<<Spacecraft as State>::VecLength>
20        + Allocator<<Spacecraft as State>::Size, <Spacecraft as State>::Size>,
21{
22    fn h_tilde<M: DimName>(
23        &self,
24        msr: &Measurement,
25        msr_types: &IndexSet<MeasurementType>,
26        rx: &Spacecraft,
27        almanac: Arc<Almanac>,
28    ) -> Result<OMatrix<f64, M, <Spacecraft as State>::Size>, ODError>
29    where
30        DefaultAllocator: Allocator<M> + Allocator<M, <Spacecraft as State>::Size>,
31    {
32        // Rebuild each row of the scalar sensitivities.
33        let mut mat = OMatrix::<f64, M, <Spacecraft as State>::Size>::zeros();
34        for (ith_row, msr_type) in msr_types.iter().enumerate() {
35            if !msr.data.contains_key(msr_type) {
36                // Skip computation, this row is zero anyway.
37                continue;
38            }
39            let scalar_h =
40                <ScalarSensitivity<Spacecraft, Spacecraft, PositionDevice> as ScalarSensitivityT<
41                    Spacecraft,
42                    Spacecraft,
43                    PositionDevice,
44                >>::new(*msr_type, msr, rx, self, almanac.clone())?;
45
46            mat.set_row(ith_row, &scalar_h.sensitivity_row);
47        }
48        Ok(mat)
49    }
50}
51
52impl ScalarSensitivityT<Spacecraft, Spacecraft, PositionDevice>
53    for ScalarSensitivity<Spacecraft, Spacecraft, PositionDevice>
54{
55    fn new(
56        msr_type: MeasurementType,
57        _msr: &Measurement,
58        _rx: &Spacecraft,
59        _tx: &PositionDevice,
60        _almanac: Arc<Almanac>,
61    ) -> Result<Self, ODError> {
62        let idx = match msr_type {
63            MeasurementType::X => 0,
64            MeasurementType::Y => 1,
65            MeasurementType::Z => 2,
66            _ => {
67                return Err(ODError::MeasurementSimError {
68                    details: format!("{msr_type:?} is not supported by XyzDevice"),
69                })
70            }
71        };
72
73        let mut sensitivity_row = OMatrix::<f64, U1, <Spacecraft as State>::Size>::zeros();
74        sensitivity_row[(0, idx)] = 1.0;
75
76        Ok(Self {
77            sensitivity_row,
78            _rx: PhantomData::<_>,
79            _tx: PhantomData::<_>,
80        })
81    }
82}
nyx_space/od/position/sensitivity.rs

nyx_space/od/position/
sensitivity.rs
