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}