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

nyx_space/od/interlink/
sensitivity.rs
