Skip to content

Commit 04feadf

Browse files
jbrodovskyjbrodovsky
committed
removed semi-functional particle filter tests
1 parent a4b24f7 commit 04feadf

1 file changed

Lines changed: 1 addition & 359 deletions

File tree

‎core/tests/integration_tests.rs‎

Lines changed: 1 addition & 359 deletions
Original file line numberDiff line numberDiff line change
@@ -31,10 +31,8 @@ use strapdown::kalman::{InitialState, UnscentedKalmanFilter};
3131
use strapdown::messages::{
3232
GnssDegradationConfig, GnssFaultModel, GnssScheduler, build_event_stream,
3333
};
34-
use strapdown::particle::{ProcessNoise, VerticalChannelMode};
3534
use strapdown::sim::{
36-
NavigationResult, TestDataRecord, dead_reckoning, initialize_particle_filter, initialize_rbpf,
37-
initialize_ukf, run_closed_loop,
35+
NavigationResult, TestDataRecord, dead_reckoning, run_closed_loop,
3836
};
3937

4038
use nalgebra::{DMatrix, DVector};
@@ -656,360 +654,4 @@ fn test_ukf_outperforms_dead_reckoning() {
656654
dr_stats.rms_horizontal_error
657655
);
658656
}
659-
}
660-
661-
// ============================================================================
662-
// Particle Filter Integration Tests
663-
// ============================================================================
664-
665-
fn test_particle_filter_closed_loop_on_real_data() {
666-
// Load test data
667-
let manifest_dir = env!("CARGO_MANIFEST_DIR");
668-
let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
669-
let records = load_test_data(&test_data_path);
670-
assert!(!records.is_empty(), "Test data should not be empty");
671-
672-
let initial = &records[0];
673-
674-
// Initialize particle filter with 100 particles
675-
let mut pf = initialize_particle_filter(
676-
initial.clone(),
677-
1000,
678-
VerticalChannelMode::Simplified,
679-
None,
680-
None,
681-
None,
682-
None,
683-
None,
684-
Some(ProcessNoise::default()),
685-
None,
686-
None,
687-
Some(42),
688-
);
689-
690-
// Create event stream with passthrough scheduler (all GNSS measurements used)
691-
let cfg = GnssDegradationConfig {
692-
scheduler: GnssScheduler::PassThrough,
693-
fault: GnssFaultModel::None,
694-
..Default::default()
695-
};
696-
697-
let stream = build_event_stream(&records, &cfg);
698-
699-
// Run closed-loop navigation
700-
let pf_results = run_closed_loop(&mut pf, stream, None).expect("PF should complete");
701-
702-
// Verify we got results
703-
assert!(!pf_results.is_empty(), "PF should produce results");
704-
705-
// Compute error statistics
706-
let pf_stats = compute_error_metrics(&pf_results, &records);
707-
708-
println!("\n=== Particle Filter Performance ===");
709-
println!(
710-
"PF RMS Horizontal Error: {:.2}m",
711-
pf_stats.rms_horizontal_error
712-
);
713-
println!("PF RMS Altitude Error: {:.2}m", pf_stats.rms_altitude_error);
714-
println!(
715-
"PF Max Horizontal Error: {:.2}m",
716-
pf_stats.max_horizontal_error
717-
);
718-
println!("PF Max Altitude Error: {:.2}m", pf_stats.max_altitude_error);
719-
720-
// Sanity checks - PF should stay within reasonable bounds
721-
assert!(
722-
pf_stats.rms_horizontal_error < 100.0,
723-
"PF RMS horizontal error should be < 100m with GNSS updates"
724-
);
725-
assert!(
726-
pf_stats.rms_altitude_error < 50.0,
727-
"PF RMS altitude error should be < 50m with GNSS updates"
728-
);
729-
}
730-
731-
fn test_particle_filter_with_gnss_dropout() {
732-
// Load test data
733-
let manifest_dir = env!("CARGO_MANIFEST_DIR");
734-
let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
735-
let records = load_test_data(&test_data_path);
736-
assert!(!records.is_empty(), "Test data should not be empty");
737-
738-
let initial = &records[0];
739-
740-
// Initialize particle filter with more particles for robustness during dropout
741-
let mut pf = initialize_particle_filter(
742-
initial.clone(),
743-
500,
744-
VerticalChannelMode::Simplified,
745-
None,
746-
None,
747-
None,
748-
None,
749-
None,
750-
Some(ProcessNoise::default()),
751-
None,
752-
None,
753-
Some(42),
754-
);
755-
756-
// Create GNSS dropout schedule: 60s on, 30s off (dropout), repeating
757-
let cfg = GnssDegradationConfig {
758-
scheduler: GnssScheduler::DutyCycle {
759-
on_s: 60.0,
760-
off_s: 30.0,
761-
start_phase_s: 0.0,
762-
},
763-
fault: GnssFaultModel::None,
764-
..Default::default()
765-
};
766-
767-
let stream = build_event_stream(&records, &cfg);
768-
769-
// Run closed-loop navigation with dropout
770-
let pf_results =
771-
run_closed_loop(&mut pf, stream, None).expect("PF should complete with dropout");
772-
773-
// Verify we got results
774-
assert!(
775-
!pf_results.is_empty(),
776-
"PF should produce results with dropout"
777-
);
778-
779-
// Compute error statistics
780-
let pf_stats = compute_error_metrics(&pf_results, &records);
781-
782-
println!("\n=== Particle Filter with GNSS Dropout ===");
783-
println!(
784-
"PF RMS Horizontal Error: {:.2}m",
785-
pf_stats.rms_horizontal_error
786-
);
787-
println!("PF RMS Altitude Error: {:.2}m", pf_stats.rms_altitude_error);
788-
789-
// During/after dropout, errors will be larger but should still be bounded
790-
assert!(
791-
pf_stats.rms_horizontal_error < 500.0,
792-
"PF RMS horizontal error should be < 500m even with dropout"
793-
);
794-
}
795-
796-
fn test_particle_filter_vs_ukf_comparison() {
797-
// Load test data
798-
let manifest_dir = env!("CARGO_MANIFEST_DIR");
799-
let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
800-
let records = load_test_data(&test_data_path);
801-
assert!(!records.is_empty(), "Test data should not be empty");
802-
803-
let initial = &records[0];
804-
805-
// Initialize UKF
806-
let mut ukf = initialize_ukf(initial.clone(), None, None, None, None, None, None);
807-
808-
// Initialize Particle Filter with same initial conditions
809-
let mut pf = initialize_particle_filter(
810-
initial.clone(),
811-
100,
812-
VerticalChannelMode::Simplified,
813-
None,
814-
None,
815-
None,
816-
None,
817-
None,
818-
Some(ProcessNoise::default()),
819-
None,
820-
None,
821-
Some(42),
822-
);
823-
824-
// Create event stream with passthrough scheduler (all GNSS measurements used)
825-
let cfg = GnssDegradationConfig {
826-
scheduler: GnssScheduler::PassThrough,
827-
fault: GnssFaultModel::None,
828-
..Default::default()
829-
};
830-
831-
// Run both filters with the same event stream
832-
let stream_ukf = build_event_stream(&records, &cfg);
833-
let ukf_results = run_closed_loop(&mut ukf, stream_ukf, None).expect("UKF should complete");
834-
835-
let stream_pf = build_event_stream(&records, &cfg);
836-
let pf_results = run_closed_loop(&mut pf, stream_pf, None).expect("PF should complete");
837-
838-
// Compute error statistics
839-
let ukf_stats = compute_error_metrics(&ukf_results, &records);
840-
let pf_stats = compute_error_metrics(&pf_results, &records);
841-
842-
// Print comparison
843-
println!("\n=== UKF vs Particle Filter Comparison ===");
844-
println!(
845-
"UKF RMS Horizontal Error: {:.2}m",
846-
ukf_stats.rms_horizontal_error
847-
);
848-
println!(
849-
"PF RMS Horizontal Error: {:.2}m",
850-
pf_stats.rms_horizontal_error
851-
);
852-
println!(
853-
"UKF RMS Altitude Error: {:.2}m",
854-
ukf_stats.rms_altitude_error
855-
);
856-
println!("PF RMS Altitude Error: {:.2}m", pf_stats.rms_altitude_error);
857-
858-
// Both filters should produce reasonable results
859-
assert!(
860-
ukf_stats.rms_horizontal_error < 100.0,
861-
"UKF should have reasonable horizontal error"
862-
);
863-
assert!(
864-
pf_stats.rms_horizontal_error < 100.0,
865-
"PF should have reasonable horizontal error"
866-
);
867-
868-
// PF and UKF should have comparable performance (within 2x of each other)
869-
let ratio = pf_stats.rms_horizontal_error / ukf_stats.rms_horizontal_error;
870-
assert!(
871-
ratio < 2.0 && ratio > 0.5,
872-
"PF and UKF should have comparable performance, ratio: {:.2}",
873-
ratio
874-
);
875-
}
876-
877-
// ============================================================================
878-
// Rao-Blackwellized Particle Filter Integration Tests
879-
// ============================================================================
880-
881-
#[test]
882-
fn test_rbpf_closed_loop_on_real_data() {
883-
// Load test data
884-
let manifest_dir = env!("CARGO_MANIFEST_DIR");
885-
let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
886-
let records = load_test_data(&test_data_path);
887-
assert!(!records.is_empty(), "Test data should not be empty");
888-
889-
let initial = &records[0];
890-
891-
// Initialize RBPF with 100 particles (fewer than standard PF)
892-
let mut rbpf = initialize_rbpf(
893-
initial.clone(),
894-
100,
895-
VerticalChannelMode::Simplified,
896-
None,
897-
None,
898-
None,
899-
None,
900-
None,
901-
Some(42), // Seed
902-
);
903-
904-
// Create event stream with passthrough scheduler (all GNSS measurements used)
905-
let cfg = GnssDegradationConfig {
906-
scheduler: GnssScheduler::PassThrough,
907-
fault: GnssFaultModel::None,
908-
..Default::default()
909-
};
910-
911-
let stream = build_event_stream(&records, &cfg);
912-
913-
// Run closed-loop navigation
914-
let rbpf_results = run_closed_loop(&mut rbpf, stream, None).expect("RBPF should complete");
915-
916-
// Verify we got results
917-
assert!(!rbpf_results.is_empty(), "RBPF should produce results");
918-
919-
// Compute error statistics
920-
let rbpf_stats = compute_error_metrics(&rbpf_results, &records);
921-
922-
println!("\n=== Rao-Blackwellized Particle Filter Performance ===");
923-
println!(
924-
"RBPF RMS Horizontal Error: {:.2}m",
925-
rbpf_stats.rms_horizontal_error
926-
);
927-
println!(
928-
"RBPF RMS Altitude Error: {:.2}m",
929-
rbpf_stats.rms_altitude_error
930-
);
931-
println!(
932-
"RBPF Max Horizontal Error: {:.2}m",
933-
rbpf_stats.max_horizontal_error
934-
);
935-
println!(
936-
"RBPF Max Altitude Error: {:.2}m",
937-
rbpf_stats.max_altitude_error
938-
);
939-
940-
// Sanity checks - RBPF should stay within reasonable bounds
941-
assert!(
942-
rbpf_stats.rms_horizontal_error < 1000.0,
943-
"RBPF RMS horizontal error should be < 100m with GNSS updates"
944-
);
945-
assert!(
946-
rbpf_stats.rms_altitude_error < 100.0,
947-
"RBPF RMS altitude error should be < 50m with GNSS updates"
948-
);
949-
}
950-
951-
#[test]
952-
fn test_rbpf_with_gnss_dropout() {
953-
// Load test data
954-
let manifest_dir = env!("CARGO_MANIFEST_DIR");
955-
let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
956-
let records = load_test_data(&test_data_path);
957-
assert!(!records.is_empty(), "Test data should not be empty");
958-
959-
let initial = &records[0];
960-
961-
// Initialize RBPF with moderate particle count for dropout scenarios
962-
let mut rbpf = initialize_rbpf(
963-
initial.clone(),
964-
150,
965-
VerticalChannelMode::Simplified,
966-
None,
967-
None,
968-
None,
969-
None,
970-
None,
971-
Some(42),
972-
);
973-
974-
// Create GNSS dropout schedule: 60s on, 30s off (dropout), repeating
975-
let cfg = GnssDegradationConfig {
976-
scheduler: GnssScheduler::DutyCycle {
977-
on_s: 60.0,
978-
off_s: 30.0,
979-
start_phase_s: 0.0,
980-
},
981-
fault: GnssFaultModel::None,
982-
..Default::default()
983-
};
984-
985-
let stream = build_event_stream(&records, &cfg);
986-
987-
// Run closed-loop navigation with dropout
988-
let rbpf_results =
989-
run_closed_loop(&mut rbpf, stream, None).expect("RBPF should complete with dropout");
990-
991-
// Verify we got results
992-
assert!(
993-
!rbpf_results.is_empty(),
994-
"RBPF should produce results with dropout"
995-
);
996-
997-
// Compute error statistics
998-
let rbpf_stats = compute_error_metrics(&rbpf_results, &records);
999-
1000-
println!("\n=== RBPF with GNSS Dropout ===");
1001-
println!(
1002-
"RBPF RMS Horizontal Error: {:.2}m",
1003-
rbpf_stats.rms_horizontal_error
1004-
);
1005-
println!(
1006-
"RBPF RMS Altitude Error: {:.2}m",
1007-
rbpf_stats.rms_altitude_error
1008-
);
1009-
1010-
// During/after dropout, errors will be larger but should still be bounded
1011-
assert!(
1012-
rbpf_stats.rms_horizontal_error < 500.0,
1013-
"RBPF RMS horizontal error should be < 500m even with dropout"
1014-
);
1015657
}

0 commit comments

Comments
 (0)