Skip to main content

lite_ekf_rs/
lib.rs

1// Copyright (C) 2026 Jorge Andre Castro
2//
3// Ce programme est un logiciel libre : vous pouvez le redistribuer et/ou le modifier
4// selon les termes de la Licence Publique Générale GNU telle que publiée par la
5// Free Software Foundation, soit la version 2 de la licence, soit (à votre convention)
6// n'importe quelle version ultérieure.
7//
8// ---
9//
10// Basé sur TinyEKF :
11// Copyright (C) 2015 Simon D. Levy
12// Publié sous licence MIT.
13
14#![no_std]
15#![forbid(unsafe_code)]
16
17use embedded_f32_sqrt::sqrt;
18
19/// Filtre de Kalman Étendu (EKF) générique pour état de taille N et mesures de taille M.
20#[derive(Debug, Clone)]
21pub struct ExtendedKalmanFilter<const N: usize, const M: usize> {
22    /// Vecteur d'état \hat{x} (taille N)
23    pub x: [f32; N],
24    /// Matrice de covariance d'erreur P (taille N x N)
25    pub p: [[f32; N]; N],
26}
27
28impl<const N: usize, const M: usize> ExtendedKalmanFilter<N, M> {
29    /// Initialise le filtre avec la diagonale de la matrice de covariance P
30    pub fn new(p_diag: &[f32; N]) -> Self {
31        let mut p = [[0.0; N]; N];
32        for i in 0..N {
33            p[i][i] = p_diag[i];
34        }
35        Self { x: [0.0; N], p }
36    }
37
38    /// Étape de prédiction : P_k = F * P_{k-1} * F^T + Q
39    pub fn predict(&mut self, fx: [f32; N], f: &[[f32; N]; N], q: &[[f32; N]; N]) {
40        self.x = fx;
41
42        // FP = F * P
43        let mut fp = [[0.0; N]; N];
44        for i in 0..N {
45            for j in 0..N {
46                for k in 0..N {
47                    fp[i][j] += f[i][k] * self.p[k][j];
48                }
49            }
50        }
51
52        // FPFt = FP * F^T
53        let mut fp_ft = [[0.0; N]; N];
54        for i in 0..N {
55            for j in 0..N {
56                for k in 0..N {
57                    fp_ft[i][j] += fp[i][k] * f[j][k]; // f[j][k] est F^T[k][j]
58                }
59            }
60        }
61
62        // P = FPFt + Q
63        for i in 0..N {
64            for j in 0..N {
65                self.p[i][j] = fp_ft[i][j] + q[i][j];
66            }
67        }
68    }
69
70    /// Étape de mise à jour (Correction) pour un vecteur de mesure z de taille M
71    pub fn update(
72        &mut self,
73        z: &[f32; M],
74        hx: &[f32; M],
75        h: &[[f32; N]; M], // Matrice M x N
76        r: &[[f32; M]; M], // Matrice M x M
77    ) -> bool {
78        // 1. HP = H * P  (M x N)
79        let mut hp = [[0.0; N]; M];
80        for i in 0..M {
81            for j in 0..N {
82                for k in 0..N {
83                    hp[i][j] += h[i][k] * self.p[k][j];
84                }
85            }
86        }
87
88        // 2. S = HP * H^T + R  (M x M)
89        let mut s = *r;
90        for i in 0..M {
91            for j in 0..M {
92                for k in 0..N {
93                    s[i][j] += hp[i][k] * h[j][k]; // h[j][k] est H^T[k][j]
94                }
95            }
96        }
97
98        // 3. Inversion de la matrice S (M x M) via la décomposition de Cholesky
99        let mut s_inv = [[0.0; M]; M];
100        if !cholesky_invert(&s, &mut s_inv) {
101            return false; // Matrice non inversible / non définie positive
102        }
103
104        // 4. PHt = P * H^T  (N x M)
105        let mut p_ht = [[0.0; M]; N];
106        for i in 0..N {
107            for j in 0..M {
108                for k in 0..N {
109                    p_ht[i][j] += self.p[i][k] * h[j][k];
110                }
111            }
112        }
113
114        // 5. Gain de Kalman : G = PHt * S^-1  (N x M)
115        let mut g = [[0.0; M]; N];
116        for i in 0..N {
117            for j in 0..M {
118                for k in 0..M {
119                    g[i][j] += p_ht[i][k] * s_inv[k][j];
120                }
121            }
122        }
123
124        // 6. Innovation : y = z - hx  (taille M)
125        let mut y = [0.0; M];
126        for i in 0..M {
127            y[i] = z[i] - hx[i];
128        }
129
130        // 7. Mise à jour de l'état : x = x + G * y  (taille N)
131        for i in 0..N {
132            for j in 0..M {
133                self.x[i] += g[i][j] * y[j];
134            }
135        }
136
137        // 8. GH = G * H  (N x N)
138        let mut gh = [[0.0; N]; N];
139        for i in 0..N {
140            for j in 0..N {
141                for k in 0..M {
142                    gh[i][j] += g[i][k] * h[k][j];
143                }
144            }
145        }
146
147        // 9. P = (I - GH) * P  (N x N)
148        let mut ghp = [[0.0; N]; N];
149        for i in 0..N {
150            for j in 0..N {
151                for k in 0..N {
152                    let i_gh_ik = if i == k { 1.0 - gh[i][k] } else { -gh[i][k] };
153                    ghp[i][j] += i_gh_ik * self.p[k][j];
154                }
155            }
156        }
157        self.p = ghp;
158
159        true
160    }
161}
162
163// --- Inversion de matrice M x M par Décomposition de Cholesky ---
164
165fn cholesky_invert<const M: usize>(a: &[[f32; M]; M], a_inv: &mut [[f32; M]; M]) -> bool {
166    let mut p = [0.0; M];
167    let mut l = *a;
168
169    // Décomposition L * L^T
170    for i in 0..M {
171        for j in i..M {
172            let mut sum = l[i][j];
173            for k in (0..i).rev() {
174                sum -= l[i][k] * l[j][k];
175            }
176            if i == j {
177                if sum <= 0.0 {
178                    return false; // Matrice non définie positive
179                }
180                match sqrt(sum) {
181                    Ok(val) => p[i] = val,
182                    Err(_) => return false,
183                }
184            } else {
185                l[j][i] = sum / p[i];
186            }
187        }
188    }
189
190    // Inversion de L
191    for i in 0..M {
192        l[i][i] = 1.0 / p[i];
193        for j in (i + 1)..M {
194            let mut sum = 0.0;
195            for k in i..j {
196                sum -= l[j][k] * l[k][i];
197            }
198            l[j][i] = sum / p[j];
199        }
200    }
201
202    // Calcul de A^-1 = (L^-1)^T * L^-1
203    for i in 0..M {
204        for j in i..M {
205            let mut sum = 0.0;
206            for k in j..M {
207                sum += l[k][i] * l[k][j];
208            }
209            a_inv[i][j] = sum;
210            a_inv[j][i] = sum; // Symétrie
211        }
212    }
213
214    true
215}