1// Kalman Filter — Predict + Update
2// Fuses noisy detector estimates into
3// one smooth, accurate tracking signal.
4
5interface State {
6 position: Vec2;
7 velocity: Vec2;
8 covariance: number; // uncertainty
9}
10
11// Predict: project state forward
12function predict(s: State, dt: number): State {
13 return {
14 position: s.position + s.velocity * dt,
15 velocity: s.velocity,
16 covariance: s.covariance + Q * dt,
17 };
18}
19
20// Update: correct with measurement
21function update(
22 s: State,
23 measurements: Measurement[]
24): State {
25 for (const z of measurements) {
26 if (z.position === null) continue;
27 const R = (z.noise * z.noise) / z.confidence;
28 const K = s.covariance / (s.covariance + R);
29 s.position += K * (z.position - s.position);
30 s.covariance *= (1 - K);
31 }
32 return s;
33}