kalman_rust/kalman/kalman_2d.rs
1use nalgebra;
2use std::error::Error;
3use std::fmt;
4
5// Error struct for failed `nalgebra` operations
6#[derive(Debug)]
7pub struct Kalman2DError {
8 typ: u16,
9}
10impl fmt::Display for Kalman2DError {
11 fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
12 match self.typ {
13 1 => write!(f, "Can't inverse matrix"),
14 _ => write!(f, "Undefined error"),
15 }
16 }
17}
18impl Error for Kalman2DError {}
19
20// Identity matrix. See the ref. https://en.wikipedia.org/wiki/Identity_matrix
21const I: nalgebra::SMatrix<f32, 4, 4> = nalgebra::SMatrix::<f32, 4, 4>::new(
22 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
23);
24
25/// Implementation of Discrete Kalman filter for case when there are two variables: X and Y.
26#[derive(Debug, Clone)]
27pub struct Kalman2D {
28 // Single cycle time
29 dt: f32,
30 // Control input
31 u: nalgebra::SMatrix<f32, 2, 1>,
32 // Standart deviation of acceleration
33 std_dev_a: f32,
34 // Standart deviation of measurement for X
35 std_dev_mx: f32,
36 // Standart deviation of measurement for Y
37 std_dev_my: f32,
38 // Transition matrix
39 A: nalgebra::SMatrix<f32, 4, 4>,
40 // Control matrix
41 B: nalgebra::SMatrix<f32, 4, 2>,
42 // Transformation (observation) matrix
43 H: nalgebra::SMatrix<f32, 2, 4>,
44 // Process noise covariance matrix
45 Q: nalgebra::SMatrix<f32, 4, 4>,
46 // Measurement noise covariance matrix
47 R: nalgebra::SMatrix<f32, 2, 2>,
48 // Error covariance matrix
49 P: nalgebra::SMatrix<f32, 4, 4>,
50 // State vector: x, y, vx, vy
51 x: nalgebra::SVector<f32, 4>,
52}
53
54impl Kalman2D {
55 /// Creates new `Kalman2D`
56 ///
57 /// Basic usage:
58 ///
59 /// ```
60 /// use kalman_rust::kalman::Kalman2D;
61 /// let dt = 0.1; // Single cycle time
62 /// let ux = 2.0; // Control input for X
63 /// let uy = 2.0; // Control input for Y
64 /// let std_dev_a = 0.25; // Standart deviation of acceleration
65 /// let std_dev_mx = 1.2; // Standart deviation of measurement for X
66 /// let std_dev_my = 1.2; // Standart deviation of measurement for Y
67 /// let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my);
68 /// ```
69 pub fn new(
70 dt: f32,
71 ux: f32,
72 uy: f32,
73 std_dev_a: f32,
74 std_dev_mx: f32,
75 std_dev_my: f32,
76 ) -> Self {
77 Kalman2D {
78 dt,
79 u: nalgebra::SMatrix::<f32, 2, 1>::new(ux, uy),
80 std_dev_a,
81 std_dev_mx,
82 std_dev_my,
83 // Ref.: Eq.(31)
84 A: nalgebra::SMatrix::<f32, 4, 4>::new(
85 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
86 ),
87 // Ref.: Eq.(32)
88 B: nalgebra::SMatrix::<f32, 4, 2>::new(
89 0.5 * dt.powi(2),
90 0.0,
91 0.0,
92 0.5 * dt.powi(2),
93 dt,
94 0.0,
95 0.0,
96 dt,
97 ),
98 // Ref.: Eq.(34)
99 H: nalgebra::SMatrix::<f32, 2, 4>::new(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
100 // Ref.: Eq.(40)
101 Q: nalgebra::SMatrix::<f32, 4, 4>::new(
102 0.25 * dt.powi(4),
103 0.0,
104 0.5 * dt.powi(3),
105 0.0,
106 0.0,
107 0.25 * dt.powi(4),
108 0.0,
109 0.5 * dt.powi(3),
110 0.5 * dt.powi(3),
111 0.0,
112 dt.powi(2),
113 0.0,
114 0.0,
115 0.5 * dt.powi(3),
116 0.0,
117 dt.powi(2),
118 ) * std_dev_a.powi(2),
119 // Ref.: Eq.(41)
120 R: nalgebra::SMatrix::<f32, 2, 2>::new(
121 std_dev_mx.powi(2),
122 0.0,
123 0.0,
124 std_dev_my.powi(2),
125 ),
126 P: nalgebra::SMatrix::<f32, 4, 4>::new(
127 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
128 ),
129 x: nalgebra::SVector::<f32, 4>::new(0.0, 0.0, 0.0, 0.0),
130 }
131 }
132 /// Creates new `Kalman2D` with initial state
133 ///
134 /// Why is it needed to set the initial state to the actual first observed coordinates of an object (sometimes)?
135 /// When the first state vector is initialized with zeros, it assumes that the object is at the origin
136 /// and the filter needs to estimate the position of the object from scratch, which can result in some initial inaccuracies.
137 /// On the other hand, initializing the first state vector with the actual observed coordinates of the object can provide
138 /// a more accurate estimate from the beginning, which can improve the overall tracking performance of the filter
139 ///
140 ///
141 /// Basic usage:
142 ///
143 /// ```
144 /// use kalman_rust::kalman::Kalman2D;
145 /// let dt = 0.1; // Single cycle time
146 /// let ux = 2.0; // Control input for X
147 /// let uy = 2.0; // Control input for Y
148 /// let std_dev_a = 0.25; // Standart deviation of acceleration
149 /// let std_dev_mx = 1.2; // Standart deviation of measurement for X
150 /// let std_dev_my = 1.2; // Standart deviation of measurement for Y
151 /// let ix = 1.0; // Initial state for X
152 /// let iy = 5.0; // Initial state for Y
153 /// let mut kalman = Kalman2D::new_with_state(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my, ix, iy);
154 /// ```
155 pub fn new_with_state(
156 dt: f32,
157 ux: f32,
158 uy: f32,
159 std_dev_a: f32,
160 std_dev_mx: f32,
161 std_dev_my: f32,
162 x: f32,
163 y: f32,
164 ) -> Self {
165 Kalman2D {
166 dt,
167 u: nalgebra::SMatrix::<f32, 2, 1>::new(ux, uy),
168 std_dev_a,
169 std_dev_mx,
170 std_dev_my,
171 // Ref.: Eq.(31)
172 A: nalgebra::SMatrix::<f32, 4, 4>::new(
173 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, dt, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
174 ),
175 // Ref.: Eq.(32)
176 B: nalgebra::SMatrix::<f32, 4, 2>::new(
177 0.5 * dt.powi(2),
178 0.0,
179 0.0,
180 0.5 * dt.powi(2),
181 dt,
182 0.0,
183 0.0,
184 dt,
185 ),
186 // Ref.: Eq.(34)
187 H: nalgebra::SMatrix::<f32, 2, 4>::new(1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0),
188 // Ref.: Eq.(40)
189 Q: nalgebra::SMatrix::<f32, 4, 4>::new(
190 0.25 * dt.powi(4),
191 0.0,
192 0.5 * dt.powi(3),
193 0.0,
194 0.0,
195 0.25 * dt.powi(4),
196 0.0,
197 0.5 * dt.powi(3),
198 0.5 * dt.powi(3),
199 0.0,
200 dt.powi(2),
201 0.0,
202 0.0,
203 0.5 * dt.powi(3),
204 0.0,
205 dt.powi(2),
206 ) * std_dev_a.powi(2),
207 // Ref.: Eq.(41)
208 R: nalgebra::SMatrix::<f32, 2, 2>::new(
209 std_dev_mx.powi(2),
210 0.0,
211 0.0,
212 std_dev_my.powi(2),
213 ),
214 P: nalgebra::SMatrix::<f32, 4, 4>::new(
215 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0,
216 ),
217 x: nalgebra::SVector::<f32, 4>::new(x, y, 0.0, 0.0),
218 }
219 }
220 /// Projects the state and the error covariance ahead
221 /// Mutates the state vector and the error covariance matrix
222 ///
223 /// Basic usage:
224 ///
225 /// ```
226 /// use kalman_rust::kalman::Kalman2D;
227 /// let dt = 0.1; // Single cycle time
228 /// let ux = 2.0; // Control input for X
229 /// let uy = 2.0; // Control input for Y
230 /// let std_dev_a = 0.25; // Standart deviation of acceleration
231 /// let std_dev_mx = 1.2; // Standart deviation of measurement for X
232 /// let std_dev_my = 1.2; // Standart deviation of measurement for Y
233 /// let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my);
234 /// let measurements = vec![(1.0, 2.0), (2.0, 3.0), (3.0, 4.0)];
235 /// for x in measurements.iter() {
236 /// // get measurement
237 /// kalman.predict();
238 /// // then do update
239 /// }
240 /// ```
241 pub fn predict(&mut self) {
242 // Ref.: Eq.(5)
243 self.x = (self.A * self.x) + (self.B * self.u);
244 // Ref.: Eq.(6)
245 self.P = self.A * self.P * self.A.transpose() + self.Q;
246 }
247 /// Computes the Kalman gain and then updates the state vector and the error covariance matrix
248 /// Mutates the state vector and the error covariance matrix.
249 ///
250 /// Basic usage:
251 ///
252 /// ```
253 /// use kalman_rust::kalman::Kalman2D;
254 /// let dt = 0.1; // Single cycle time
255 /// let ux = 2.0; // Control input for X
256 /// let uy = 2.0; // Control input for Y
257 /// let std_dev_a = 0.25; // Standart deviation of acceleration
258 /// let std_dev_mx = 1.2; // Standart deviation of measurement for X
259 /// let std_dev_my = 1.2; // Standart deviation of measurement for Y
260 /// let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my);
261 /// let measurements = vec![(1.0, 2.0), (2.0, 3.0), (3.0, 4.0)];
262 /// for (mx, my) in measurements.iter() {
263 /// kalman.predict();
264 /// kalman.update(*mx, *my).unwrap(); // assuming that there is noise in measurement
265 /// }
266 /// ```
267 pub fn update(&mut self, _zx: f32, _zy: f32) -> Result<(), Kalman2DError> {
268 // Ref.: Eq.(7)
269 let gain = match (self.H * self.P * self.H.transpose() + self.R).try_inverse() {
270 Some(inv) => self.P * self.H.transpose() * inv,
271 None => return Err(Kalman2DError { typ: 1 }),
272 };
273 // Ref.: Eq.(8)
274 let z = nalgebra::SMatrix::<f32, 2, 1>::new(_zx, _zy);
275 let r = z - self.H * self.x;
276 // Ref.: Eq.(9)
277 self.x = self.x + gain * r;
278 // Ref.: Eq.(10)
279 self.P = (I - gain * self.H) * self.P;
280 Ok(())
281 }
282 /// Returns the current state (only X and Y, not Vx and Vy)
283 pub fn get_state(&self) -> (f32, f32) {
284 (self.x[0], self.x[1])
285 }
286 /// Returns the current velocity (only Vx and Vy, not X and Y)
287 pub fn get_velocity(&self) -> (f32, f32) {
288 (self.x[2], self.x[3])
289 }
290 /// Returns prediction without mutating the state vector and the error covariance matrix
291 pub fn get_predicted_position(&self) -> (f32, f32) {
292 let x_pred = (self.A * self.x) + (self.B * self.u);
293 (x_pred[0], x_pred[1])
294 }
295 /// Returns position uncertainty from P matrix
296 pub fn get_position_uncertainty(&self) -> f32 {
297 (self.P[(0, 0)].powi(2) + self.P[(1, 1)].powi(2)).sqrt()
298 }
299 /// Returns the current state (both (X, Y) and (Vx, Vy))
300 pub fn get_vector_state(&self) -> nalgebra::SVector<f32, 4> {
301 self.x
302 }
303}
304
305mod tests {
306 use super::*;
307 #[test]
308 fn test_2d_kalman() {
309 let dt = 0.04; // 1/25 = 25 fps - just an example
310 let ux = 1.0;
311 let uy = 1.0;
312 let std_dev_a = 2.0;
313 let std_dev_mx = 0.1;
314 let std_dev_my = 0.1;
315
316 // Sample measurements
317 // Note: in this example Y-axis going from up to down
318 let xs = vec![
319 311, 312, 313, 311, 311, 312, 312, 313, 312, 312, 312, 312, 312, 312, 312, 312, 312,
320 312, 311, 311, 311, 311, 311, 310, 311, 311, 311, 310, 310, 308, 307, 308, 308, 308,
321 307, 307, 307, 308, 307, 307, 307, 307, 307, 308, 307, 309, 306, 307, 306, 307, 308,
322 306, 306, 306, 305, 307, 307, 307, 306, 306, 306, 307, 307, 308, 307, 307, 308, 307,
323 306, 308, 309, 309, 309, 309, 308, 309, 309, 309, 308, 311, 311, 307, 311, 307, 313,
324 311, 307, 311, 311, 306, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312,
325 312, 312, 312, 312, 312, 312, 312, 312, 312, 312,
326 ];
327 let ys = vec![
328 5, 6, 8, 10, 11, 12, 12, 13, 16, 16, 18, 18, 19, 19, 20, 20, 22, 22, 23, 23, 24, 24,
329 28, 30, 32, 35, 39, 42, 44, 46, 56, 58, 70, 60, 52, 64, 51, 70, 70, 70, 66, 83, 80, 85,
330 80, 98, 79, 98, 61, 94, 101, 94, 104, 94, 107, 112, 108, 108, 109, 109, 121, 108, 108,
331 120, 122, 122, 128, 130, 122, 140, 122, 122, 140, 122, 134, 141, 136, 136, 154, 155,
332 155, 150, 161, 162, 169, 171, 181, 175, 175, 163, 178, 178, 178, 178, 178, 178, 178,
333 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178,
334 ];
335
336 // Assume that initial X,Y coordinates match the first measurement
337 let ix = xs[0] as f32; // Initial state for X
338 let iy = ys[0] as f32; // Initial state for Y
339 let mut kalman = Kalman2D::new_with_state(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my, ix, iy);
340 kalman.x.x = xs[0] as f32;
341 kalman.x.y = ys[0] as f32;
342 let mut predictions: Vec<Vec<f32>> = vec![];
343 let mut updated_states: Vec<Vec<f32>> = vec![];
344 for (x, y) in xs.iter().zip(ys.iter()) {
345 // Considering that the measurements are noisy
346 let mx = *x as f32;
347 let my = *y as f32;
348
349 // Predict stage
350 kalman.predict();
351 let state = kalman.get_vector_state();
352 predictions.push(vec![state.x, state.y]);
353
354 // Update stage
355 kalman.update(mx, my).unwrap();
356 let updated_state = kalman.get_vector_state();
357 updated_states.push(vec![updated_state.x, updated_state.y]);
358 }
359
360 // println!("measurement X;measurement Y;prediction X;prediction Y;updated X;updated Y");
361 // for i in 0..xs.len() {
362 // println!("{};{};{};{};{};{}", xs[i], ys[i], predictions[i][0], predictions[i][1], updated_states[i][0], updated_states[i][1]);
363 // }
364 }
365}