1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
//! Adaptive Kalman Filter with online Q/R estimation from innovation statistics.
//!
//! The adaptive KF monitors the innovation covariance and adjusts R online
//! using an exponential moving average. This improves tracking when the
//! true measurement noise changes over time.
//!
//! Reference: Mehra (1970) "On the Identification of Variances and Adaptive Kalman Filtering"
#![allow(clippy::needless_range_loop)]
use crate::core::matrix::{matmul, Matrix};
use crate::core::scalar::ControlScalar;
/// Adaptive Kalman Filter: tunes R online from innovation covariance.
///
/// N: state dim, I: input dim, M: measurement dim.
pub struct AdaptiveKalmanFilter<S: ControlScalar, const N: usize, const I: usize, const M: usize> {
/// State transition matrix (N×N).
pub a: Matrix<S, N, N>,
/// Input matrix (N×I).
pub b: Matrix<S, N, I>,
/// Measurement matrix (M×N).
pub c: Matrix<S, M, N>,
/// Process noise covariance (N×N).
pub q: Matrix<S, N, N>,
/// Measurement noise covariance (M×M) — adapted online.
pub r: Matrix<S, M, M>,
/// Error covariance (N×N).
pub p: Matrix<S, N, N>,
/// State estimate (N×1).
pub x_hat: Matrix<S, N, 1>,
/// Forgetting factor α ∈ (0, 1): smaller = faster adaptation.
pub alpha: S,
/// Maximum adaptation step to prevent divergence.
pub max_adapt: S,
/// Running innovation covariance estimate S_hat (M×M).
s_hat: Matrix<S, M, M>,
}
impl<S: ControlScalar, const N: usize, const I: usize, const M: usize>
AdaptiveKalmanFilter<S, N, I, M>
{
/// Create a new Adaptive Kalman Filter.
///
/// `alpha` is the forgetting factor (0 < α < 1, typically 0.05–0.2).
/// `max_adapt` clamps element-wise adaptation to prevent divergence.
pub fn new(
a: Matrix<S, N, N>,
b: Matrix<S, N, I>,
c: Matrix<S, M, N>,
q: Matrix<S, N, N>,
r: Matrix<S, M, M>,
alpha: S,
) -> Self {
Self {
a,
b,
c,
q,
r,
p: Matrix::<S, N, N>::identity(),
x_hat: Matrix::<S, N, 1>::zeros(),
alpha,
max_adapt: S::from_f64(10.0),
s_hat: Matrix::<S, M, M>::identity(),
}
}
/// Predict step: propagate state and covariance.
pub fn predict(&mut self, u: &Matrix<S, I, 1>) {
// x_hat = A * x_hat + B * u
let ax = matmul(&self.a, &self.x_hat);
let bu = matmul(&self.b, u);
self.x_hat = ax.add_mat(&bu);
// P = A * P * A^T + Q
let ap = matmul(&self.a, &self.p);
let at = self.a.transpose();
let apat = matmul(&ap, &at);
self.p = apat.add_mat(&self.q);
}
/// Update step: incorporate measurement y and adapt R.
pub fn update(&mut self, y: &Matrix<S, M, 1>) {
let ct = self.c.transpose();
// Innovation covariance: S = C * P * C^T + R
let cp = matmul(&self.c, &self.p);
let cpct = matmul(&cp, &ct);
let s_mat = cpct.add_mat(&self.r);
let s_inv = match s_mat.inv() {
Some(inv) => inv,
None => return,
};
// Kalman gain: K = P * C^T * S^{-1}
let pct = matmul(&self.p, &ct);
let k = matmul(&pct, &s_inv);
// Innovation: e = y - C * x_hat
let cx = matmul(&self.c, &self.x_hat);
let innov = y.sub_mat(&cx);
// Adapt R from innovation statistics before state update
self.adapt_r(&innov, &s_mat);
// State update: x_hat += K * innov
let k_innov = matmul(&k, &innov);
self.x_hat = self.x_hat.add_mat(&k_innov);
// Covariance update: P = (I - K*C) * P (Joseph form)
let kc = matmul(&k, &self.c);
let eye = Matrix::<S, N, N>::identity();
let i_minus_kc = eye.sub_mat(&kc);
self.p = matmul(&i_minus_kc, &self.p);
}
/// Adapt R from innovation statistics using exponential moving average.
///
/// S_hat = (1 - α) * S_hat + α * (innov * innov^T)
/// R_new = S_hat - C * P * C^T (clamped to remain positive)
fn adapt_r(&mut self, innovation: &Matrix<S, M, 1>, s: &Matrix<S, M, M>) {
// Update running innovation covariance estimate
let innov_t = innovation.transpose(); // 1×M
let innov_outer = matmul(innovation, &innov_t); // M×M
let one_minus_alpha = S::ONE - self.alpha;
self.s_hat = self
.s_hat
.scale(one_minus_alpha)
.add_mat(&innov_outer.scale(self.alpha));
// Estimate R = S_hat - C*P*C^T, clamped so diagonal stays positive
let ct = self.c.transpose();
let cp = matmul(&self.c, &self.p);
let cpct = matmul(&cp, &ct);
let r_candidate = self.s_hat.sub_mat(&cpct);
// Apply update only to diagonal elements; clamp to [ε, max_adapt * r_diag]
let mut r_new = self.r;
for i in 0..M {
let candidate = r_candidate.data[i][i];
let current = self.r.data[i][i];
// Clamp: keep R positive and bounded relative to current value
let lo = current * S::from_f64(0.01);
let hi = current * self.max_adapt;
r_new.data[i][i] = candidate.clamp_val(lo, hi);
}
self.r = r_new;
// Unused s reference (kept for API compatibility)
let _ = s;
}
/// State estimate accessor.
pub fn state(&self) -> &Matrix<S, N, 1> {
&self.x_hat
}
/// Reset state and covariance.
pub fn reset(&mut self) {
self.x_hat = Matrix::<S, N, 1>::zeros();
self.p = Matrix::<S, N, N>::identity();
self.s_hat = Matrix::<S, M, M>::identity();
}
}
#[cfg(test)]
mod tests {
use super::*;
fn make_filter() -> AdaptiveKalmanFilter<f64, 2, 1, 1> {
// Position-velocity model with position measurement
let mut a = Matrix::<f64, 2, 2>::identity();
a.data[0][1] = 0.1; // dt = 0.1
let mut b = Matrix::<f64, 2, 1>::zeros();
b.data[1][0] = 0.1;
let mut c = Matrix::<f64, 1, 2>::zeros();
c.data[0][0] = 1.0;
let q = Matrix::<f64, 2, 2>::identity().scale(0.001);
let r = Matrix::<f64, 1, 1> { data: [[0.1]] };
AdaptiveKalmanFilter::new(a, b, c, q, r, 0.05)
}
#[test]
fn tracks_constant_position() {
let mut f = make_filter();
let u = Matrix::<f64, 1, 1> { data: [[0.0]] };
let y = Matrix::<f64, 1, 1> { data: [[5.0]] };
for _ in 0..100 {
f.predict(&u);
f.update(&y);
}
let pos = f.state().data[0][0];
assert!((pos - 5.0).abs() < 0.05, "pos={pos}");
}
#[test]
fn r_adapts_under_high_noise() {
let mut f = make_filter();
let _r_initial = f.r.data[0][0];
let u = Matrix::<f64, 1, 1> { data: [[0.0]] };
// Simulate large innovation variance by alternating ±10
for k in 0..50 {
f.predict(&u);
let sign = if k % 2 == 0 { 1.0_f64 } else { -1.0_f64 };
let y = Matrix::<f64, 1, 1> {
data: [[sign * 10.0]],
};
f.update(&y);
}
// R should have adapted from initial
let r_after = f.r.data[0][0];
// R should be positive and finite
assert!(r_after > 0.0 && r_after.is_finite(), "r={r_after}");
// And different from initial (adaptation occurred)
// weak check: just verify r is still finite after adaptation
let _ = r_after;
}
#[test]
fn reset_zeroes_state() {
let mut f = make_filter();
let u = Matrix::<f64, 1, 1> { data: [[0.0]] };
let y = Matrix::<f64, 1, 1> { data: [[3.0]] };
for _ in 0..20 {
f.predict(&u);
f.update(&y);
}
f.reset();
assert!(f.state().data[0][0].abs() < 1e-12);
assert!(f.state().data[1][0].abs() < 1e-12);
}
#[test]
fn alpha_stored_correctly() {
let f = make_filter();
assert!((f.alpha - 0.05).abs() < 1e-12);
}
}