Skip to content

Commit 8d33fed

Browse files
ChristopherRabotinChristopherRabotin
committed
Finish up od modulus tests
1 parent 76f6752 commit 8d33fed

File tree

3 files changed

+121
-94
lines changed

3 files changed

+121
-94
lines changed

‎examples/04_lro_od/plot_od_rslt.py

+5-1
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,8 @@
99

1010
@click.command
1111
@click.option("-p", "--path", type=str, default="./04_lro_od_results.parquet")
12-
def main(path: str):
12+
@click.option("-f", "--full", type=bool, default=True)
13+
def main(path: str, full: bool):
1314
df = pl.read_parquet(path)
1415

1516
df = (
@@ -123,6 +124,9 @@ def main(path: str):
123124
y=["Sigma Vx (RIC) (km/s)", "Sigma Vy (RIC) (km/s)", "Sigma Vz (RIC) (km/s)"],
124125
).show()
125126

127+
if not full:
128+
return
129+
126130
# Load the RIC diff.
127131
for fname, errname in [
128132
("04_lro_od_truth_error", "OD vs Flown"),

‎src/cosmic/orbitdual.rs

+1-1
Original file line numberDiff line numberDiff line change
@@ -476,7 +476,7 @@ impl OrbitDual {
476476
param: StateParameter::MeanAnomaly,
477477
})
478478
} else if self.ecc()?.real() > 1.0 {
479-
info!("computing the hyperbolic anomaly");
479+
debug!("computing the hyperbolic anomaly");
480480
// From GMAT's TrueToHyperbolicAnomaly
481481
Ok(OrbitPartial {
482482
dual: ((self.ta_deg()?.dual.to_radians().sin()

‎tests/orbit_determination/simulator.rs

+115-92
Original file line numberDiff line numberDiff line change
@@ -36,17 +36,7 @@ fn devices() -> BTreeMap<String, GroundStation> {
3636

3737
let mut devices = BTreeMap::new();
3838
for gs in GroundStation::load_many(ground_station_file).unwrap() {
39-
devices.insert(
40-
gs.name.clone(),
41-
gs.with_msr_type(
42-
MeasurementType::Azimuth,
43-
StochasticNoise::default_angle_deg(),
44-
)
45-
.with_msr_type(
46-
MeasurementType::Elevation,
47-
StochasticNoise::default_angle_deg(),
48-
),
49-
);
39+
devices.insert(gs.name.clone(), gs);
5040
}
5141

5242
devices
@@ -70,21 +60,26 @@ fn spacecraft(almanac: Arc<Almanac>) -> Spacecraft {
7060
orbit.into()
7161
}
7262

63+
#[fixture]
64+
fn trajectory(spacecraft: Spacecraft, almanac: Arc<Almanac>) -> Trajectory {
65+
// Generate a trajectory
66+
let (_, trajectory) = Propagator::default(SpacecraftDynamics::new(OrbitalDynamics::two_body()))
67+
.with(spacecraft, almanac.clone())
68+
.for_duration_with_traj(0.25 * spacecraft.orbit.period().unwrap())
69+
.unwrap();
70+
71+
trajectory
72+
}
73+
7374
#[fixture]
7475
fn tracking_data(
75-
spacecraft: Spacecraft,
76+
trajectory: Trajectory,
7677
devices: BTreeMap<String, GroundStation>,
7778
almanac: Arc<Almanac>,
7879
) -> TrackingDataArc {
7980
// Test that continuous tracking
8081
let _ = pretty_env_logger::try_init();
8182

82-
// Generate a trajectory
83-
let (_, trajectory) = Propagator::default(SpacecraftDynamics::new(OrbitalDynamics::two_body()))
84-
.with(spacecraft, almanac.clone())
85-
.for_duration_with_traj(0.25 * spacecraft.orbit.period().unwrap())
86-
.unwrap();
87-
8883
println!("{trajectory}");
8984

9085
// Save the trajectory to parquet
@@ -222,6 +217,7 @@ fn od_with_modulus_cov_test(
222217
spacecraft: Spacecraft,
223218
tracking_data: TrackingDataArc,
224219
devices: BTreeMap<String, GroundStation>,
220+
trajectory: Trajectory,
225221
almanac: Arc<Almanac>,
226222
) {
227223
let mut arc = tracking_data;
@@ -278,80 +274,107 @@ fn od_with_modulus_cov_test(
278274
)
279275
.unwrap();
280276

281-
// Test the rejection count?
282-
let any_rejected = odp.residuals.iter().any(|resid| resid.unwrap().rejected);
277+
// Check the final error.
278+
let estimate = odp.estimates.last().unwrap();
279+
let rss_pos_km = trajectory
280+
.at(estimate.epoch())
281+
.unwrap()
282+
.orbit
283+
.rss_radius_km(&estimate.orbital_state())
284+
.unwrap();
285+
286+
println!("rss_pos_km = {rss_pos_km}");
283287

284-
assert!(
285-
!any_rejected,
286-
"expected zero rejections with properly configured modulus"
287-
);
288+
let reject_count = odp
289+
.residuals
290+
.iter()
291+
.map(|resid| {
292+
if let Some(resid) = resid {
293+
if resid.rejected {
294+
1
295+
} else {
296+
0
297+
}
298+
} else {
299+
0
300+
}
301+
})
302+
.sum::<u32>();
303+
304+
assert!(reject_count < 10, "wrong number of expected rejections");
288305
}
289306

290-
// #[rstest]
291-
// fn od_with_modulus_as_bias_cov_test(
292-
// spacecraft: Spacecraft,
293-
// tracking_data: TrackingDataArc,
294-
// mut devices: BTreeMap<String, GroundStation>,
295-
// almanac: Arc<Almanac>,
296-
// ) {
297-
// let mut arc = tracking_data;
298-
299-
// // Assume JPL DSN Code is used, cf. DSN docs 214, section 2.2.2.2.
300-
// let jpl_dsn_code_length_km = 75660.0;
301-
// // Set a bias instead of assuming a modulus!
302-
303-
// arc.set_moduli(MeasurementType::Range, jpl_dsn_code_length_km);
304-
// arc.apply_moduli();
305-
306-
// let uncertainty = SpacecraftUncertainty::builder()
307-
// .nominal(spacecraft)
308-
// .frame(LocalFrame::RIC)
309-
// .x_km(0.5)
310-
// .y_km(0.5)
311-
// .z_km(0.5)
312-
// .vx_km_s(0.5e-3)
313-
// .vy_km_s(0.5e-3)
314-
// .vz_km_s(0.5e-3)
315-
// .build();
316-
317-
// assert!((uncertainty.x_km - 0.5).abs() < f64::EPSILON);
318-
// assert!((uncertainty.y_km - 0.5).abs() < f64::EPSILON);
319-
// assert!((uncertainty.z_km - 0.5).abs() < f64::EPSILON);
320-
// assert!((uncertainty.vx_km_s - 0.5e-3).abs() < f64::EPSILON);
321-
// assert!((uncertainty.vy_km_s - 0.5e-3).abs() < f64::EPSILON);
322-
// assert!((uncertainty.vz_km_s - 0.5e-3).abs() < f64::EPSILON);
323-
324-
// println!("{uncertainty}");
325-
326-
// let estimate = uncertainty.to_estimate().unwrap();
327-
328-
// println!("{estimate}");
329-
330-
// let kf = KF::no_snc(estimate);
331-
332-
// let setup = Propagator::default(SpacecraftDynamics::new(OrbitalDynamics::two_body()));
333-
// let prop = setup.with(spacecraft.with_stm(), almanac.clone());
334-
335-
// let mut odp = SpacecraftODProcess::ekf(
336-
// prop,
337-
// kf,
338-
// devices,
339-
// EkfTrigger::new(10, Unit::Minute * 15),
340-
// None,
341-
// almanac,
342-
// );
343-
344-
// odp.process_arc(&arc).unwrap();
345-
346-
// odp.to_parquet(
347-
// &arc,
348-
// "./output_data/od_with_modulus.parquet",
349-
// ExportCfg::default(),
350-
// )
351-
// .unwrap();
352-
353-
// // Test the rejection count?
354-
// let any_rejected = odp.residuals.iter().any(|resid| resid.unwrap().rejected);
355-
356-
// assert!(!any_rejected, "expected zero rejections");
357-
// }
307+
#[rstest]
308+
fn od_with_modulus_as_bias_cov_test(
309+
spacecraft: Spacecraft,
310+
mut tracking_data: TrackingDataArc,
311+
mut devices: BTreeMap<String, GroundStation>,
312+
trajectory: Trajectory,
313+
almanac: Arc<Almanac>,
314+
) {
315+
// Assume JPL DSN Code is used, cf. DSN docs 214, section 2.2.2.2.
316+
let jpl_dsn_code_length_km = 75660.0;
317+
318+
tracking_data.set_moduli(MeasurementType::Range, jpl_dsn_code_length_km);
319+
tracking_data.apply_moduli();
320+
// Forget there ever was a modulus!
321+
tracking_data.moduli = None;
322+
323+
// Set a bias instead of assuming a modulus.
324+
for (name, device) in devices.clone() {
325+
let biased_device = device
326+
.with_msr_bias_constant(MeasurementType::Range, jpl_dsn_code_length_km)
327+
.unwrap();
328+
devices.insert(name, biased_device);
329+
}
330+
331+
let uncertainty = SpacecraftUncertainty::builder()
332+
.nominal(spacecraft)
333+
.frame(LocalFrame::RIC)
334+
.x_km(0.5)
335+
.y_km(0.5)
336+
.z_km(0.5)
337+
.vx_km_s(0.5e-3)
338+
.vy_km_s(0.5e-3)
339+
.vz_km_s(0.5e-3)
340+
.build();
341+
342+
let estimate = uncertainty.to_estimate().unwrap();
343+
344+
let kf = KF::no_snc(estimate);
345+
346+
let setup = Propagator::default(SpacecraftDynamics::new(OrbitalDynamics::two_body()));
347+
let prop = setup.with(spacecraft.with_stm(), almanac.clone());
348+
349+
let mut odp = SpacecraftODProcess::ekf(
350+
prop,
351+
kf,
352+
devices,
353+
EkfTrigger::new(10, Unit::Minute * 15),
354+
None,
355+
almanac,
356+
);
357+
358+
odp.process_arc(&tracking_data).unwrap();
359+
360+
odp.to_parquet(
361+
&tracking_data,
362+
"./output_data/od_with_modulus.parquet",
363+
ExportCfg::default(),
364+
)
365+
.unwrap();
366+
367+
// Check the final error.
368+
let estimate = odp.estimates.last().unwrap();
369+
let rss_pos_km = trajectory
370+
.at(estimate.epoch())
371+
.unwrap()
372+
.orbit
373+
.rss_radius_km(&estimate.orbital_state())
374+
.unwrap();
375+
376+
assert!(
377+
rss_pos_km > 100_000.0,
378+
"expected bias to not correctly solve OD"
379+
)
380+
}

0 commit comments

Comments
 (0)