Merge pull request #234 from jbrodovsky/copilot/fix-rbpf-length-mismatch

jbrodovsky · web-flow · commit ee2542398e89 · 2026-01-19T12:13:57.000-05:00
Fix filter output length mismatch: store initial state before event processing
diff --git a/core/src/sim.rs b/core/src/sim.rs
@@ -1914,14 +1914,20 @@ pub fn run_closed_loop<F: NavigationFilter>(
     let start_time = stream.start_time;
     let mut results: Vec<NavigationResult> = Vec::with_capacity(stream.events.len());
     let total = stream.events.len();
-    let mut last_ts: Option<DateTime<Utc>> = None;
     let mut monitor = HealthMonitor::new(health_limits.unwrap_or_default());
 
     info!(
         "Starting closed-loop navigation filter with {} events",
         total
     );
 
+    // Store the initial state (before processing any events)
+    let mean = filter.get_estimate();
+    let cov = filter.get_certainty();
+    results.push(NavigationResult::from((&start_time, &mean, &cov)));
+    debug!("Initial filter state at {}: {:?}", start_time, mean);
+    let mut last_ts = start_time;
+
     for (i, event) in stream.events.into_iter().enumerate() {
         // Print detailed progress every 100 iterations or at key milestones
         if i % 10 == 0 || i == total {
@@ -1987,23 +1993,13 @@ pub fn run_closed_loop<F: NavigationFilter>(
             }
         }
 
-        // If timestamp changed, or it's the last event, record the previous state
-        if Some(ts) != last_ts {
-            if let Some(prev_ts) = last_ts {
-                let mean = filter.get_estimate();
-                let cov = filter.get_certainty();
-                results.push(NavigationResult::from((&prev_ts, &mean, &cov)));
-                debug!("Filter state at {}: {:?}", ts, mean);
-            }
-            last_ts = Some(ts);
-        }
-
-        // If this is the last event, also push
-        if i == total - 1 {
+        // Store state when timestamp changes
+        if ts != last_ts {
             let mean = filter.get_estimate();
             let cov = filter.get_certainty();
-            debug!("Filter state at {}: {:?}", ts, mean);
             results.push(NavigationResult::from((&ts, &mean, &cov)));
+            debug!("Filter state at {}: {:?}", ts, mean);
+            last_ts = ts;
         }
     }
     debug!("Closed-loop simulation complete");
diff --git a/core/tests/integration_tests.rs b/core/tests/integration_tests.rs
@@ -1918,3 +1918,126 @@ fn test_rbpf_with_degraded_gnss() {
         assert!(result.altitude.is_finite());
     }
 }
+
+/// Test that filter output length matches input data length
+///
+/// This test verifies that all filter implementations (UKF, EKF, ESKF, dead reckoning)
+/// produce output with the same number of records as the input data. This is critical for
+/// downstream analysis tools that expect aligned data streams.
+#[test]
+fn test_filter_output_length_matches_input() {
+    let manifest_dir = env!("CARGO_MANIFEST_DIR");
+    let test_data_path = Path::new(manifest_dir).join("tests/test_data.csv");
+    let records = load_test_data(&test_data_path);
+
+    assert!(
+        !records.is_empty(),
+        "Test data should contain at least one record"
+    );
+
+    let input_length = records.len();
+    println!("Testing with {} input records", input_length);
+
+    // Test dead reckoning
+    let dr_results = dead_reckoning(&records);
+    assert_eq!(
+        dr_results.len(),
+        input_length,
+        "Dead reckoning output length {} should match input length {}",
+        dr_results.len(),
+        input_length
+    );
+    println!(
+        "✓ Dead reckoning: {} outputs for {} inputs",
+        dr_results.len(),
+        input_length
+    );
+
+    // Create initial state from first record
+    let initial_state = create_initial_state(&records[0]);
+    let imu_biases = vec![0.0; 6]; // Zero initial bias estimates
+    let initial_covariance = DEFAULT_INITIAL_COVARIANCE.to_vec();
+    let process_noise = DMatrix::from_diagonal(&DVector::from_vec(DEFAULT_PROCESS_NOISE.to_vec()));
+    let degradation = GnssDegradationConfig::default();
+
+    // Test UKF
+    let mut ukf = UnscentedKalmanFilter::new(
+        initial_state.clone(),
+        imu_biases.clone(),
+        None, // No measurement bias
+        initial_covariance.clone(),
+        process_noise.clone(),
+        1e-3, // alpha
+        2.0,  // beta
+        0.0,  // kappa
+    );
+
+    let event_stream = build_event_stream(&records, &degradation);
+    let ukf_results = run_closed_loop(&mut ukf, event_stream, None)
+        .expect("UKF closed loop should complete successfully");
+
+    assert_eq!(
+        ukf_results.len(),
+        input_length,
+        "UKF output length {} should match input length {}",
+        ukf_results.len(),
+        input_length
+    );
+    println!(
+        "✓ UKF: {} outputs for {} inputs",
+        ukf_results.len(),
+        input_length
+    );
+
+    // Test EKF
+    let mut ekf = ExtendedKalmanFilter::new(
+        initial_state.clone(),
+        imu_biases.clone(),
+        initial_covariance.clone(),
+        process_noise.clone(),
+        true,
+    );
+
+    let event_stream = build_event_stream(&records, &degradation);
+    let ekf_results = run_closed_loop(&mut ekf, event_stream, None)
+        .expect("EKF closed loop should complete successfully");
+
+    assert_eq!(
+        ekf_results.len(),
+        input_length,
+        "EKF output length {} should match input length {}",
+        ekf_results.len(),
+        input_length
+    );
+    println!(
+        "✓ EKF: {} outputs for {} inputs",
+        ekf_results.len(),
+        input_length
+    );
+
+    // Test ESKF
+    let mut eskf =
+        ErrorStateKalmanFilter::new(initial_state, imu_biases, initial_covariance, process_noise);
+
+    let event_stream = build_event_stream(&records, &degradation);
+    let eskf_results = run_closed_loop(&mut eskf, event_stream, None)
+        .expect("ESKF closed loop should complete successfully");
+
+    assert_eq!(
+        eskf_results.len(),
+        input_length,
+        "ESKF output length {} should match input length {}",
+        eskf_results.len(),
+        input_length
+    );
+    println!(
+        "✓ ESKF: {} outputs for {} inputs",
+        eskf_results.len(),
+        input_length
+    );
+
+    println!(
+        "\n✅ All filters produce output length matching input length: {}",
+        input_length
+    );
+}
diff --git a/sim/src/main.rs b/sim/src/main.rs
@@ -833,9 +833,17 @@ fn process_file(
 
             let start_time = event_stream.start_time;
             let mut results = Vec::with_capacity(event_stream.events.len());
-            let mut last_ts: Option<chrono::DateTime<chrono::Utc>> = None;
             let mut monitor = HealthMonitor::new(HealthLimits::default());
 
+            // Store the initial state (before processing any events)
+            let (mean, cov) = rbpf.estimate();
+            results.push(NavigationResult::from_particle_filter(
+                &start_time,
+                &mean,
+                &cov,
+            ));
+            let mut last_ts = start_time;
+
             for event in event_stream.events.into_iter() {
                 let elapsed_s = match &event {
                     Event::Imu { elapsed_s, .. } => *elapsed_s,
@@ -874,13 +882,10 @@ fn process_file(
                     return Err(e.into());
                 }
 
-                if Some(ts) != last_ts {
-                    if let Some(prev_ts) = last_ts {
-                        results.push(NavigationResult::from_particle_filter(
-                            &prev_ts, &mean, &cov,
-                        ));
-                    }
-                    last_ts = Some(ts);
+                // Store state when timestamp changes
+                if ts != last_ts {
+                    results.push(NavigationResult::from_particle_filter(&ts, &mean, &cov));
+                    last_ts = ts;
                 }
             }
 
@@ -1714,9 +1719,17 @@ fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box<dyn Error
 
         let start_time = event_stream.start_time;
         let mut results = Vec::with_capacity(event_stream.events.len());
-        let mut last_ts: Option<chrono::DateTime<chrono::Utc>> = None;
         let mut monitor = HealthMonitor::new(HealthLimits::default());
 
+        // Store the initial state (before processing any events)
+        let (mean, cov) = rbpf.estimate();
+        results.push(NavigationResult::from_particle_filter(
+            &start_time,
+            &mean,
+            &cov,
+        ));
+        let mut last_ts = start_time;
+
         for event in event_stream.events.into_iter() {
             let elapsed_s = match &event {
                 Event::Imu { elapsed_s, .. } => *elapsed_s,
@@ -1755,13 +1768,10 @@ fn run_particle_filter(args: &ParticleFilterSimArgs) -> Result<(), Box<dyn Error
                 return Err(e.into());
             }
 
-            if Some(ts) != last_ts {
-                if let Some(prev_ts) = last_ts {
-                    results.push(NavigationResult::from_particle_filter(
-                        &prev_ts, &mean, &cov,
-                    ));
-                }
-                last_ts = Some(ts);
+            // Store state when timestamp changes
+            if ts != last_ts {
+                results.push(NavigationResult::from_particle_filter(&ts, &mean, &cov));
+                last_ts = ts;
             }
         }
 
