nyx_space/od/position/
sensitivity.rs

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

nyx_space/od/position/
sensitivity.rs
