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 = <InterlinkTxSpacecraft as TrackingDevice<Spacecraft>>::location(
97            tx,
98            rx.orbit.epoch,
99            rx.orbit.frame,
100            almanac.clone(),
101        )
102        .context(ODAlmanacSnafu {
103            action: "computing transmitter location when computing sensitivity matrix",
104        })?;
105
106        let delta_r = receiver.radius_km - transmitter.radius_km;
107        let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;
108
109        match msr_type {
110            MeasurementType::Doppler => {
111                // If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range.
112                let ρ_km = msr.data.get(&MeasurementType::Range).ok_or_else(|| {
113                    ODError::MeasurementSimError {
114                        details: "Range measurement data is missing".to_string(),
115                    }
116                })?;
117                let ρ_dot_km_s = msr.data.get(&MeasurementType::Doppler).ok_or_else(|| {
118                    ODError::MeasurementSimError {
119                        details: "Doppler measurement data is missing".to_string(),
120                    }
121                })?;
122                let m11 = delta_r.x / ρ_km;
123                let m12 = delta_r.y / ρ_km;
124                let m13 = delta_r.z / ρ_km;
125                let m21 = delta_v.x / ρ_km - ρ_dot_km_s * delta_r.x / ρ_km.powi(2);
126                let m22 = delta_v.y / ρ_km - ρ_dot_km_s * delta_r.y / ρ_km.powi(2);
127                let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2);
128
129                let sensitivity_row =
130                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
131                        m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0,
132                    ]);
133
134                Ok(Self {
135                    sensitivity_row,
136                    _rx: PhantomData::<_>,
137                    _tx: PhantomData::<_>,
138                })
139            }
140            MeasurementType::Range => {
141                let ρ_km = msr.data.get(&MeasurementType::Range).ok_or_else(|| {
142                    ODError::MeasurementSimError {
143                        details: "Range measurement data is missing".to_string(),
144                    }
145                })?;
146                let m11 = delta_r.x / ρ_km;
147                let m12 = delta_r.y / ρ_km;
148                let m13 = delta_r.z / ρ_km;
149
150                let sensitivity_row =
151                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
152                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
153                    ]);
154
155                Ok(Self {
156                    sensitivity_row,
157                    _rx: PhantomData::<_>,
158                    _tx: PhantomData::<_>,
159                })
160            }
161            MeasurementType::Azimuth
162            | MeasurementType::Elevation
163            | MeasurementType::ReceiveFrequency
164            | MeasurementType::TransmitFrequency => Err(ODError::MeasurementSimError {
165                details: format!("{msr_type:?} is not supported for interlink"),
166            }),
167        }
168    }
169}
nyx_space/od/interlink/sensitivity.rs

nyx_space/od/interlink/
sensitivity.rs
