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::io::ConfigError;
20use crate::linalg::DefaultAllocator;
21use crate::linalg::allocator::Allocator;
22use crate::md::prelude::Interpolatable;
23use crate::od::{GroundStation, ODAlmanacSnafu, ODError, TrackingDevice};
24use crate::{Spacecraft, State};
25use anise::constants::SPEED_OF_LIGHT_KM_S;
26use anise::errors::OrientationSnafu;
27use anise::prelude::Almanac;
28use indexmap::IndexSet;
29use nalgebra::{DimName, OMatrix, U1};
30use snafu::ResultExt;
31use std::marker::PhantomData;
32
33use super::measurement::Measurement;
34use super::{MeasurementType, TrackingDataArc};
35
36pub trait ScalarSensitivityT<SolveState: State, Rx, Tx>
37where
38    Self: Sized,
39    DefaultAllocator: Allocator<SolveState::Size>
40        + Allocator<SolveState::VecLength>
41        + Allocator<SolveState::Size, SolveState::Size>,
42{
43    fn new(
44        msr_type: MeasurementType,
45        msr: &Measurement,
46        rx: &Rx,
47        tx: &Tx,
48        almanac: &Almanac,
49    ) -> Result<Self, ODError>;
50}
51
52/// Trait required to build a triplet of a solve-for state, a receiver, and a transmitter.
53pub trait TrackerSensitivity<SolveState: Interpolatable, Rx>: TrackingDevice<SolveState>
54where
55    Self: Sized,
56    DefaultAllocator: Allocator<SolveState::Size>
57        + Allocator<SolveState::VecLength>
58        + Allocator<SolveState::Size, SolveState::Size>,
59{
60    /// Returns the sensitivity matrix of size MxS where M is the number of simultaneous measurements
61    /// and S is the size of the state being solved for.
62    fn h_tilde<M: DimName>(
63        &self,
64        msr: &Measurement,
65        msr_types: &IndexSet<MeasurementType>, // Consider switching to array
66        rx: &Rx,
67        almanac: &Almanac,
68    ) -> Result<OMatrix<f64, M, SolveState::Size>, ODError>
69    where
70        DefaultAllocator: Allocator<M> + Allocator<M, SolveState::Size>;
71
72    /// Returns whether this tracker is expected to be compatible with the tracking data arc
73    fn is_compatible(&self, _tracker: &str, _arc: &TrackingDataArc) -> Result<(), ODError> {
74        Ok(())
75    }
76}
77
78pub struct ScalarSensitivity<SolveState: State, Rx, Tx>
79where
80    DefaultAllocator: Allocator<SolveState::Size>
81        + Allocator<SolveState::VecLength>
82        + Allocator<SolveState::Size, SolveState::Size>
83        + Allocator<U1, SolveState::Size>,
84{
85    pub sensitivity_row: OMatrix<f64, U1, SolveState::Size>,
86    pub _rx: PhantomData<Rx>,
87    pub _tx: PhantomData<Tx>,
88}
89
90impl TrackerSensitivity<Spacecraft, Spacecraft> for GroundStation
91where
92    DefaultAllocator: Allocator<<Spacecraft as State>::Size>
93        + Allocator<<Spacecraft as State>::VecLength>
94        + Allocator<<Spacecraft as State>::Size, <Spacecraft as State>::Size>,
95{
96    fn h_tilde<M: DimName>(
97        &self,
98        msr: &Measurement,
99        msr_types: &IndexSet<MeasurementType>,
100        rx: &Spacecraft,
101        almanac: &Almanac,
102    ) -> Result<OMatrix<f64, M, <Spacecraft as State>::Size>, ODError>
103    where
104        DefaultAllocator: Allocator<M> + Allocator<M, <Spacecraft as State>::Size>,
105    {
106        // Rebuild each row of the scalar sensitivities.
107        let mut mat = OMatrix::<f64, M, <Spacecraft as State>::Size>::identity();
108        for (ith_row, msr_type) in msr_types.iter().enumerate() {
109            if !msr.data.contains_key(msr_type) {
110                // Skip computation, this row is zero anyway.
111                continue;
112            }
113            let scalar_h =
114                <ScalarSensitivity<Spacecraft, Spacecraft, GroundStation> as ScalarSensitivityT<
115                    Spacecraft,
116                    Spacecraft,
117                    GroundStation,
118                >>::new(*msr_type, msr, rx, self, almanac)?;
119
120            mat.set_row(ith_row, &scalar_h.sensitivity_row);
121        }
122        Ok(mat)
123    }
124
125    fn is_compatible(&self, tracker: &str, arc: &TrackingDataArc) -> Result<(), ODError> {
126        // Ensure that the arc doppler config matches this ground station doppler config.
127        for msr in &arc.measurements {
128            if msr.doppler_config != self.doppler_config {
129                return Err(ODError::ODConfigError {
130                    source: ConfigError::InvalidConfig {
131                        msg: format!(
132                            "Tracker `{tracker}` Doppler config does not match measurement config @ {}\nGround Station:\n{:?}\nMeasurement:\n{:?}",
133                            msr.epoch, self.doppler_config, msr.doppler_config
134                        ),
135                    },
136                });
137            }
138        }
139        Ok(())
140    }
141}
142
143impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
144    for ScalarSensitivity<Spacecraft, Spacecraft, GroundStation>
145{
146    fn new(
147        msr_type: MeasurementType,
148        _msr: &Measurement,
149        rx: &Spacecraft,
150        tx: &GroundStation,
151        almanac: &Almanac,
152    ) -> Result<Self, ODError> {
153        let receiver = rx.orbit;
154
155        // Compute the device location in the receiver frame because we compute the sensitivity in that frame.
156        // This frame is required because the scalar measurements are frame independent, but the sensitivity
157        // must be in the estimation frame.
158        let transmitter = tx
159            .location(receiver.epoch, receiver.frame, almanac)
160            .context(ODAlmanacSnafu {
161                action: "computing transmitter location when computing sensitivity matrix",
162            })?;
163
164        // Relative geometry in estimation frame
165        let delta_r = receiver.radius_km - transmitter.radius_km;
166        let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;
167
168        let rho_km = delta_r.norm();
169        // let rho_km = *msr.data.get(&MeasurementType::Range).unwrap();
170        if rho_km < 1e-6 {
171            return Err(ODError::MeasurementSimError {
172                details: "Zero separation between ground station and spacecraft".to_string(),
173            });
174        }
175
176        // Line-of-sight unit vector pointing from station to spacecraft: d(rho)/d(r)
177        let u_los = delta_r / rho_km;
178
179        let sensitivity_row = match msr_type {
180            MeasurementType::Doppler => {
181                // Nominal line-of-sight range-rate from trajectory geometry
182                let rho_dot_km_s = u_los.dot(&delta_v);
183                // let rho_dot_km_s = msr.data.get(&MeasurementType::Doppler).unwrap();
184                let m11 = delta_r.x / rho_km;
185                let m12 = delta_r.y / rho_km;
186                let m13 = delta_r.z / rho_km;
187                let m21 = delta_v.x / rho_km - rho_dot_km_s * delta_r.x / rho_km.powi(2);
188                let m22 = delta_v.y / rho_km - rho_dot_km_s * delta_r.y / rho_km.powi(2);
189                let m23 = delta_v.z / rho_km - rho_dot_km_s * delta_r.z / rho_km.powi(2);
190
191                OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
192                    m21, m22, m23, m11, m12, m13, 0.0, 0.0, 0.0,
193                ])
194            }
195            MeasurementType::Range => {
196                // Velocity sensitivity due to retarded bounce epoch: d(rho)/d(v) = -tau * u_los
197                // This is required because we're computing the sensitivity at the reception epoch
198                // and not at the bounce epoch.
199                let tau_s = if tx.light_time_correction {
200                    rho_km / SPEED_OF_LIGHT_KM_S
201                } else {
202                    0.0
203                };
204
205                OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
206                    u_los.x,
207                    u_los.y,
208                    u_los.z,
209                    -tau_s * u_los.x,
210                    -tau_s * u_los.y,
211                    -tau_s * u_los.z,
212                    0.0,
213                    0.0,
214                    0.0,
215                ])
216            }
217
218            MeasurementType::Azimuth | MeasurementType::Elevation => {
219                // Transform line-of-sight vector into station topocentric SEZ frame
220                let rx_in_tx = almanac
221                    .transform_to(receiver, tx.location.frame.into(), None)
222                    .context(ODAlmanacSnafu {
223                        action: "transforming receiver to station frame for topocentric angles",
224                    })?;
225
226                let d_sez = rx_in_tx.radius_km; // Topocentric vector
227                let s = d_sez.x;
228                let e = d_sez.y;
229                let z = d_sez.z - tx.location.height_km;
230                let rho2_horiz = s * s + e * e;
231                let rho2_total = rho2_horiz + z * z;
232
233                if rho2_horiz < 1e-12 {
234                    return Err(ODError::MeasurementSimError {
235                        details: "Singularity at zenith for topocentric angle sensitivities".into(),
236                    });
237                }
238
239                // Partials in local SEZ frame
240                let d_sez_partials = if msr_type == MeasurementType::Azimuth {
241                    // Azimuth beta = atan2(E, -S) => d(beta)/d(S) = E / (S^2 + E^2), d(beta)/d(E) = -S / (S^2 + E^2)
242                    nalgebra::Vector3::new(e / rho2_horiz, -s / rho2_horiz, 0.0)
243                } else {
244                    // Elevation el = asin(Z / rho)
245                    let sqrt_horiz = rho2_horiz.sqrt();
246                    nalgebra::Vector3::new(
247                        -(s * z) / (rho2_total * sqrt_horiz),
248                        -(e * z) / (rho2_total * sqrt_horiz),
249                        sqrt_horiz / rho2_total,
250                    )
251                };
252
253                // Rotation matrix from SEZ back to estimation frame: R_sez_to_inertial
254                let rot_mat = almanac
255                    .rotate(tx.location.frame.into(), receiver.frame, receiver.epoch)
256                    .context(OrientationSnafu {
257                        action: "computing SEZ-to-inertial rotation",
258                    })
259                    .context(ODAlmanacSnafu {
260                        action: "computing SEZ-to-inertial rotation for angles",
261                    })?;
262
263                let d_inertial = rot_mat * d_sez_partials;
264
265                OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
266                    d_inertial.x,
267                    d_inertial.y,
268                    d_inertial.z,
269                    0.0,
270                    0.0,
271                    0.0,
272                    0.0,
273                    0.0,
274                    0.0,
275                ])
276            }
277
278            MeasurementType::ReceiveFrequency
279            | MeasurementType::TransmitFrequency
280            | MeasurementType::TransmitFrequencyRate => {
281                return Err(ODError::MeasurementSimError {
282                    details: format!("{msr_type:?} is only supported in CCSDS TDM parsing"),
283                });
284            }
285
286            MeasurementType::X | MeasurementType::Y | MeasurementType::Z => {
287                return Err(ODError::MeasurementSimError {
288                    details: format!("{msr_type:?} is not supported for ground stations"),
289                });
290            }
291        };
292
293        Ok(Self {
294            sensitivity_row,
295            _rx: PhantomData,
296            _tx: PhantomData,
297        })
298    }
299}
nyx_space/od/msr/sensitivity.rs

nyx_space/od/msr/
sensitivity.rs
