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
34pub trait 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
71pub struct 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    pub sensitivity_row: OMatrix<f64, U1, SolveState::Size>,
79    pub _rx: PhantomData<Rx>,
80    pub _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                // Always recompute the expected to range, a better model for scalar OD processing.
146                let ρ_km = tx
147                    .azimuth_elevation_of(receiver, None, &almanac)
148                    .context(ODAlmanacSnafu {
149                        action: "computing range for Doppler measurement",
150                    })?
151                    .range_km;
152
153                let ρ_dot_km_s = msr.data.get(&MeasurementType::Doppler).unwrap();
154                let m11 = delta_r.x / ρ_km;
155                let m12 = delta_r.y / ρ_km;
156                let m13 = delta_r.z / ρ_km;
157                let m21 = delta_v.x / ρ_km - ρ_dot_km_s * delta_r.x / ρ_km.powi(2);
158                let m22 = delta_v.y / ρ_km - ρ_dot_km_s * delta_r.y / ρ_km.powi(2);
159                let m23 = delta_v.z / ρ_km - ρ_dot_km_s * delta_r.z / ρ_km.powi(2);
160
161                let sensitivity_row =
162                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
163                        m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0,
164                    ]);
165
166                Ok(Self {
167                    sensitivity_row,
168                    _rx: PhantomData::<_>,
169                    _tx: PhantomData::<_>,
170                })
171            }
172            MeasurementType::Range => {
173                let ρ_km = msr.data.get(&MeasurementType::Range).unwrap();
174                let m11 = delta_r.x / ρ_km;
175                let m12 = delta_r.y / ρ_km;
176                let m13 = delta_r.z / ρ_km;
177
178                let sensitivity_row =
179                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
180                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
181                    ]);
182
183                Ok(Self {
184                    sensitivity_row,
185                    _rx: PhantomData::<_>,
186                    _tx: PhantomData::<_>,
187                })
188            }
189            MeasurementType::Azimuth => {
190                let denom = delta_r.x.powi(2) + delta_r.y.powi(2);
191                let m11 = -delta_r.y / denom;
192                let m12 = delta_r.x / denom;
193                let m13 = 0.0;
194
195                // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame.
196
197                let sensitivity_row =
198                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
199                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
200                    ]);
201
202                Ok(Self {
203                    sensitivity_row,
204                    _rx: PhantomData::<_>,
205                    _tx: PhantomData::<_>,
206                })
207            }
208            MeasurementType::Elevation => {
209                let r2 = delta_r.norm().powi(2);
210                let z2 = delta_r.z.powi(2);
211
212                // Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame.
213                let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - z2).sqrt());
214                let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - z2).sqrt());
215                let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2;
216
217                let sensitivity_row =
218                    OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
219                        m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
220                    ]);
221
222                Ok(Self {
223                    sensitivity_row,
224                    _rx: PhantomData::<_>,
225                    _tx: PhantomData::<_>,
226                })
227            }
228            MeasurementType::ReceiveFrequency | MeasurementType::TransmitFrequency => {
229                Err(ODError::MeasurementSimError {
230                    details: format!("{msr_type:?} is only supported in CCSDS TDM parsing"),
231                })
232            }
233            MeasurementType::X | MeasurementType::Y | MeasurementType::Z => {
234                Err(ODError::MeasurementSimError {
235                    details: format!("{msr_type:?} is not supported for ground stations"),
236                })
237            }
238        }
239    }
240}
nyx_space/od/msr/sensitivity.rs

nyx_space/od/msr/
sensitivity.rs
