extern crate hyperdual;
extern crate rand;
extern crate rand_distr;
use self::hyperdual::linalg::norm;
use self::hyperdual::{hyperspace_from_vector, Hyperdual};
use self::rand::thread_rng;
use self::rand_distr::{Distribution, Normal};
use super::serde::ser::SerializeSeq;
use super::serde::{Serialize, Serializer};
use super::{Measurement, MeasurementDevice, TimeTagged};
use crate::dimensions::{
DimName, Matrix1x6, Matrix2x6, Vector1, Vector2, VectorN, U1, U2, U3, U6, U7,
};
use crate::dynamics::spacecraft::SpacecraftState;
use crate::time::Epoch;
use celestia::{Cosm, Frame, State};
use utils::{r2, r3};
#[derive(Debug, Clone)]
pub struct GroundStation<'a> {
pub name: String,
pub elevation_mask: f64,
pub latitude: f64,
pub longitude: f64,
pub height: f64,
pub frame: Frame,
pub cosm: &'a Cosm,
range_noise: Normal<f64>,
range_rate_noise: Normal<f64>,
}
impl<'a> GroundStation<'a> {
pub fn from_noise_values(
name: String,
elevation_mask: f64,
latitude: f64,
longitude: f64,
height: f64,
range_noise: f64,
range_rate_noise: f64,
frame: Frame,
cosm: &'a Cosm,
) -> Self {
Self {
name,
elevation_mask,
latitude,
longitude,
height,
frame,
cosm,
range_noise: Normal::new(0.0, range_noise).unwrap(),
range_rate_noise: Normal::new(0.0, range_rate_noise).unwrap(),
}
}
pub fn dss65_madrid(
elevation_mask: f64,
range_noise: f64,
range_rate_noise: f64,
cosm: &'a Cosm,
) -> Self {
Self::from_noise_values(
"Madrid".to_string(),
elevation_mask,
40.427_222,
4.250_556,
0.834_939,
range_noise,
range_rate_noise,
cosm.frame("IAU Earth"),
cosm,
)
}
pub fn dss34_canberra(
elevation_mask: f64,
range_noise: f64,
range_rate_noise: f64,
cosm: &'a Cosm,
) -> Self {
Self::from_noise_values(
"Canberra".to_string(),
elevation_mask,
-35.398_333,
148.981_944,
0.691_750,
range_noise,
range_rate_noise,
cosm.frame("IAU Earth"),
cosm,
)
}
pub fn dss13_goldstone(
elevation_mask: f64,
range_noise: f64,
range_rate_noise: f64,
cosm: &'a Cosm,
) -> Self {
Self::from_noise_values(
"Goldstone".to_string(),
elevation_mask,
35.247_164,
243.205,
1.071_149_04,
range_noise,
range_rate_noise,
cosm.frame("IAU Earth"),
cosm,
)
}
}
impl<'a> MeasurementDevice<State, StdMeasurement> for GroundStation<'a> {
fn measure(&self, rx: &State) -> Option<StdMeasurement> {
use std::f64::consts::PI;
let dt = rx.dt;
let station_state =
State::from_geodesic(self.latitude, self.longitude, self.height, dt, self.frame);
let tx = self.cosm.frame_chg(&station_state, rx.frame);
let rho_ecef = rx.radius() - tx.radius();
let rho_sez =
r2(PI / 2.0 - self.latitude.to_radians()) * r3(self.longitude.to_radians()) * rho_ecef;
let elevation = (rho_sez[(2, 0)] / rho_ecef.norm()).asin().to_degrees();
Some(StdMeasurement::new(
dt,
tx,
*rx,
elevation >= self.elevation_mask,
&self.range_noise,
&self.range_rate_noise,
))
}
}
impl<'a> MeasurementDevice<SpacecraftState, StdMeasurement> for GroundStation<'a> {
fn measure(&self, sc_rx: &SpacecraftState) -> Option<StdMeasurement> {
let rx = &sc_rx.orbit;
match rx.frame {
Frame::Geoid { .. } => {
use std::f64::consts::PI;
let dt = rx.dt;
let station_state = State::from_geodesic(
self.latitude,
self.longitude,
self.height,
dt,
self.frame,
);
let tx = self.cosm.frame_chg(&station_state, rx.frame);
let rho_ecef = rx.radius() - tx.radius();
let rho_sez = r2(PI / 2.0 - self.latitude.to_radians())
* r3(self.longitude.to_radians())
* rho_ecef;
let elevation = (rho_sez[(2, 0)] / rho_ecef.norm()).asin().to_degrees();
Some(StdMeasurement::new(
dt,
tx,
*rx,
elevation >= self.elevation_mask,
&self.range_noise,
&self.range_rate_noise,
))
}
_ => {
error!("Receiver is not on a geoid");
None
}
}
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct StdMeasurement {
pub dt: Epoch,
pub obs: Vector2<f64>,
visible: bool,
h_tilde: Matrix2x6<f64>,
}
impl StdMeasurement {
pub fn range(&self) -> f64 {
self.obs[(0, 0)]
}
pub fn range_rate(&self) -> f64 {
self.obs[(1, 0)]
}
fn compute_sensitivity(
state: &VectorN<Hyperdual<f64, U7>, U6>,
range_noise: f64,
range_rate_noise: f64,
) -> (Vector2<f64>, Matrix2x6<f64>) {
let range_vec = state.fixed_rows::<U3>(0).into_owned();
let velocity_vec = state.fixed_rows::<U3>(3).into_owned();
let delta_v_vec = velocity_vec / norm(&range_vec);
let range = norm(&range_vec) + Hyperdual::from(range_noise);
let range_rate = range_vec.dot(&delta_v_vec) + Hyperdual::from(range_rate_noise);
let mut fx = Vector2::zeros();
let mut pmat = Matrix2x6::zeros();
for i in 0..U2::dim() {
fx[i] = if i == 0 {
range.real()
} else {
range_rate.real()
};
for j in 1..U7::dim() {
pmat[(i, j - 1)] = if i == 0 { range[j] } else { range_rate[j] };
}
}
(fx, pmat)
}
pub fn noiseless(dt: Epoch, tx: State, rx: State, visible: bool) -> StdMeasurement {
Self::raw(dt, tx, rx, visible, 0.0, 0.0)
}
pub fn new<D: Distribution<f64>>(
dt: Epoch,
tx: State,
rx: State,
visible: bool,
range_dist: &D,
range_rate_dist: &D,
) -> StdMeasurement {
Self::raw(
dt,
tx,
rx,
visible,
range_dist.sample(&mut thread_rng()),
range_rate_dist.sample(&mut thread_rng()),
)
}
pub fn raw(
dt: Epoch,
tx: State,
rx: State,
visible: bool,
range_noise: f64,
range_rate_noise: f64,
) -> StdMeasurement {
assert_eq!(tx.frame, rx.frame, "tx & rx in different frames");
assert_eq!(tx.dt, rx.dt, "tx & rx states have different times");
let hyperstate = hyperspace_from_vector(&(rx - tx).to_cartesian_vec());
let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate, range_noise, range_rate_noise);
StdMeasurement {
dt,
obs,
visible,
h_tilde,
}
}
pub fn real(dt: Epoch, range: f64, range_rate: f64) -> Self {
Self {
dt,
obs: Vector2::new(range, range_rate),
visible: true,
h_tilde: Matrix2x6::zeros(),
}
}
}
impl Measurement for StdMeasurement {
type StateSize = U6;
type MeasurementSize = U2;
fn observation(&self) -> Vector2<f64> {
self.obs
}
fn sensitivity(&self) -> Matrix2x6<f64> {
self.h_tilde
}
fn visible(&self) -> bool {
self.visible
}
}
impl TimeTagged for StdMeasurement {
fn epoch(&self) -> Epoch {
self.dt
}
fn set_epoch(&mut self, dt: Epoch) {
self.dt = dt
}
}
impl Serialize for StdMeasurement {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: Serializer,
{
let mut seq = serializer.serialize_seq(Some(3))?;
seq.serialize_element(&self.dt.as_mjd_tai_days())?;
let obs = self.observation();
seq.serialize_element(&obs[(0, 0)])?;
seq.serialize_element(&obs[(1, 0)])?;
seq.end()
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct RangeMsr {
pub dt: Epoch,
pub obs: Vector1<f64>,
visible: bool,
h_tilde: Matrix1x6<f64>,
}
impl RangeMsr {
pub fn range(&self) -> f64 {
self.obs[(0, 0)]
}
fn compute_sensitivity(
state: &VectorN<Hyperdual<f64, U7>, U6>,
) -> (Vector1<f64>, Matrix1x6<f64>) {
let range_vec = state.fixed_rows::<U3>(0).into_owned();
let range = norm(&range_vec);
let fx = Vector1::new(range.real());
let mut pmat = Matrix1x6::zeros();
for j in 1..U7::dim() {
pmat[(j - 1)] = range[j];
}
(fx, pmat)
}
pub fn new(_: Epoch, tx: State, rx: State, visible: bool) -> RangeMsr {
assert_eq!(tx.frame, rx.frame, "tx and rx in different frames");
assert_eq!(tx.dt, rx.dt, "tx and rx states have different times");
let dt = tx.dt;
let hyperstate = hyperspace_from_vector(&(rx - tx).to_cartesian_vec());
let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate);
RangeMsr {
dt,
obs,
visible,
h_tilde,
}
}
}
impl Measurement for RangeMsr {
type StateSize = U6;
type MeasurementSize = U1;
fn observation(&self) -> Vector1<f64> {
self.obs
}
fn sensitivity(&self) -> Matrix1x6<f64> {
self.h_tilde
}
fn visible(&self) -> bool {
self.visible
}
}
impl TimeTagged for RangeMsr {
fn epoch(&self) -> Epoch {
self.dt
}
fn set_epoch(&mut self, dt: Epoch) {
self.dt = dt
}
}
impl Serialize for RangeMsr {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: Serializer,
{
let mut seq = serializer.serialize_seq(Some(3))?;
seq.serialize_element(&self.dt.as_mjd_tai_days())?;
let obs = self.observation();
seq.serialize_element(&obs[(0, 0)])?;
seq.end()
}
}
#[derive(Debug, Clone, Copy, PartialEq)]
pub struct DopplerMsr {
pub dt: Epoch,
pub obs: Vector1<f64>,
visible: bool,
h_tilde: Matrix1x6<f64>,
}
impl DopplerMsr {
pub fn range_rate(&self) -> f64 {
self.obs[(0, 0)]
}
fn compute_sensitivity(
state: &VectorN<Hyperdual<f64, U7>, U6>,
) -> (Vector1<f64>, Matrix1x6<f64>) {
let range_vec = state.fixed_rows::<U3>(0).into_owned();
let velocity_vec = state.fixed_rows::<U3>(3).into_owned();
let delta_v_vec = velocity_vec / norm(&range_vec);
let range_rate = range_vec.dot(&delta_v_vec);
let fx = Vector1::new(range_rate.real());
let mut pmat = Matrix1x6::zeros();
for j in 1..U7::dim() {
pmat[(0, j - 1)] = range_rate[j];
}
(fx, pmat)
}
pub fn new(_: Epoch, tx: State, rx: State, visible: bool) -> DopplerMsr {
assert_eq!(tx.frame, rx.frame, "tx and rx in different frames");
assert_eq!(tx.dt, rx.dt, "tx and rx states have different times");
let dt = tx.dt;
let hyperstate = hyperspace_from_vector(&(rx - tx).to_cartesian_vec());
let (obs, h_tilde) = Self::compute_sensitivity(&hyperstate);
DopplerMsr {
dt,
obs,
visible,
h_tilde,
}
}
}
impl Measurement for DopplerMsr {
type StateSize = U6;
type MeasurementSize = U1;
fn observation(&self) -> Vector1<f64> {
self.obs
}
fn sensitivity(&self) -> Matrix1x6<f64> {
self.h_tilde
}
fn visible(&self) -> bool {
self.visible
}
}
impl TimeTagged for DopplerMsr {
fn epoch(&self) -> Epoch {
self.dt
}
fn set_epoch(&mut self, dt: Epoch) {
self.dt = dt
}
}
impl Serialize for DopplerMsr {
fn serialize<S>(&self, serializer: S) -> Result<S::Ok, S::Error>
where
S: Serializer,
{
let mut seq = serializer.serialize_seq(Some(3))?;
seq.serialize_element(&self.dt.as_mjd_tai_days())?;
let obs = self.observation();
seq.serialize_element(&obs[(0, 0)])?;
seq.end()
}
}