Skip to content

Commit d1e3e37

Browse files
Copilotjbrodovsky
andcommitted
Add integration test to verify filter output length matches input
- Created comprehensive test verifying UKF, EKF, ESKF, and dead reckoning - Test confirms all filters produce output matching input data length (5366 records) - Critical for downstream analysis tools that expect aligned data streams Co-authored-by: jbrodovsky <57160841+jbrodovsky@users.noreply.github.com>
1 parent f125609 commit d1e3e37

1 file changed

Lines changed: 109 additions & 0 deletions

File tree

core/tests/integration_tests.rs

Lines changed: 109 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1918,3 +1918,112 @@ fn test_rbpf_with_degraded_gnss() {
19181918
assert!(result.altitude.is_finite());
19191919
}
19201920
}
1921+
1922+
1923+
/// Test that filter output length matches input data length
1924+
///
1925+
/// This test verifies that all filter implementations (UKF, EKF, ESKF, dead reckoning)
1926+
/// produce output with the same number of records as the input data. This is critical for
1927+
/// downstream analysis tools that expect aligned data streams.
1928+
#[test]
1929+
fn test_filter_output_length_matches_input() {
1930+
let manifest_dir = env!("CARGO_MANIFEST_DIR");
1931+
let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
1932+
let records = load_test_data(&test_data_path);
1933+
1934+
assert!(
1935+
!records.is_empty(),
1936+
"Test data should contain at least one record"
1937+
);
1938+
1939+
let input_length = records.len();
1940+
println!("Testing with {} input records", input_length);
1941+
1942+
// Test dead reckoning
1943+
let dr_results = dead_reckoning(&records);
1944+
assert_eq!(
1945+
dr_results.len(),
1946+
input_length,
1947+
"Dead reckoning output length {} should match input length {}",
1948+
dr_results.len(),
1949+
input_length
1950+
);
1951+
println!("✓ Dead reckoning: {} outputs for {} inputs", dr_results.len(), input_length);
1952+
1953+
// Create initial state from first record
1954+
let initial_state = create_initial_state(&records[0]);
1955+
let imu_biases = vec![0.0; 6]; // Zero initial bias estimates
1956+
let initial_covariance = DEFAULT_INITIAL_COVARIANCE.to_vec();
1957+
let process_noise = DMatrix::from_diagonal(&DVector::from_vec(DEFAULT_PROCESS_NOISE.to_vec()));
1958+
let degradation = GnssDegradationConfig::default();
1959+
1960+
// Test UKF
1961+
let mut ukf = UnscentedKalmanFilter::new(
1962+
initial_state.clone(),
1963+
imu_biases.clone(),
1964+
None, // No measurement bias
1965+
initial_covariance.clone(),
1966+
process_noise.clone(),
1967+
1e-3, // alpha
1968+
2.0, // beta
1969+
0.0, // kappa
1970+
);
1971+
1972+
let event_stream = build_event_stream(&records, &degradation);
1973+
let ukf_results = run_closed_loop(&mut ukf, event_stream, None)
1974+
.expect("UKF closed loop should complete successfully");
1975+
1976+
assert_eq!(
1977+
ukf_results.len(),
1978+
input_length,
1979+
"UKF output length {} should match input length {}",
1980+
ukf_results.len(),
1981+
input_length
1982+
);
1983+
println!("✓ UKF: {} outputs for {} inputs", ukf_results.len(), input_length);
1984+
1985+
// Test EKF
1986+
let mut ekf = ExtendedKalmanFilter::new(
1987+
initial_state.clone(),
1988+
imu_biases.clone(),
1989+
initial_covariance.clone(),
1990+
process_noise.clone(),
1991+
true,
1992+
);
1993+
1994+
let event_stream = build_event_stream(&records, &degradation);
1995+
let ekf_results = run_closed_loop(&mut ekf, event_stream, None)
1996+
.expect("EKF closed loop should complete successfully");
1997+
1998+
assert_eq!(
1999+
ekf_results.len(),
2000+
input_length,
2001+
"EKF output length {} should match input length {}",
2002+
ekf_results.len(),
2003+
input_length
2004+
);
2005+
println!("✓ EKF: {} outputs for {} inputs", ekf_results.len(), input_length);
2006+
2007+
// Test ESKF
2008+
let mut eskf = ErrorStateKalmanFilter::new(
2009+
initial_state,
2010+
imu_biases,
2011+
initial_covariance,
2012+
process_noise,
2013+
);
2014+
2015+
let event_stream = build_event_stream(&records, &degradation);
2016+
let eskf_results = run_closed_loop(&mut eskf, event_stream, None)
2017+
.expect("ESKF closed loop should complete successfully");
2018+
2019+
assert_eq!(
2020+
eskf_results.len(),
2021+
input_length,
2022+
"ESKF output length {} should match input length {}",
2023+
eskf_results.len(),
2024+
input_length
2025+
);
2026+
println!("✓ ESKF: {} outputs for {} inputs", eskf_results.len(), input_length);
2027+
2028+
println!("\n✅ All filters produce output length matching input length: {}", input_length);
2029+
}

0 commit comments

Comments
 (0)