@@ -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