nyx_space/od/interlink/
sensitivity.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;
21use crate::od::interlink::InterlinkTxSpacecraft;
22use crate::od::msr::{Measurement, MeasurementType};
23use crate::od::prelude::sensitivity::{ScalarSensitivityT, TrackerSensitivity};
24use crate::od::{ODAlmanacSnafu, ODError, TrackingDevice};
25use crate::{Spacecraft, State};
26use anise::prelude::Almanac;
27use indexmap::IndexSet;
28use nalgebra::{DimName, OMatrix, U1};
29use snafu::ResultExt;
30use std::marker::PhantomData;
31use std::sync::Arc;
32
33struct ScalarSensitivity<SolveState: State, Rx, Tx>
34where
35    DefaultAllocator: Allocator<SolveState::Size>
36        + Allocator<SolveState::VecLength>
37        + Allocator<SolveState::Size, SolveState::Size>
38        + Allocator<U1, SolveState::Size>,
39{
40    sensitivity_row: OMatrix<f64, U1, SolveState::Size>,
41    _rx: PhantomData<Rx>,
42    _tx: PhantomData<Tx>,
43}
44
45impl TrackerSensitivity<Spacecraft, Spacecraft> for InterlinkTxSpacecraft
46where
47    DefaultAllocator: Allocator<<Spacecraft as State>::Size>
48        + Allocator<<Spacecraft as State>::VecLength>
49        + Allocator<<Spacecraft as State>::Size, <Spacecraft as State>::Size>,
50{
51    fn h_tilde<M: DimName>(
52        &self,
53        msr: &Measurement,
54        msr_types: &IndexSet<MeasurementType>,
55        rx: &Spacecraft,
56        almanac: Arc<Almanac>,
57    ) -> Result<OMatrix<f64, M, <Spacecraft as State>::Size>, ODError>
58    where
59        DefaultAllocator: Allocator<M> + Allocator<M, <Spacecraft as State>::Size>,
60    {
61        // Rebuild each row of the scalar sensitivities.
62        let mut mat = OMatrix::<f64, M, <Spacecraft as State>::Size>::identity();
63        for (ith_row, msr_type) in msr_types.iter().enumerate() {
64            if !msr.data.contains_key(msr_type) {
65                // Skip computation, this row is zero anyway.
66                continue;
67            }
68            let scalar_h =
69                <ScalarSensitivity<Spacecraft, Spacecraft, InterlinkTxSpacecraft> as ScalarSensitivityT<
70                    Spacecraft,
71                    Spacecraft,
72                    InterlinkTxSpacecraft,
73                >>::new(*msr_type, msr, rx, self, almanac.clone())?;
74
75            mat.set_row(ith_row, &scalar_h.sensitivity_row);
76        }
77        Ok(mat)
78    }
79}
80
81impl ScalarSensitivityT<Spacecraft, Spacecraft, InterlinkTxSpacecraft>
82    for ScalarSensitivity<Spacecraft, Spacecraft, InterlinkTxSpacecraft>
83{
84    fn new(
85        msr_type: MeasurementType,
86        msr: &Measurement,
87        rx: &Spacecraft,
88        tx: &InterlinkTxSpacecraft,
89        almanac: Arc<Almanac>,
90    ) -> Result<Self, ODError> {
91        let receiver = rx.orbit;
92
93        // Compute the device location in the receiver frame because we compute the sensitivity in that frame.
94        // This frame is required because the scalar measurements are frame independent, but the sensitivity
95        // must be in the estimation frame.
96        let transmitter = tx
97            .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone())
98            .context(ODAlmanacSnafu {
99                action: "computing transmitter location when computing sensitivity matrix",
100            })?;
101
102        let delta_r = receiver.radius_km - transmitter.radius_km;
103        let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;
104
105        match msr_type {
106            MeasurementType::Doppler => {
107                // If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range.
108                let ρ_km = msr.data.get(&MeasurementType::Range).ok_or_else(|| {
109                    ODError::MeasurementSimError {
110                        details: "Range measurement data is missing".to_string(),
111                    }
112                })?;
113                let ρ_dot_km_s = msr.data.get(&MeasurementType::Doppler).ok_or_else(|| {
114                    ODError::MeasurementSimError {
115                        details: "Doppler measurement data is missing".to_string(),
116                    }
117                })?;
118                let m11 = delta_r.x / ρ_km;
119                let m12 = delta_r.y / ρ_km;
120                let m13 = delta_r.z / ρ_km;
121                let m21 = delta_v.x / ρ_km - ρ_dot_km_s * delta_r.x / ρ_km.powi(2);
122                let m22 = delta_v.y / ρ_km - ρ_dot_km_s * delta_r.y / ρ_km.powi(2);
123                let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2);
124
125                let sensitivity_row =
126                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
127                        m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0,
128                    ]);
129
130                Ok(Self {
131                    sensitivity_row,
132                    _rx: PhantomData::<_>,
133                    _tx: PhantomData::<_>,
134                })
135            }
136            MeasurementType::Range => {
137                let ρ_km = msr.data.get(&MeasurementType::Range).ok_or_else(|| {
138                    ODError::MeasurementSimError {
139                        details: "Range measurement data is missing".to_string(),
140                    }
141                })?;
142                let m11 = delta_r.x / ρ_km;
143                let m12 = delta_r.y / ρ_km;
144                let m13 = delta_r.z / ρ_km;
145
146                let sensitivity_row =
147                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
148                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
149                    ]);
150
151                Ok(Self {
152                    sensitivity_row,
153                    _rx: PhantomData::<_>,
154                    _tx: PhantomData::<_>,
155                })
156            }
157            MeasurementType::Azimuth
158            | MeasurementType::Elevation
159            | MeasurementType::ReceiveFrequency
160            | MeasurementType::TransmitFrequency => Err(ODError::MeasurementSimError {
161                details: format!("{msr_type:?} is not supported for interlink"),
162            }),
163        }
164    }
165}
nyx_space/od/interlink/sensitivity.rs

nyx_space/od/interlink/
sensitivity.rs
