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

nyx_space/od/msr/
sensitivity.rs
