nyx_space/od/msr/
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::md::prelude::Interpolatable;
22use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDevice};
23use crate::{Spacecraft, State};
24use anise::prelude::Almanac;
25use indexmap::IndexSet;
26use nalgebra::{DimName, OMatrix, U1};
27use snafu::ResultExt;
28use std::marker::PhantomData;
29
30use super::MeasurementType;
31use super::measurement::Measurement;
32
33pub trait ScalarSensitivityT<SolveState: State, Rx, Tx>
34where
35    Self: Sized,
36    DefaultAllocator: Allocator<SolveState::Size>
37        + Allocator<SolveState::VecLength>
38        + Allocator<SolveState::Size, SolveState::Size>,
39{
40    fn new(
41        msr_type: MeasurementType,
42        msr: &Measurement,
43        rx: &Rx,
44        tx: &Tx,
45        almanac: &Almanac,
46    ) -> Result<Self, ODError>;
47}
48
49/// Trait required to build a triplet of a solve-for state, a receiver, and a transmitter.
50pub trait TrackerSensitivity<SolveState: Interpolatable, Rx>: TrackingDevice<SolveState>
51where
52    Self: Sized,
53    DefaultAllocator: Allocator<SolveState::Size>
54        + Allocator<SolveState::VecLength>
55        + Allocator<SolveState::Size, SolveState::Size>,
56{
57    /// Returns the sensitivity matrix of size MxS where M is the number of simultaneous measurements
58    /// and S is the size of the state being solved for.
59    fn h_tilde<M: DimName>(
60        &self,
61        msr: &Measurement,
62        msr_types: &IndexSet<MeasurementType>, // Consider switching to array
63        rx: &Rx,
64        almanac: &Almanac,
65    ) -> Result<OMatrix<f64, M, SolveState::Size>, ODError>
66    where
67        DefaultAllocator: Allocator<M> + Allocator<M, SolveState::Size>;
68}
69
70pub struct ScalarSensitivity<SolveState: State, Rx, Tx>
71where
72    DefaultAllocator: Allocator<SolveState::Size>
73        + Allocator<SolveState::VecLength>
74        + Allocator<SolveState::Size, SolveState::Size>
75        + Allocator<U1, SolveState::Size>,
76{
77    pub sensitivity_row: OMatrix<f64, U1, SolveState::Size>,
78    pub _rx: PhantomData<Rx>,
79    pub _tx: PhantomData<Tx>,
80}
81
82impl TrackerSensitivity<Spacecraft, Spacecraft> for GroundStation
83where
84    DefaultAllocator: Allocator<<Spacecraft as State>::Size>
85        + Allocator<<Spacecraft as State>::VecLength>
86        + Allocator<<Spacecraft as State>::Size, <Spacecraft as State>::Size>,
87{
88    fn h_tilde<M: DimName>(
89        &self,
90        msr: &Measurement,
91        msr_types: &IndexSet<MeasurementType>,
92        rx: &Spacecraft,
93        almanac: &Almanac,
94    ) -> Result<OMatrix<f64, M, <Spacecraft as State>::Size>, ODError>
95    where
96        DefaultAllocator: Allocator<M> + Allocator<M, <Spacecraft as State>::Size>,
97    {
98        // Rebuild each row of the scalar sensitivities.
99        let mut mat = OMatrix::<f64, M, <Spacecraft as State>::Size>::identity();
100        for (ith_row, msr_type) in msr_types.iter().enumerate() {
101            if !msr.data.contains_key(msr_type) {
102                // Skip computation, this row is zero anyway.
103                continue;
104            }
105            let scalar_h =
106                <ScalarSensitivity<Spacecraft, Spacecraft, GroundStation> as ScalarSensitivityT<
107                    Spacecraft,
108                    Spacecraft,
109                    GroundStation,
110                >>::new(*msr_type, msr, rx, self, almanac)?;
111
112            mat.set_row(ith_row, &scalar_h.sensitivity_row);
113        }
114        Ok(mat)
115    }
116}
117
118impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
119    for ScalarSensitivity<Spacecraft, Spacecraft, GroundStation>
120{
121    fn new(
122        msr_type: MeasurementType,
123        msr: &Measurement,
124        rx: &Spacecraft,
125        tx: &GroundStation,
126        almanac: &Almanac,
127    ) -> Result<Self, ODError> {
128        let receiver = rx.orbit;
129
130        // Compute the device location in the receiver frame because we compute the sensitivity in that frame.
131        // This frame is required because the scalar measurements are frame independent, but the sensitivity
132        // must be in the estimation frame.
133        let transmitter = tx
134            .location(rx.orbit.epoch, rx.orbit.frame, almanac)
135            .context(ODAlmanacSnafu {
136                action: "computing transmitter location when computing sensitivity matrix",
137            })?;
138
139        let delta_r = receiver.radius_km - transmitter.radius_km;
140        let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;
141
142        match msr_type {
143            MeasurementType::Doppler => {
144                // Always recompute the expected to range, a better model for scalar OD processing.
145                let ρ_km = tx
146                    .azimuth_elevation_of(receiver, None, almanac)
147                    .context(ODAlmanacSnafu {
148                        action: "computing range for Doppler measurement",
149                    })?
150                    .range_km;
151
152                let ρ_dot_km_s = msr.data.get(&MeasurementType::Doppler).unwrap();
153                let m11 = delta_r.x / ρ_km;
154                let m12 = delta_r.y / ρ_km;
155                let m13 = delta_r.z / ρ_km;
156                let m21 = delta_v.x / ρ_km - ρ_dot_km_s * delta_r.x / ρ_km.powi(2);
157                let m22 = delta_v.y / ρ_km - ρ_dot_km_s * delta_r.y / ρ_km.powi(2);
158                let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2);
159
160                let sensitivity_row =
161                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
162                        m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0,
163                    ]);
164
165                Ok(Self {
166                    sensitivity_row,
167                    _rx: PhantomData::<_>,
168                    _tx: PhantomData::<_>,
169                })
170            }
171            MeasurementType::Range => {
172                let ρ_km = msr.data.get(&MeasurementType::Range).unwrap();
173                let m11 = delta_r.x / ρ_km;
174                let m12 = delta_r.y / ρ_km;
175                let m13 = delta_r.z / ρ_km;
176
177                let sensitivity_row =
178                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
179                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
180                    ]);
181
182                Ok(Self {
183                    sensitivity_row,
184                    _rx: PhantomData::<_>,
185                    _tx: PhantomData::<_>,
186                })
187            }
188            MeasurementType::Azimuth => {
189                let denom = delta_r.x.powi(2) + delta_r.y.powi(2);
190                let m11 = -delta_r.y / denom;
191                let m12 = delta_r.x / denom;
192                let m13 = 0.0;
193
194                // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame.
195
196                let sensitivity_row =
197                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
198                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
199                    ]);
200
201                Ok(Self {
202                    sensitivity_row,
203                    _rx: PhantomData::<_>,
204                    _tx: PhantomData::<_>,
205                })
206            }
207            MeasurementType::Elevation => {
208                let r2 = delta_r.norm().powi(2);
209                let z2 = delta_r.z.powi(2);
210
211                // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame.
212                let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - z2).sqrt());
213                let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - z2).sqrt());
214                let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2;
215
216                let sensitivity_row =
217                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
218                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
219                    ]);
220
221                Ok(Self {
222                    sensitivity_row,
223                    _rx: PhantomData::<_>,
224                    _tx: PhantomData::<_>,
225                })
226            }
227            MeasurementType::ReceiveFrequency
228            | MeasurementType::TransmitFrequency
229            | MeasurementType::TransmitFrequencyRate => Err(ODError::MeasurementSimError {
230                details: format!("{msr_type:?} is only supported in CCSDS TDM parsing"),
231            }),
232            MeasurementType::X | MeasurementType::Y | MeasurementType::Z => {
233                Err(ODError::MeasurementSimError {
234                    details: format!("{msr_type:?} is not supported for ground stations"),
235                })
236            }
237        }
238    }
239}
nyx_space/od/msr/sensitivity.rs

nyx_space/od/msr/
sensitivity.rs
