Skip to content

Commit

Permalink
Debugging in progress
Browse files Browse the repository at this point in the history
  • Loading branch information
@ChristopherRabotin
ChristopherRabotin committed Nov 29, 2024
1 parent 3881e21 commit 6c3fd08
Show file tree
Hide file tree
Showing 3 changed files with 66 additions and 73 deletions.
7 changes: 5 additions & 2 deletions src/od/filter/kalman.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -299,8 +299,11 @@ where

// Compute the prefit ratio for the automatic rejection
let r_k_inv = r_k.clone().try_inverse().ok_or(ODError::SingularNoiseRk)?;
let ratio_mat = prefit.transpose() * r_k_inv * &prefit;
let ratio = ratio_mat[0].sqrt();

let ratio_mat = prefit.transpose() * &r_k_inv * &prefit;
let ratio = dbg!(ratio_mat[0].powi(2));
let o_r_k = r_k[(0, 0)];
dbg!(prefit[0], o_r_k, prefit[0].abs() < o_r_k,);

if let Some(resid_reject) = resid_rejection {
if ratio > resid_reject.num_sigmas {
Expand Down
56 changes: 15 additions & 41 deletions src/od/msr/sensitivity.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -127,29 +127,18 @@ impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
almanac: Arc<Almanac>,
) -> Result<Self, ODError> {
let receiver = rx.orbit;

// Compute the device location in the receiver frame because we compute the sensitivity in that frame.
// This frame is required because the scalar measurements are frame independent, but the sensitivity
// must be in the estimation frame.
// let transmitter = tx
// .location(rx.orbit.epoch, rx.orbit.frame, almanac.clone())
// .context(ODAlmanacSnafu {
// action: "computing transmitter location when computing sensitivity matrix",
// })?;

// let delta_r = receiver.radius_km - transmitter.radius_km;
// let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;

let receiver = rx.orbit;
// Compute the device location ensuring that the receiver and transmitter are in the same frame.
let transmitter = tx
.location(rx.orbit.epoch, rx.orbit.frame, almanac.clone())
.context(ODAlmanacSnafu {
action: "computing transmitter location when computing sensitivity matrix",
})?;

// Should this be in the SEZ frame?
// let delta_r = receiver.radius_km - transmitter.radius_km;
// let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;
let delta_r = receiver.radius_km - transmitter.radius_km;
let delta_v = receiver.velocity_km_s - transmitter.velocity_km_s;

// SEZ DCM is topo to fixed
let sez_dcm = transmitter
Expand All @@ -166,22 +155,6 @@ impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
// action: "transforming transmitter to SEZ",
// })?;

// Convert the receiver into the transmitter frame.
let rx_in_tx_frame = almanac
.transform_to(rx.orbit, transmitter.frame, None)
.unwrap();
// Convert into SEZ frame
let rx_sez = (sez_dcm.transpose() * rx_in_tx_frame).unwrap();
// .context(EphemerisPhysicsSnafu { action: "" })
// .context(EphemerisSnafu {
// action: "transforming received to SEZ",
// })?;

// let delta_r = rx_sez.radius_km - tx_sez.radius_km;
// let delta_v = rx_sez.velocity_km_s - tx_sez.velocity_km_s;
let delta_r = tx_sez.radius_km;
let delta_v = tx_sez.velocity_km_s;

match msr_type {
MeasurementType::Doppler => {
// If we have a simultaneous measurement of the range, use that, otherwise we compute the expected range.
Expand Down Expand Up @@ -233,10 +206,13 @@ impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
})
}
MeasurementType::Azimuth => {
let m11 = -delta_r.y / (delta_r.x.powi(2) + delta_r.y.powi(2));
let m12 = delta_r.x / (delta_r.x.powi(2) + delta_r.y.powi(2));
let denom = tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2);
let m11 = -tx_sez.radius_km.y / denom;
let m12 = tx_sez.radius_km.x / denom;
let m13 = 0.0;

// Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame.

let effective_sensitivity_row =
OMatrix::<f64, U1, Const<6>>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0])
* sez_dcm.state_dcm();
Expand All @@ -253,11 +229,14 @@ impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
})

Check warning on line 229 in src/od/msr/sensitivity.rs

View check run for this annotation

Codecov / codecov/patch

src/od/msr/sensitivity.rs#L225-L229 

Added lines #L225 - L229 were not covered by tests
}
MeasurementType::Elevation => {
let r2 = delta_r.norm().powi(2);
let r2 = tx_sez.radius_km.norm().powi(2);
let z2 = tx_sez.radius_km.z.powi(2);

// Build the sensitivity matrix in the transmitter frame and rotate back into the inertial frame.

let m11 = -(delta_r.x * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt());
let m12 = -(delta_r.y * delta_r.z) / (r2 * (r2 - delta_r.z.powi(2)).sqrt());
let m13 = (delta_r.x.powi(2) + delta_r.y.powi(2)).sqrt() / r2;
let m11 = -(tx_sez.radius_km.x * tx_sez.radius_km.z) / (r2 * (r2 - z2).sqrt());
let m12 = -(tx_sez.radius_km.y * tx_sez.radius_km.z) / (r2 * (r2 - z2).sqrt());
let m13 = (tx_sez.radius_km.x.powi(2) + tx_sez.radius_km.y.powi(2)).sqrt() / r2;

let effective_sensitivity_row =
OMatrix::<f64, U1, Const<6>>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0])
Expand All @@ -268,11 +247,6 @@ impl ScalarSensitivityT<Spacecraft, Spacecraft, GroundStation>
sensitivity_row[i] = val;
}

Check warning on line 248 in src/od/msr/sensitivity.rs

View check run for this annotation

Codecov / codecov/patch

src/od/msr/sensitivity.rs#L232-L248 

Added lines #L232 - L248 were not covered by tests

// let sensitivity_row =
// OMatrix::<f64, U1, <Spacecraft as State>::Size>::from_row_slice(&[
// m11, m12, m13, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
// ]);

Ok(Self {
sensitivity_row,
_rx: PhantomData::<_>,
Expand Down
76 changes: 46 additions & 30 deletions tests/orbit_determination/robust_az_el.rs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,6 +3,7 @@ extern crate pretty_env_logger;

use anise::constants::celestial_objects::{JUPITER_BARYCENTER, MOON, SATURN_BARYCENTER, SUN};
use anise::constants::frames::IAU_EARTH_FRAME;
use nalgebra::Const;
use nyx::cosmic::Orbit;
use nyx::dynamics::orbital::OrbitalDynamics;
use nyx::dynamics::SpacecraftDynamics;
Expand Down Expand Up @@ -34,42 +35,42 @@ fn almanac() -> Arc<Almanac> {
fn devices(almanac: Arc<Almanac>) -> BTreeMap<String, GroundStation> {
let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap();
let elevation_mask = 10.0;
let integration_time = Some(60 * Unit::Second);
let integration_time = None; //Some(60 * Unit::Second);

let dss65_madrid = GroundStation::dss65_madrid(
elevation_mask,
StochasticNoise::MIN,
StochasticNoise::MIN,
StochasticNoise::default_range_km(),
StochasticNoise::default_doppler_km_s(),
iau_earth,
)
.with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN)
.with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN)
.without_msr_type(MeasurementType::Range)
.without_msr_type(MeasurementType::Doppler)
// .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN)
// .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN)
// .without_msr_type(MeasurementType::Range)
// .without_msr_type(MeasurementType::Doppler)
.with_integration_time(integration_time);

let dss34_canberra = GroundStation::dss34_canberra(
elevation_mask,
StochasticNoise::MIN,
StochasticNoise::MIN,
StochasticNoise::default_range_km(),
StochasticNoise::default_doppler_km_s(),
iau_earth,
)
.with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN)
.with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN)
.without_msr_type(MeasurementType::Range)
.without_msr_type(MeasurementType::Doppler)
// .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN)
// .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN)
// .without_msr_type(MeasurementType::Range)
// .without_msr_type(MeasurementType::Doppler)
.with_integration_time(integration_time);

let dss13_goldstone = GroundStation::dss13_goldstone(
elevation_mask,
StochasticNoise::MIN,
StochasticNoise::MIN,
StochasticNoise::default_range_km(),
StochasticNoise::default_doppler_km_s(),
iau_earth,
)
.with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN)
.with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN)
.without_msr_type(MeasurementType::Range)
.without_msr_type(MeasurementType::Doppler)
// .with_msr_type(MeasurementType::Azimuth, StochasticNoise::MIN)
// .with_msr_type(MeasurementType::Elevation, StochasticNoise::MIN)
// .without_msr_type(MeasurementType::Range)
// .without_msr_type(MeasurementType::Doppler)
.with_integration_time(integration_time);

let mut devices = BTreeMap::new();
Expand Down Expand Up @@ -133,7 +134,7 @@ fn estimator_setup() -> Propagator<SpacecraftDynamics> {
let step_size = 60.0 * Unit::Second;
let opts = IntegratorOptions::with_max_step(step_size);

let bodies = vec![MOON, SUN, JUPITER_BARYCENTER];
let bodies = vec![MOON, SUN, JUPITER_BARYCENTER, SATURN_BARYCENTER];
let estimator = SpacecraftDynamics::new(OrbitalDynamics::point_masses(bodies));

Propagator::new(estimator, IntegratorMethod::DormandPrince78, opts)
Expand All @@ -159,7 +160,7 @@ fn od_robust_test_ekf_rng_dop_az_el(

let iau_earth = almanac.frame_from_uid(IAU_EARTH_FRAME).unwrap();
// Define the ground stations.
let ekf_num_meas = 3000;
let ekf_num_meas = 10;
// Set the disable time to be very low to test enable/disable sequence
let ekf_disable_time = 3 * Unit::Minute;
let elevation_mask = 0.0;
Expand Down Expand Up @@ -190,7 +191,7 @@ fn od_robust_test_ekf_rng_dop_az_el(
]);

// Define the propagator information.
let prop_time = 0.2 * Unit::Day;
let prop_time = 1.1 * initial_state.orbit.period().unwrap();

let initial_state_dev = initial_estimate.nominal_state;
let (init_rss_pos_km, init_rss_vel_km_s) =
Expand Down Expand Up @@ -242,17 +243,32 @@ fn od_robust_test_ekf_rng_dop_az_el(

let trig = EkfTrigger::new(ekf_num_meas, ekf_disable_time);

let mut odp = SpacecraftODProcess::ekf(
prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()),
let mut odp = ODProcess::<
SpacecraftDynamics,
Const<1>,
Const<3>,
KF<Spacecraft, Const<3>, Const<1>>,
GroundStation,
>::ekf(
prop_est,
kf,
devices,
trig,
Some(ResidRejectCrit::default()),
almanac,
);

// let mut odp = SpacecraftODProcess::ekf(
// prop_est, kf, devices, trig, None, // Some(ResidRejectCrit::default()),
// almanac,
// );

// Let's filter and iterate on the initial subset of the arc to refine the initial estimate
let subset = arc.clone().filter_by_offset(..3.hours());
let remaining = arc.filter_by_offset(3.hours()..);
// let subset = arc.clone().filter_by_offset(..3.hours());
// let remaining = arc.filter_by_offset(3.hours()..);

odp.process_arc(&subset).unwrap();
odp.iterate_arc(&subset, IterationConf::once()).unwrap();
odp.process_arc(&arc).unwrap();
odp.iterate_arc(&arc, IterationConf::once()).unwrap();

// Grab the comparison state, which differs from the initial state because of the integration time of the measurements.
let cmp_state = traj
Expand All @@ -269,10 +285,10 @@ fn od_robust_test_ekf_rng_dop_az_el(
(cmp_state.orbit - odp.estimates[0].state().orbit).unwrap()
);

odp.process_arc(&remaining).unwrap();
// odp.process_arc(&remaining).unwrap();

odp.to_parquet(
&remaining,
&arc,
path.with_file_name("ekf_rng_dpl_az_el_odp.parquet"),
ExportCfg::default(),
)
Expand Down

0 comments on commit 6c3fd08

Please sign in to comment.