From 6c3fd084335a9651464aef80ab2050cf37d7fe30 Mon Sep 17 00:00:00 2001 From: Christopher Rabotin Date: Thu, 28 Nov 2024 17:36:48 -0700 Subject: [PATCH] Debugging in progress --- src/od/filter/kalman.rs | 7 ++- src/od/msr/sensitivity.rs | 56 +++++------------ tests/orbit_determination/robust_az_el.rs | 76 ++++++++++++++--------- 3 files changed, 66 insertions(+), 73 deletions(-) diff --git a/src/od/filter/kalman.rs b/src/od/filter/kalman.rs index 3ebca8ce..0a9510e2 100644 --- a/src/od/filter/kalman.rs +++ b/src/od/filter/kalman.rs @@ -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 { diff --git a/src/od/msr/sensitivity.rs b/src/od/msr/sensitivity.rs index 6813de55..efec9d11 100644 --- a/src/od/msr/sensitivity.rs +++ b/src/od/msr/sensitivity.rs @@ -127,29 +127,18 @@ impl ScalarSensitivityT almanac: Arc, ) -> Result { 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 @@ -166,22 +155,6 @@ impl ScalarSensitivityT // 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. @@ -233,10 +206,13 @@ impl ScalarSensitivityT }) } 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::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) * sez_dcm.state_dcm(); @@ -253,11 +229,14 @@ impl ScalarSensitivityT }) } 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::>::from_row_slice(&[m11, m12, m13, 0.0, 0.0, 0.0]) @@ -268,11 +247,6 @@ impl ScalarSensitivityT sensitivity_row[i] = val; } - // let sensitivity_row = - // OMatrix::::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::<_>, diff --git a/tests/orbit_determination/robust_az_el.rs b/tests/orbit_determination/robust_az_el.rs index a0f2733d..10e3dd8d 100644 --- a/tests/orbit_determination/robust_az_el.rs +++ b/tests/orbit_determination/robust_az_el.rs @@ -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; @@ -34,42 +35,42 @@ fn almanac() -> Arc { fn devices(almanac: Arc) -> BTreeMap { 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(); @@ -133,7 +134,7 @@ fn estimator_setup() -> Propagator { 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) @@ -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; @@ -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) = @@ -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, 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 @@ -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(), )