nabled_sim/
estimation.rs

1//! EKF pipeline and innovation monitoring (no control crate dependency).
2
3use nabled_core::scalar::NabledReal;
4use nabled_linalg::lu::LuProviderScalar;
5use nabled_ml::stats::rolling::rolling_covariance;
6use nabled_sensor::ekf::{EkConfig, EkModel, ekf_predict, ekf_update};
7use nabled_sensor::kalman::KalmanState;
8use ndarray::{Array1, Array2, ArrayView1};
9
10use crate::SimError;
11
12/// Extended Kalman filter orchestration wrapper.
13#[derive(Clone)]
14pub struct EstimationPipeline<T> {
15    pub model:  EkModel<T>,
16    pub config: EkConfig<T>,
17    pub state:  KalmanState<T>,
18}
19
20impl<T: NabledReal + LuProviderScalar> EstimationPipeline<T> {
21    #[must_use]
22    pub fn new(model: EkModel<T>, config: EkConfig<T>, state: KalmanState<T>) -> Self {
23        Self { model, config, state }
24    }
25
26    /// Run predict step in place.
27    pub fn predict(&mut self) -> Result<(), SimError> {
28        self.state = ekf_predict(&self.state, &self.model, &self.config)?;
29        Ok(())
30    }
31
32    /// Run update step in place.
33    pub fn update(&mut self, measurement: &ArrayView1<'_, T>) -> Result<(), SimError> {
34        let predicted = self.state.clone();
35        self.state = ekf_update(&predicted, measurement, &self.model, &self.config)?;
36        Ok(())
37    }
38
39    /// Predict then update in one call.
40    pub fn predict_update(&mut self, measurement: &ArrayView1<'_, T>) -> Result<(), SimError> {
41        self.predict()?;
42        self.update(measurement)
43    }
44}
45
46/// Track innovation vectors and expose rolling covariance (S20 pattern).
47#[derive(Debug, Clone, PartialEq)]
48pub struct InnovationMonitor<T> {
49    pub window:  usize,
50    innovations: Array2<T>,
51    count:       usize,
52}
53
54impl<T: NabledReal + Default> InnovationMonitor<T> {
55    pub fn new(window: usize, innovation_dim: usize) -> Result<Self, SimError> {
56        if window == 0 {
57            return Err(SimError::InvalidInput("window must be positive".to_string()));
58        }
59        Ok(Self { window, innovations: Array2::zeros((0, innovation_dim)), count: 0 })
60    }
61
62    /// Append one innovation row.
63    pub fn push(&mut self, innovation: &Array1<T>) {
64        let mut row = self.innovations.clone();
65        if row.nrows() == 0 {
66            row = Array2::zeros((1, innovation.len()));
67            row.row_mut(0).assign(innovation);
68        } else if row.ncols() != innovation.len() {
69            return;
70        } else {
71            let mut stacked = Array2::zeros((row.nrows() + 1, innovation.len()));
72            stacked.slice_mut(ndarray::s![0..row.nrows(), ..]).assign(&row);
73            stacked.row_mut(row.nrows()).assign(innovation);
74            row = stacked;
75        }
76        self.innovations = row;
77        self.count += 1;
78    }
79
80    /// Number of stored innovations.
81    #[must_use]
82    pub fn len(&self) -> usize { self.count }
83
84    #[must_use]
85    pub fn is_empty(&self) -> bool { self.count == 0 }
86
87    /// Rolling covariance over stored innovations (delegates to `nabled-ml::stats`).
88    #[must_use]
89    pub fn rolling_covariance(&self) -> Array2<T> {
90        if self.innovations.nrows() == 0 {
91            return Array2::zeros((0, 0));
92        }
93        rolling_covariance(&self.innovations.view(), self.window)
94    }
95}
96
97#[cfg(test)]
98mod tests {
99    use ndarray::{Array2, arr1, arr2};
100
101    use super::*;
102
103    #[test]
104    fn innovation_monitor_tracks_rolling_covariance() {
105        let mut monitor = InnovationMonitor::new(3, 2).expect("monitor");
106        for row in [[0.1, -0.2], [0.0, 0.1], [-0.1, 0.0], [0.2, 0.1]] {
107            monitor.push(&arr1(&row));
108        }
109        let cov: Array2<f64> = monitor.rolling_covariance();
110        assert_eq!(cov.nrows(), 4);
111        assert!(cov[[3, 3]].is_finite());
112        assert!(cov[[3, 0]].is_finite());
113    }
114
115    #[test]
116    fn estimation_pipeline_predict_update() {
117        let model = EkModel {
118            predict_state:    |x: &ArrayView1<'_, f64>| arr1(&[x[0].cos()]),
119            predict_jacobian: |x: &ArrayView1<'_, f64>| arr2(&[[-x[0].sin()]]),
120            measure:          |x: &ArrayView1<'_, f64>| arr1(&[x[0]]),
121            measure_jacobian: |_: &ArrayView1<'_, f64>| arr2(&[[1.0]]),
122        };
123        let config =
124            EkConfig { process_noise: arr2(&[[0.01]]), measurement_noise: arr2(&[[0.05]]) };
125        let mut pipeline = EstimationPipeline::new(model, config, KalmanState {
126            mean:       arr1(&[0.2]),
127            covariance: arr2(&[[1.0]]),
128        });
129        pipeline.predict_update(&arr1(&[0.9]).view()).expect("predict-update");
130        assert!(pipeline.state.mean[0] > 0.2);
131    }
132}
nabled_sim/estimation.rs

nabled_sim/
estimation.rs
