├── .gitignore ├── Cargo.toml ├── README.md ├── diagram.png ├── images ├── kalman-1d.png └── kalman-2d.png └── src ├── kalman ├── kalman_1d.rs ├── kalman_2d.rs └── mod.rs └── lib.rs /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | /Cargo.lock 3 | images/*.ods 4 | images/*.py 5 | images/*.csv 6 | bash_scripts/*.ps1 7 | bash_scripts/*.py -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "kalman-rust" 3 | repository = "https://github.com/LdDl/kalman-rs" 4 | readme = "README.md" 5 | keywords = ["computer-vision", "kalman", "tracking", "filtering"] 6 | license = "MIT" 7 | version = "0.2.3" 8 | edition = "2021" 9 | authors = ["Dimitrii Lopanov "] 10 | exclude = ["/.github", "/ci", "/tools", "release.toml", "rustfmt.toml"] 11 | description = "Dead simple implementation of Discrete Kalman filter for object tracking purposes" 12 | homepage = "https://github.com/LdDl/kalman-rs" 13 | documentation = "https://docs.rs/kalman-rs" 14 | categories = ["algorithms", "computer-vision", "mathematics", "science"] 15 | 16 | # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html 17 | 18 | [dependencies] 19 | nalgebra = "0.32.3" 20 | uuid = { version = "1.5.0", features = ["serde", "v4"] } 21 | chrono = { version = "0.4.31", features = ["serde"] } 22 | rand = { version = "0.8.5" } -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Implementation of Discrete Kalman filter for object tracking purposes 2 | 3 | The Kalman filter estimates the state of a system at time $k$ via the linear stochastic difference equation considering the state of a system at time $k$ is evolved from the previous state at time $k-1$. See the ref. https://en.wikipedia.org/wiki/Kalman_filter 4 | 5 | In other words, the purpose of Kalman filter is to predict the next state via using prior knowledge of the current state. 6 | 7 | In this repository Hybrid Kalman filter is implemented considering continuous-time model while discrete-time measurements. See the ref. - https://en.wikipedia.org/wiki/Kalman_filter#Hybrid_Kalman_filter 8 | 9 | ## Table of Contents 10 | 11 | - [Main algorithm and equations](#main-algorithm-and-equations) 12 | - [How to use](#how-to-use) 13 | - [References](#References) 14 | 15 | ## Main algorithm and equations 16 | 17 | Define mentioned _linear stochastic difference equation_: 18 | 19 | $$\chi_{k} = A⋅\chi_{k-1} + B⋅u_{k-1} + w_{k-1} \tag{1}$$ 20 | 21 | Define measurement model: 22 | $$z_{k} = H⋅\chi_{k} + v_{k}\tag{2}$$ 23 | 24 | Let's denote variables: 25 | 26 | * $A$ (sometimes it's written as $F$, but I prefer to stick with $A$) - [Transition matrix](https://en.wikipedia.org/wiki/State-transition_matrix) of size $n \times n$ relating state $k-1$ to state $k$ 27 | * $B$ - Control input matrix of size $n \times l$ which is applied to *optional* control input $u_{k-1}$ 28 | * $H$ - Transformation (observation) matrix of size $m \times n$. 29 | * $u_{k}$ - Control input 30 | * $w_{k}$ - Process noise vector with covariance $Q$. Gaussian noise with the normal probability distribution: 31 | $$w(t) \sim N(0, Q) \tag{3}$$ 32 | * $v_{k}$ - Measurement noise vector (uncertainty) with covariance $R$. Gaussian noise with the normal probability distribution: 33 | $$v(t) \sim N(0, R) \tag{4}$$ 34 | 35 | ### Prediction 36 | 37 | Let's use the dash sign " $-$ " as superscript to indicate the a priory state. 38 | 39 | A priory state in matrix notation is defined as 40 | 41 | $$\hat{\chi}^-_ {k} = A⋅\hat{\chi}_ {k-1} + B⋅u_ {k-1} \tag{5}$$ 42 | 43 | $$\text{, where $\hat{\chi}^-_ {k}$ - a priory state (a.k.a. predicted), $\hat{\chi}_ {k-1}$ - a posteriory state (a.k.a. previous)} $$ 44 | 45 | __Note: A posteriory state $\hat{\chi}_{k-1}$ on 0-th time step (initial) should be *guessed*__ 46 | 47 | Error covariance matrix $P^-$ is defined as 48 | 49 | $$P^-_ {k} = A⋅P_ {k-1}⋅A^{T} + Q \tag{6}$$ 50 | 51 | $$\text{, where $P_ {k-1}$ - previously estimated error covariance matrix of size $n \times n$ (should match transition matrix dimensions), } $$ 52 | $$\text{Q - process noise covariance}$$ 53 | 54 | __Note:__ $P_ {k-1}$ __on 0-th time step (initial) should be *guessed*__ 55 | 56 | ### Correction 57 | 58 | The Kalman gain (which minimizes the estimate variance) in matrix notation is defined as: 59 | 60 | $$K_ {k} = P^-_ {k}⋅H^{T}⋅(H⋅P^-_ {k}⋅H^{T}+R)^{-1} \tag{7}$$ 61 | 62 | $$\text{, where H - transformation matrix, R - measurement noise covariance}$$ 63 | 64 | After evaluating the Kalman gain we need to update a priory state $\hat{\chi}^-_ {k}$. In order to do that we need to calculate measurement residual: 65 | 66 | $$r_ {k} = z_ {k} - H⋅\hat{\chi}^-_ {k} \tag{8}$$ 67 | 68 | $$\text{, where $z_ {k}$ - true measurement, $H⋅\hat{\chi}^-_ {k}$ - previously estimated measurement}$$ 69 | 70 | Then we can update predicted state $\hat{\chi}_ {k}$: 71 | 72 | $$\hat{\chi}_ {k} = \hat{\chi}^-_ {k} + K_{k}⋅r_{k}$$ 73 | 74 | $$\text{or} \tag{9}$$ 75 | 76 | $$\hat{\chi}_ {k} = \hat{\chi}^-_ {k} + K_{k}⋅(z_{k} - H⋅\hat{\chi}^-_{k})$$ 77 | 78 | After that we should update error covariance matrix $P_{k}$ which will be used in next time stap (an so on): 79 | $$P_{k} = (I - K_{k}⋅H)⋅P^-_{k}\tag{10}$$ 80 | $$\text{, where $I$ - identity matrix (square matrix with ones on the main diagonal and zeros elsewhere)}$$ 81 | 82 | 83 | ### Overall 84 | The whole algorithm can be described as high-level diagram: 85 |

86 | 87 |

Fig 1. Operation of the Kalman filter. Welch & Bishop, 'An Introduction to the Kalman Filter'

88 |

89 | 90 | ## 1-D Kalman filter 91 | 92 | Considering acceleration motion let's write down its equations: 93 | 94 | Velocity: 95 | $$v = v_{0} + at \tag{11}$$ 96 | $$v(t) = x'(t) $$ 97 | $$a(t) = v'(t) = x''(t)$$ 98 | 99 | Position: 100 | $$x = x_{0} + v_{0}t + \frac{at^2}{2} \tag{12}$$ 101 | 102 | Let's write $(11)$ and $(12)$ in Lagrange form: 103 | 104 | $$x'_ {k} = x'_ {k-1} + x''_{k-1}\Delta t \tag{13}$$ 105 | 106 | $$x_{k} = x_{k-1} + x'_ {k-1}\Delta t + \frac{x''_{k-1}(\Delta t^2)}{2} \tag{14}$$ 107 | 108 | State vector $\chi_{k}$ looks like: 109 | 110 | $$\chi_{k} = \begin{bmatrix} 111 | x_{k} \\ 112 | x'_ {k} 113 | \end{bmatrix} = \begin{bmatrix} 114 | x_{k-1} + x'_ {k-1}\Delta t + \frac{x''_ {k-1}(\Delta t^2)}{2} \\ 115 | x'_ {k-1} + x''_{k-1}\Delta t 116 | \end{bmatrix} \tag{15}$$ 117 | 118 | Matrix form of $\chi_{k}$ : 119 | 120 | $$\chi_{k} = \begin{bmatrix} x_{k} \\ 121 | x'_ {k} \end{bmatrix} = \begin{bmatrix} 1 & \Delta t \\ 122 | 0 & 1\end{bmatrix} ⋅ \begin{bmatrix} x_{k-1} \\ 123 | x'_ {k-1} \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ 124 | \Delta t \end{bmatrix} ⋅ x''_ {k-1} = \begin{bmatrix} 1 & \Delta t \\ 125 | 0 & 1\end{bmatrix} ⋅ \chi_{k-1} + \begin{bmatrix} \frac{\Delta t^2}{2} \\ 126 | \Delta t \end{bmatrix} ⋅ x''_{k-1} \tag{16}$$ 127 | 128 | 129 | Taking close look on $(16)$ and $(1)$ we can write transition matrix $A$ and control input matrix $B$ as follows: 130 | 131 | $$A = \begin{bmatrix} 1 & \Delta t \\ 132 | 0 & 1\end{bmatrix} \tag{17}$$ 133 | 134 | $$B = \begin{bmatrix} \frac{\Delta t^2}{2} \\ 135 | \Delta t \end{bmatrix} \tag{18}$$ 136 | 137 | Let's find transformation matrix $H$. According to $(2)$: 138 | 139 | $$z_{k} = H⋅\chi_{k} + v_{k} = \begin{bmatrix} 1 & 0 \end{bmatrix} ⋅\begin{bmatrix} x_{k} \\ 140 | {x'_ {k}} \end{bmatrix} + v_{k} \tag{19}$$ 141 | 142 | $$ H = \begin{bmatrix} 1 & 0 \end{bmatrix} \tag{20}$$ 143 | 144 | __Notice:__ $v_{k}$ __in__ $(19)$ __- is not speed, but measurement noise! Don't be confused with notation. E.g.:__ 145 | 146 | $$ \text{$ \chi_{k} = \begin{bmatrix} 375.74 \\ 147 | 0 - \text{assume zero velocity} \end{bmatrix} $, $ v_{k} = 2.64 => $} $$ 148 | 149 | $$ \text{$ => z_{k} = \begin{bmatrix} 1 & 0 \end{bmatrix} ⋅\begin{bmatrix} 375.74 \\ 150 | 0 \end{bmatrix} + 2.64 = \begin{bmatrix} 378.38 & 2.64 \end{bmatrix} $ - you can see that first vector argument it is just noise $v_{k}$ added}$$ 151 | 152 | $$ \text{to observation $x_{k}$ and the second argument is noise $v_{k}$ itself.}$$ 153 | 154 | Process noise covariance matrix $Q$: 155 | 156 | $$Q = \begin{matrix} 157 | & \begin{matrix}x && x'\end{matrix} \\ 158 | \begin{matrix}x \\ 159 | x'\end{matrix} & 160 | \begin{bmatrix} \sigma^2_{x} & \sigma_{x} \sigma_{x'} \\ 161 | \sigma_{x'} \sigma_{x} & \sigma^2_{x'}\end{bmatrix} 162 | \\\\ 163 | \end{matrix} \tag{21}$$ 164 | 165 | $$\text{, where} $$ 166 | 167 | $$ \text{$\sigma_{x}$ - standart deviation of position} $$ 168 | 169 | $$ \text{$\sigma_{x'}$ - standart deviation of velocity} $$ 170 | 171 | Since we know about $(14)$ we can define $\sigma_{x}$ and $\sigma_{x'}$ as: 172 | 173 | $$ \sigma_{x} = \sigma_{x''} \frac{\Delta t^2}{2} \tag{22}$$ 174 | 175 | $$ \sigma_{x'} = \sigma_{x''} \Delta t \tag{23}$$ 176 | 177 | $$\text{, where $\sigma_{x''}$ - standart deviation of acceleration (tuned value)} $$ 178 | 179 | And now process noise covariance matrix $Q$ could be defined as: 180 | 181 | $$ Q = \begin{bmatrix} (\sigma_{x''} \frac{\Delta t^2}{2})^2 & \sigma_{x''} \frac{\Delta t^2}{2} \sigma_{x''} \Delta t \\ 182 | \sigma_{x''} \Delta t \sigma_{x''} \frac{\Delta t^2}{2} & (\sigma_{x''} \Delta t)^2 \end{bmatrix} = $$ 183 | 184 | $$ = \begin{bmatrix} (\sigma_{x''} \frac{\Delta t^2}{2})^2 & (\sigma_{x''})^2 \frac{\Delta t^2}{2} \Delta t \\ 185 | (\sigma_{x''})^2 \Delta t \frac{\Delta t^2}{2} & (\sigma_{x''} \Delta t)^2 \end{bmatrix} = \begin{bmatrix} (\frac{\Delta t^2}{2})^2 & \frac{\Delta t^2}{2} \Delta t \\ 186 | \Delta t \frac{\Delta t^2}{2} & \Delta t^2 \end{bmatrix} \sigma^2_{x''}$$ 187 | 188 | $$ = \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\ 189 | \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix} \sigma^2_{x''} \tag{24}$$ 190 | 191 | $$ \text{Assuming that $x''$ - is acceleration $a$, $Q = \begin{bmatrix} \frac{\Delta t^4}{4} & \frac{\Delta t^3}{2} \\ 192 | \frac{\Delta t^3}{2} & \Delta t^2 \end{bmatrix} \sigma^2_{a}$} \tag{25}$$ 193 | 194 | Covariance of measurement noise $R$ is scalar (matrix of size $1 \times 1$) and it is defined as variance of the measurement noise: 195 | 196 | $$R = \begin{matrix} 197 | \begin{matrix}& x\end{matrix} \\ 198 | \begin{matrix}x\end{matrix} 199 | \begin{bmatrix}\sigma^2_{z}\end{bmatrix} 200 | \\\\ 201 | \end{matrix} = \sigma^2_{z} \tag{26}$$ 202 | 203 | Rust implementation is [here](./src/kalman/kalman_1d.rs#L4) 204 | 205 | Example of usage: 206 | ```rust 207 | let dt = 0.1; 208 | let u = 2.0; 209 | let std_dev_a = 0.25; 210 | let std_dev_m = 1.2; 211 | 212 | let t: nalgebra::SVector:: = nalgebra::SVector::::from_iterator(float_loop(0.0, 100.0, dt)); 213 | let track = t.map(|t| dt*(t*t - t)); 214 | 215 | 216 | let mut kalman = Kalman1D::new(dt, u, std_dev_a, std_dev_m); 217 | let mut measurement: Vec = vec![]; 218 | let mut predictions: Vec= vec![]; 219 | for (t, x) in t.iter().zip(track.iter()) { 220 | // Add some noise to perfect track 221 | let v: f32 = StdRng::from_entropy().sample::(Standard) * (50.0+50.0) - 50.0; // Generate noise in [-50, 50) 222 | let z = kalman.H.x * x + v; 223 | measurement.push(z); 224 | 225 | // Predict stage 226 | kalman.predict(); 227 | predictions.push(kalman.x.x); 228 | 229 | // Update stage 230 | kalman.update(z).unwrap(); 231 | } 232 | println!("time;perfect;measurement;prediction"); 233 | for i in 0..track.len() { 234 | println!("{};{};{};{}", t[i], track[i], measurement[i], predictions[i]); 235 | } 236 | ``` 237 | 238 | How exported chart does look like: 239 | 240 | 241 | 242 | ## 2-D Kalman filter 243 | 244 | Considering acceleration motion again let's write down its equations: 245 | 246 | Considering the same physical model as in $(13)$ - $(14)$ let's write down state vector $\chi_{k}$: 247 | 248 | $$\chi_{k} = \begin{bmatrix} 249 | x_{k} \\ 250 | y_{k} \\ 251 | x'_ {k} \\ 252 | y'_ {k} \end{bmatrix} = \begin{bmatrix} 253 | x_{k-1} + x'_ {k-1}\Delta t + \frac{x''_ {k-1}(\Delta t^2)}{2} \\ 254 | y_{k-1} + y'_ {k-1}\Delta t + \frac{y''_ {k-1}(\Delta t^2)}{2} \\ 255 | x'_ {k-1} + x''_ {k-1}\Delta t \\ 256 | y'_ {k-1} + y''_ {k-1}\Delta t 257 | \end{bmatrix} \tag{27}$$ 258 | 259 | Matrix form of $\chi_{k}$ : 260 | 261 | $$\chi_{k} = \begin{bmatrix} x_{k} \\ 262 | y_{k} \\ 263 | x'_ {k} \\ 264 | y'_ {k} 265 | \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 266 | 0 & 1 & 0 & \Delta t \\ 267 | 0 & 0 & 1 & 0 \\ 268 | 0 & 0 & 0 & 1 \end{bmatrix} ⋅ \begin{bmatrix} x_{k-1} \\ 269 | y_{k-1} \\ 270 | x'_ {k-1} \\ 271 | y'_ {k-1} \end{bmatrix} + \begin{bmatrix} \frac{\Delta t^2}{2} & 0 \\ 272 | 0 & \frac{\Delta t^2}{2} \\ 273 | \Delta t & 0 \\ 274 | 0 & \Delta t \end{bmatrix} ⋅ \begin{bmatrix} x''_ {k-1} \\ 275 | y''_ {k-1} \end{bmatrix} = $$ 276 | $$ = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 277 | 0 & 1 & 0 & \Delta t \\ 278 | 0 & 0 & 1 & 0 \\ 279 | 0 & 0 & 0 & 1 \end{bmatrix} ⋅ \chi_{k-1} + \begin{bmatrix} \frac{\Delta t^2}{2} & 0 \\ 280 | 0 & \frac{\Delta t^2}{2} \\ 281 | \Delta t & 0 \\ 282 | 0 & \Delta t \end{bmatrix} ⋅ \begin{bmatrix} x''_ {k-1} \\ 283 | y''_{k-1} \end{bmatrix} \tag{28}$$ 284 | 285 | $$ \text{Assuming that $x''$ and $y''$ - is acceleration $a$, }$$ 286 | 287 | $$ a_{k-1} = \begin{bmatrix} x''_ {k-1} \\ 288 | y''_{k-1} \end{bmatrix} \tag{29}$$ 289 | 290 | $$\chi_{k} = \begin{bmatrix} x_{k} \\ 291 | y_{k} \\ 292 | x'_ {k} \\ 293 | y'_ {k} 294 | \end{bmatrix} = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 295 | 0 & 1 & 0 & \Delta t \\ 296 | 0 & 0 & 1 & 0 \\ 297 | 0 & 0 & 0 & 1 \end{bmatrix} ⋅ \chi_{k-1} + \begin{bmatrix} \frac{\Delta t^2}{2} & 0 \\ 298 | 0 & \frac{\Delta t^2}{2} \\ 299 | \Delta t & 0 \\ 300 | 0 & \Delta t \end{bmatrix} ⋅ a_{k-1} \tag{30}$$ 301 | 302 | 303 | Taking close look on $(16)$ and $(1)$ we can write transition matrix $A$ and control input matrix $B$ as follows: 304 | 305 | $$A = \begin{bmatrix} 1 & 0 & \Delta t & 0 \\ 306 | 0 & 1 & 0 & \Delta t \\ 307 | 0 & 0 & 1 & 0 \\ 308 | 0 & 0 & 0 & 1 \end{bmatrix} \tag{31}$$ 309 | 310 | $$B = \begin{bmatrix} \frac{\Delta t^2}{2} & 0 \\ 311 | 0 & \frac{\Delta t^2}{2} \\ 312 | \Delta t & 0 \\ 313 | 0 & \Delta t \end{bmatrix} \tag{32}$$ 314 | 315 | Let's find transformation matrix $H$. According to $(2)$ and $(19)$ - $(20)$: 316 | 317 | $$z_{k} = H⋅\chi_{k} + v_{k} = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 318 | 0 & 1 & 0 & 0 \end{bmatrix} ⋅\begin{bmatrix} x_{k} \\ 319 | y_{k} \\ 320 | {x'_ {k}} \\ 321 | {y'_ {k}} \end{bmatrix} + v_{k} \tag{33}$$ 322 | 323 | $$ H = \begin{bmatrix} 1 & 0 & 0 & 0 \\ 324 | 0 & 1 & 0 & 0 \end{bmatrix} \tag{34}$$ 325 | 326 | Process noise covariance matrix $Q$: 327 | 328 | $$Q = \begin{matrix} 329 | & \begin{matrix}x && y && x' && y'\end{matrix} \\ 330 | \begin{matrix}x \\ 331 | y \\ 332 | x' \\ 333 | y'\end{matrix} & 334 | \begin{bmatrix} \sigma^2_{x} & 0 & \sigma_{x} \sigma_{x'} & 0 \\ 335 | 0 & \sigma^2_{y} & 0 & \sigma_{y} \sigma_{y'} \\ 336 | \sigma_{x'} \sigma_{x} & 0 & \sigma^2_{x'} & 0 \\ 337 | 0 & \sigma_{y'} \sigma_{y} & 0 & \sigma^2_{y'}\end{bmatrix} 338 | \\\\ 339 | \end{matrix} \tag{35}$$ 340 | 341 | $$\text{, where} $$ 342 | 343 | $$ \text{$\sigma_{x}$ - standart deviation of position for $x$ component} $$ 344 | 345 | $$ \text{$\sigma_{y}$ - standart deviation of position for $y$ component} $$ 346 | 347 | $$ \text{$\sigma_{x'}$ - standart deviation of velocity for $x$ component} $$ 348 | 349 | $$ \text{$\sigma_{y'}$ - standart deviation of velocity for $y$ component} $$ 350 | 351 | Since we know about $(14)$ we can define $\sigma_{x}$, $\sigma_{y}$, $\sigma_{x'}$ and $\sigma_{y'}$ as: 352 | 353 | $$ \sigma_{x} = \sigma_{x''} \frac{\Delta t^2}{2} \tag{36}$$ 354 | 355 | $$ \sigma_{y} = \sigma_{y''} \frac{\Delta t^2}{2} \tag{37}$$ 356 | 357 | $$ \sigma_{x'} = \sigma_{x''} \Delta t \tag{38}$$ 358 | 359 | $$ \sigma_{y'} = \sigma_{y''} \Delta t \tag{39}$$ 360 | 361 | $$\text{, where $\sigma_{x''}$ and $\sigma_{y''}$ - standart deviation of acceleration (tuned values)} $$ 362 | 363 | And now process noise covariance matrix $Q$ could be defined as: 364 | 365 | $$ Q = \begin{bmatrix} (\sigma_{x''} \frac{\Delta t^2}{2})^2 & 0 & \sigma_{x''} \frac{\Delta t^2}{2} \sigma_{x''} \Delta t & 0 \\ 366 | 0 & (\sigma_{y''} \frac{\Delta t^2}{2})^2 & 0 & \sigma_{y''} \frac{\Delta t^2}{2} \sigma_{y''} \Delta t \\ 367 | \sigma_{x''} \frac{\Delta t^2}{2} \sigma_{x''} \Delta t & 0 & (\sigma_{x''} \Delta t)^2 & 0 \\ 368 | 0 & \sigma_{y''} \frac{\Delta t^2}{2} \sigma_{y''} \Delta t & 0 & (\sigma_{y''} \Delta t)^2 \end{bmatrix} = $$ 369 | 370 | $$ = \begin{bmatrix} (\sigma_{x''} \frac{\Delta t^2}{2})^2 & 0 & (\sigma_{x''})^2 \frac{\Delta t^2}{2} \Delta t & 0 \\ 371 | 0 & (\sigma_{y''} \frac{\Delta t^2}{2})^2 & 0 & (\sigma_{y''})^2 \frac{\Delta t^2}{2} \Delta t \\ 372 | (\sigma_{x''})^2 \frac{\Delta t^2}{2} \Delta t & 0 & (\sigma_{x''} \Delta t)^2 & 0 \\ 373 | 0 & (\sigma_{y''})^2 \frac{\Delta t^2}{2}\Delta t & 0 & (\sigma_{y''} \Delta t)^2 \end{bmatrix} = \text{| Knowing that $x''$ and $y''$ - acceleration|} = $$ 374 | $$ = \begin{bmatrix} (\frac{\Delta t^2}{2})^2 & 0 & \frac{\Delta t^2}{2} \Delta t & 0 \\ 375 | 0 & (\frac{\Delta t^2}{2})^2 & 0 & \frac{\Delta t^2}{2} \Delta t \\ 376 | \frac{\Delta t^2}{2} \Delta t & 0 & \Delta t^2 & 0 \\ 377 | 0 & \Delta t \frac{\Delta t^2}{2} & 0 & \Delta t^2 \end{bmatrix} \sigma^2_{a''}$$ 378 | 379 | $$ = \begin{bmatrix} \frac{\Delta t^4}{4} & 0 & \frac{\Delta t^3}{2} & 0 \\ 380 | 0 & \frac{\Delta t^4}{4} & 0 & \frac{\Delta t^3}{2} \\ 381 | \frac{\Delta t^3}{2} & 0 & \Delta t^2 & 0 \\ 382 | 0 & \frac{\Delta t^3}{2} & 0 & \Delta t^2 \end{bmatrix} \sigma^2_{a''} \tag{40}$$ 383 | 384 | Covariance of measurement noise $R$ is matrix of size $2 \times 2$ (since there are two components - $x$ and $y$) and it is defined as variance of the measurement noise: 385 | 386 | $$R = \begin{matrix} 387 | \begin{matrix}& x & y\end{matrix} \\ 388 | \begin{matrix}x \\ 389 | y \end{matrix} 390 | \begin{bmatrix}\sigma^2_{x} & 0 \\ 391 | 0 & \sigma^2_{y} \end{bmatrix} 392 | \\\\ 393 | \end{matrix} = \begin{bmatrix}\sigma^2_{x} & 0 \\ 394 | 0 & \sigma^2_{y} \end{bmatrix} \tag{41}$$ 395 | 396 | Rust implementation is [here](./src/kalman/kalman_2d.rs#L4) 397 | 398 | Example of usage: 399 | ```rust 400 | let dt = 0.04; // 1/25 = 25 fps - just an example 401 | let ux = 1.0; 402 | let uy = 1.0; 403 | let std_dev_a = 2.0; 404 | let std_dev_mx = 0.1; 405 | let std_dev_my = 0.1; 406 | 407 | // Sample measurements 408 | // Note: in this example Y-axis going from up to down 409 | let xs = vec![311, 312, 313, 311, 311, 312, 312, 313, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 311, 311, 311, 311, 311, 310, 311, 311, 311, 310, 310, 308, 307, 308, 308, 308, 307, 307, 307, 308, 307, 307, 307, 307, 307, 308, 307, 309, 306, 307, 306, 307, 308, 306, 306, 306, 305, 307, 307, 307, 306, 306, 306, 307, 307, 308, 307, 307, 308, 307, 306, 308, 309, 309, 309, 309, 308, 309, 309, 309, 308, 311, 311, 307, 311, 307, 313, 311, 307, 311, 311, 306, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312]; 410 | let ys = vec![5, 6, 8, 10, 11, 12, 12, 13, 16, 16, 18, 18, 19, 19, 20, 20, 22, 22, 23, 23, 24, 24, 28, 30, 32, 35, 39, 42, 44, 46, 56, 58, 70, 60, 52, 64, 51, 70, 70, 70, 66, 83, 80, 85, 80, 98, 79, 98, 61, 94, 101, 94, 104, 94, 107, 112, 108, 108, 109, 109, 121, 108, 108, 120, 122, 122, 128, 130, 122, 140, 122, 122, 140, 122, 134, 141, 136, 136, 154, 155, 155, 150, 161, 162, 169, 171, 181, 175, 175, 163, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178]; 411 | 412 | let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my); 413 | // Assume that initial X,Y coordinates match the first measurement 414 | kalman.x.x = xs[0] as f32; 415 | kalman.x.y = ys[0] as f32; 416 | let mut predictions: Vec> = vec![]; 417 | for (x, y) in xs.iter().zip(ys.iter()) { 418 | // Considering that the measurements are noisy 419 | let mx = *x as f32; 420 | let my = *y as f32; 421 | 422 | // Predict stage 423 | kalman.predict(); 424 | predictions.push(vec![kalman.x.x, kalman.x.y]); 425 | 426 | // Update stage 427 | kalman.update(mx, my).unwrap(); 428 | } 429 | println!("measurement X;measurement Y;prediction X;prediction Y"); 430 | for i in 0..xs.len() { 431 | println!("{};{};{};{}", xs[i], ys[i], predictions[i][0], predictions[i][1]); 432 | } 433 | ``` 434 | 435 | How exported chart does look like: 436 | 437 | 438 | 439 | 440 | ## 2-D Kalman filter (with acceleration component and no control input) 441 | 442 | __W.I.P.__ 443 | 444 | @todo: math-jax / rust code / rust test / plots 445 | 446 | ## How to use 447 | 448 | Add dependency to your Cargo.toml file 449 | 450 | ```toml 451 | [package] 452 | .... 453 | 454 | [dependencies] 455 | ... 456 | kalman-rust = "0.2.2" 457 | ... 458 | ``` 459 | 460 | Start using it, e.g. Kalman2D: 461 | ```rust 462 | use kalman_rust::kalman::{ 463 | Kalman2D 464 | }; 465 | 466 | fn main() { 467 | let dt = 0.04; // 1/25 = 25 fps - just an example 468 | let ux = 1.0; 469 | let uy = 1.0; 470 | let std_dev_a = 2.0; 471 | let std_dev_mx = 0.1; 472 | let std_dev_my = 0.1; 473 | 474 | // Sample measurements 475 | // Note: in this example Y-axis going from up to down 476 | let xs = vec![311, 312, 313, 311, 311, 312, 312, 313, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 311, 311, 311, 311, 311, 310, 311, 311, 311, 310, 310, 308, 307, 308, 308, 308, 307, 307, 307, 308, 307, 307, 307, 307, 307, 308, 307, 309, 306, 307, 306, 307, 308, 306, 306, 306, 305, 307, 307, 307, 306, 306, 306, 307, 307, 308, 307, 307, 308, 307, 306, 308, 309, 309, 309, 309, 308, 309, 309, 309, 308, 311, 311, 307, 311, 307, 313, 311, 307, 311, 311, 306, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312]; 477 | let ys = vec![5, 6, 8, 10, 11, 12, 12, 13, 16, 16, 18, 18, 19, 19, 20, 20, 22, 22, 23, 23, 24, 24, 28, 30, 32, 35, 39, 42, 44, 46, 56, 58, 70, 60, 52, 64, 51, 70, 70, 70, 66, 83, 80, 85, 80, 98, 79, 98, 61, 94, 101, 94, 104, 94, 107, 112, 108, 108, 109, 109, 121, 108, 108, 120, 122, 122, 128, 130, 122, 140, 122, 122, 140, 122, 134, 141, 136, 136, 154, 155, 155, 150, 161, 162, 169, 171, 181, 175, 175, 163, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178]; 478 | 479 | // Assume that initial X,Y coordinates match the first measurement 480 | let ix = xs[0] as f32; // Initial state for X 481 | let iy = ys[0] as f32; // Initial state for Y 482 | let mut kalman = Kalman2D::new_with_state(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my, ix, iy); 483 | let mut predictions: Vec> = vec![]; 484 | let mut updated_states: Vec> = vec![]; 485 | for (x, y) in xs.iter().zip(ys.iter()) { 486 | // Considering that the measurements are noisy 487 | let mx = *x as f32; 488 | let my = *y as f32; 489 | 490 | // Predict stage 491 | kalman.predict(); 492 | let state = kalman.get_vector_tate(); 493 | predictions.push(vec![state.x, state.y]); 494 | 495 | // Update stage 496 | kalman.update(mx, my).unwrap(); 497 | let updated_state = kalman.get_vector_tate(); 498 | updated_states.push(vec![updated_state.x, updated_state.y]); 499 | } 500 | 501 | println!("measurement X;measurement Y;prediction X;prediction Y;updated X;updated Y"); 502 | for i in 0..xs.len() { 503 | println!("{};{};{};{};{};{}", xs[i], ys[i], predictions[i][0], predictions[i][1], updated_states[i][0], updated_states[i][1]); 504 | } 505 | } 506 | ``` 507 | 508 | ## References 509 | * [Greg Welch and Gary Bishop, ‘An Introduction to the Kalman Filter’, July 24, 2006](https://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf) 510 | * [Introducion to the Kalman Filter by Alex Becker](https://www.kalmanfilter.net/default.aspx) 511 | * [Kalman filter on wikipedia](https://en.wikipedia.org/wiki/Kalman_filter) 512 | * [State-transition matrix](https://en.wikipedia.org/wiki/State-transition_matrix) 513 | * [Python implementation by Rahmad Sadli](https://machinelearningspace.com/object-tracking-python/) 514 | 515 | # P.S. 516 | I did struggle on displaying matrices in GitHub's MathJax markdown. If you know better way to do it you are welcome 517 | -------------------------------------------------------------------------------- /diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LdDl/kalman-rs/7a09636c79772832b8809605c70e1f5cb88798a9/diagram.png -------------------------------------------------------------------------------- /images/kalman-1d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LdDl/kalman-rs/7a09636c79772832b8809605c70e1f5cb88798a9/images/kalman-1d.png -------------------------------------------------------------------------------- /images/kalman-2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/LdDl/kalman-rs/7a09636c79772832b8809605c70e1f5cb88798a9/images/kalman-2d.png -------------------------------------------------------------------------------- /src/kalman/kalman_1d.rs: -------------------------------------------------------------------------------- 1 | use std::fmt; 2 | use std::error::Error; 3 | use nalgebra; 4 | 5 | // Error struct for failed `nalgebra` operations 6 | #[derive(Debug)] 7 | pub struct Kalman1DError{typ: u16} 8 | impl fmt::Display for Kalman1DError { 9 | fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { 10 | match self.typ { 11 | 1 => write!(f, "Can inverse matrix"), 12 | _ => write!(f, "Undefined error") 13 | } 14 | } 15 | } 16 | impl Error for Kalman1DError {} 17 | 18 | 19 | // Identity matrix. See the ref. https://en.wikipedia.org/wiki/Identity_matrix 20 | const I: nalgebra::SMatrix:: = nalgebra::SMatrix::::new( 21 | 1.0, 0.0, 22 | 0.0, 1.0, 23 | ); 24 | 25 | /// Implementation of Discrete Kalman filter for case when there is only on variable X. 26 | #[derive(Debug, Clone)] 27 | pub struct Kalman1D { 28 | // Single cycle time 29 | dt: f32, 30 | // Control input 31 | u: f32, 32 | // Standart deviation of acceleration 33 | std_dev_a: f32, 34 | // Standart deviation of measurement 35 | std_dev_m: f32, 36 | // Transition matrix 37 | A: nalgebra::SMatrix, 38 | // Control matrix 39 | B: nalgebra::SMatrix, 40 | // Transformation (observation) matrix 41 | H: nalgebra::SMatrix, 42 | // Process noise covariance matrix 43 | Q: nalgebra::SMatrix, 44 | // Measurement noise covariance matrix 45 | R: nalgebra::SMatrix, 46 | // Error covariance matrix 47 | P: nalgebra::SMatrix, 48 | // State vector: x, vx 49 | x: nalgebra::SVector, 50 | } 51 | 52 | impl Kalman1D { 53 | /// Creates new `Kalman1D` 54 | /// 55 | /// Basic usage: 56 | /// 57 | /// ``` 58 | /// use kalman_rust::kalman::Kalman1D; 59 | /// let dt = 0.1; // Single cycle time 60 | /// let u = 2.0; // Control input 61 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 62 | /// let std_dev_m = 1.2; // Standart deviation of measurement 63 | /// let mut kalman = Kalman1D::new(dt, u, std_dev_a, std_dev_m); 64 | /// ``` 65 | pub fn new(dt: f32, u: f32, std_dev_a: f32, std_dev_m: f32) -> Self { 66 | Kalman1D { 67 | dt, 68 | u, 69 | std_dev_a, 70 | std_dev_m, 71 | // Ref.: Eq.(17) 72 | A: nalgebra::SMatrix::::new( 73 | 1.0, dt, 74 | 0.0, 1.0, 75 | ), 76 | // Ref.: Eq.(18) 77 | B: nalgebra::SMatrix::::new( 78 | 0.5 * dt.powi(2), 79 | dt, 80 | ), 81 | // Ref.: Eq.(20) 82 | H: nalgebra::SMatrix::::new( 83 | 1.0, 0.0, 84 | ), 85 | // Ref.: Eq.(25) 86 | Q: nalgebra::SMatrix::::new( 87 | 0.25 * dt.powi(4), 0.5 * dt.powi(3), 88 | 0.5 * dt.powi(3), dt.powi(2), 89 | )*std_dev_a.powi(2), 90 | // Ref.: Eq.(26) 91 | R: nalgebra::SMatrix::::new( 92 | std_dev_m.powi(2), 93 | ), 94 | P: nalgebra::SMatrix::::new( 95 | 1.0, 0.0, 96 | 0.0, 1.0, 97 | ), 98 | x: nalgebra::SVector::::new( 99 | 0.0, 100 | 0.0, 101 | ), 102 | } 103 | } 104 | /// Projects the state and the error covariance ahead 105 | /// Mutates the state vector and the error covariance matrix 106 | /// 107 | /// Basic usage: 108 | /// 109 | /// ``` 110 | /// use kalman_rust::kalman::Kalman1D; 111 | /// let dt = 0.1; // Single cycle time 112 | /// let u = 2.0; // Control input 113 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 114 | /// let std_dev_m = 1.2; // Standart deviation of measurement 115 | /// let mut kalman = Kalman1D::new(dt, u, std_dev_a, std_dev_m); 116 | /// let measurements = vec![1.0, 2.0, 3.0, 4.0, 5.0]; 117 | /// for x in measurements.iter() { 118 | /// // get measurement 119 | /// kalman.predict(); 120 | /// // then do update 121 | /// } 122 | /// ``` 123 | pub fn predict(&mut self) { 124 | // Ref.: Eq.(5) 125 | self.x = (self.A*self.x) + (self.B*self.u); 126 | // Ref.: Eq.(6) 127 | self.P = self.A*self.P*self.A.transpose() + self.Q; 128 | } 129 | /// Computes the Kalman gain and then updates the state vector and the error covariance matrix 130 | /// Mutates the state vector and the error covariance matrix. 131 | /// 132 | /// Basic usage: 133 | /// 134 | /// ``` 135 | /// use kalman_rust::kalman::Kalman1D; 136 | /// let dt = 0.1; // Single cycle time 137 | /// let u = 2.0; // Control input 138 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 139 | /// let std_dev_m = 1.2; // Standart deviation of measurement 140 | /// let mut kalman = Kalman1D::new(dt, u, std_dev_a, std_dev_m); 141 | /// let measurements = vec![1.0, 2.0, 3.0, 4.0, 5.0]; 142 | /// for x in measurements { 143 | /// kalman.predict(); 144 | /// kalman.update(x).unwrap(); // assuming that there is noise in measurement 145 | /// } 146 | /// ``` 147 | pub fn update(&mut self, _z: f32) -> Result<(), Kalman1DError> { 148 | // Ref.: Eq.(7) 149 | let gain = match (self.H*self.P*self.H.transpose() + self.R).try_inverse() { 150 | Some(inv) => self.P*self.H.transpose()*inv, 151 | None => return Err(Kalman1DError{typ: 1}), 152 | }; 153 | // Ref.: Eq.(8) 154 | let z = nalgebra::SMatrix::::new(_z); 155 | let r = z - self.H*self.x; 156 | // Ref.: Eq.(9) 157 | self.x = self.x + gain*r; 158 | // Ref.: Eq.(10) 159 | self.P = (I - gain*self.H)*self.P; 160 | Ok(()) 161 | } 162 | /// Returns the current state (only X, not Vx) 163 | pub fn get_state(&self) -> f32 { 164 | self.x[0] 165 | } 166 | /// Returns the current state (both X and Vx) 167 | pub fn get_vector_tate(&self) -> nalgebra::SVector:: { 168 | self.x 169 | } 170 | } 171 | 172 | fn float_loop(start: f32, threshold: f32, step_size: f32) -> impl Iterator { 173 | std::iter::successors(Some(start), move |&prev| { 174 | let next = prev + step_size; 175 | (next < threshold).then_some(next) 176 | }) 177 | } 178 | 179 | #[cfg(test)] 180 | mod tests { 181 | use super::*; 182 | use rand::prelude::*; 183 | use rand::distributions::Standard; 184 | #[test] 185 | fn test_1d_kalman() { 186 | // Just and adoptation of https://machinelearningspace.com/object-tracking-python/ 187 | let dt = 0.1; 188 | let u = 2.0; 189 | let std_dev_a = 0.25; 190 | let std_dev_m = 1.2; 191 | 192 | let t: nalgebra::SVector:: = nalgebra::SVector::::from_iterator(float_loop(0.0, 100.0, dt)); 193 | // let t: = (0..100).map(|t| t as f32).collect(); 194 | let track = t.map(|t| dt*(t*t - t)); 195 | 196 | let mut kalman = Kalman1D::new(dt, u, std_dev_a, std_dev_m); 197 | let mut measurement: Vec = vec![]; 198 | let mut predictions: Vec= vec![]; 199 | for (t, x) in t.iter().zip(track.iter()) { 200 | 201 | // Add some noise to perfect track 202 | let v: f32 = StdRng::from_entropy().sample::(Standard) * (50.0+50.0) - 50.0; // Generate noise in [-50, 50) 203 | let z = kalman.H.x * x + v; 204 | measurement.push(z); 205 | 206 | // Predict stage 207 | kalman.predict(); 208 | let state = kalman.get_vector_tate(); 209 | predictions.push(state.x); 210 | 211 | // Update stage 212 | kalman.update(z).unwrap(); 213 | } 214 | // println!("time;perfect;measurement;prediction"); 215 | // for i in 0..track.len() { 216 | // println!("{};{};{};{}", t[i], track[i], measurement[i], predictions[i]); 217 | // } 218 | } 219 | } 220 | -------------------------------------------------------------------------------- /src/kalman/kalman_2d.rs: -------------------------------------------------------------------------------- 1 | use std::fmt; 2 | use std::error::Error; 3 | use nalgebra; 4 | 5 | // Error struct for failed `nalgebra` operations 6 | #[derive(Debug)] 7 | pub struct Kalman2DError{typ: u16} 8 | impl fmt::Display for Kalman2DError { 9 | fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { 10 | match self.typ { 11 | 1 => write!(f, "Can inverse matrix"), 12 | _ => write!(f, "Undefined error") 13 | } 14 | } 15 | } 16 | impl Error for Kalman2DError {} 17 | 18 | 19 | // Identity matrix. See the ref. https://en.wikipedia.org/wiki/Identity_matrix 20 | const I: nalgebra::SMatrix:: = nalgebra::SMatrix::::new( 21 | 1.0, 0.0, 0.0, 0.0, 22 | 0.0, 1.0, 0.0, 0.0, 23 | 0.0, 0.0, 1.0, 0.0, 24 | 0.0, 0.0, 0.0, 1.0, 25 | ); 26 | 27 | /// Implementation of Discrete Kalman filter for case when there are two variables: X and Y. 28 | #[derive(Debug, Clone)] 29 | pub struct Kalman2D { 30 | // Single cycle time 31 | dt: f32, 32 | // Control input 33 | u: nalgebra::SMatrix, 34 | // Standart deviation of acceleration 35 | std_dev_a: f32, 36 | // Standart deviation of measurement for X 37 | std_dev_mx: f32, 38 | // Standart deviation of measurement for Y 39 | std_dev_my: f32, 40 | // Transition matrix 41 | A: nalgebra::SMatrix, 42 | // Control matrix 43 | B: nalgebra::SMatrix, 44 | // Transformation (observation) matrix 45 | H: nalgebra::SMatrix, 46 | // Process noise covariance matrix 47 | Q: nalgebra::SMatrix, 48 | // Measurement noise covariance matrix 49 | R: nalgebra::SMatrix, 50 | // Error covariance matrix 51 | P: nalgebra::SMatrix, 52 | // State vector: x, y, vx, vy 53 | x: nalgebra::SVector, 54 | } 55 | 56 | impl Kalman2D { 57 | /// Creates new `Kalman2D` 58 | /// 59 | /// Basic usage: 60 | /// 61 | /// ``` 62 | /// use kalman_rust::kalman::Kalman2D; 63 | /// let dt = 0.1; // Single cycle time 64 | /// let ux = 2.0; // Control input for X 65 | /// let uy = 2.0; // Control input for Y 66 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 67 | /// let std_dev_mx = 1.2; // Standart deviation of measurement for X 68 | /// let std_dev_my = 1.2; // Standart deviation of measurement for Y 69 | /// let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my); 70 | /// ``` 71 | pub fn new(dt: f32, ux: f32, uy: f32, std_dev_a: f32, std_dev_mx: f32, std_dev_my: f32) -> Self { 72 | Kalman2D { 73 | dt, 74 | u: nalgebra::SMatrix::::new( 75 | ux, 76 | uy, 77 | ), 78 | std_dev_a, 79 | std_dev_mx, 80 | std_dev_my, 81 | // Ref.: Eq.(31) 82 | A: nalgebra::SMatrix::::new( 83 | 1.0, 0.0, dt, 0.0, 84 | 0.0, 1.0, 0.0, dt, 85 | 0.0, 0.0, 1.0, 0.0, 86 | 0.0, 0.0, 0.0, 1.0, 87 | ), 88 | // Ref.: Eq.(32) 89 | B: nalgebra::SMatrix::::new( 90 | 0.5 * dt.powi(2), 0.0, 91 | 0.0, 0.5 * dt.powi(2), 92 | dt, 0.0, 93 | 0.0, dt, 94 | ), 95 | // Ref.: Eq.(34) 96 | H: nalgebra::SMatrix::::new( 97 | 1.0, 0.0, 0.0, 0.0, 98 | 0.0, 1.0, 0.0, 0.0, 99 | ), 100 | // Ref.: Eq.(40) 101 | Q: nalgebra::SMatrix::::new( 102 | 0.25 * dt.powi(4), 0.0, 0.5 * dt.powi(3), 0.0, 103 | 0.0, 0.25 * dt.powi(4), 0.0, 0.5 * dt.powi(3), 104 | 0.5 * dt.powi(3), 0.0, dt.powi(2), 0.0, 105 | 0.0, 0.5 * dt.powi(3), 0.0, dt.powi(2), 106 | ) * std_dev_a.powi(2), 107 | // Ref.: Eq.(41) 108 | R: nalgebra::SMatrix::::new( 109 | std_dev_mx.powi(2), 0.0, 110 | 0.0, std_dev_my.powi(2), 111 | ), 112 | P: nalgebra::SMatrix::::new( 113 | 1.0, 0.0, 0.0, 0.0, 114 | 0.0, 1.0, 0.0, 0.0, 115 | 0.0, 0.0, 1.0, 0.0, 116 | 0.0, 0.0, 0.0, 1.0, 117 | ), 118 | x: nalgebra::SVector::::new( 119 | 0.0, 120 | 0.0, 121 | 0.0, 122 | 0.0 123 | ), 124 | } 125 | } 126 | /// Creates new `Kalman2D` with initial state 127 | /// 128 | /// Why is it needed to set the initial state to the actual first observed coordinates of an object (sometimes)? 129 | /// When the first state vector is initialized with zeros, it assumes that the object is at the origin 130 | /// and the filter needs to estimate the position of the object from scratch, which can result in some initial inaccuracies. 131 | /// On the other hand, initializing the first state vector with the actual observed coordinates of the object can provide 132 | /// a more accurate estimate from the beginning, which can improve the overall tracking performance of the filter 133 | /// 134 | /// 135 | /// Basic usage: 136 | /// 137 | /// ``` 138 | /// use kalman_rust::kalman::Kalman2D; 139 | /// let dt = 0.1; // Single cycle time 140 | /// let ux = 2.0; // Control input for X 141 | /// let uy = 2.0; // Control input for Y 142 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 143 | /// let std_dev_mx = 1.2; // Standart deviation of measurement for X 144 | /// let std_dev_my = 1.2; // Standart deviation of measurement for Y 145 | /// let ix = 1.0; // Initial state for X 146 | /// let iy = 5.0; // Initial state for Y 147 | /// let mut kalman = Kalman2D::new_with_state(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my, ix, iy); 148 | /// ``` 149 | pub fn new_with_state(dt: f32, ux: f32, uy: f32, std_dev_a: f32, std_dev_mx: f32, std_dev_my: f32, x: f32, y: f32) -> Self { 150 | Kalman2D { 151 | dt, 152 | u: nalgebra::SMatrix::::new( 153 | ux, 154 | uy, 155 | ), 156 | std_dev_a, 157 | std_dev_mx, 158 | std_dev_my, 159 | // Ref.: Eq.(31) 160 | A: nalgebra::SMatrix::::new( 161 | 1.0, 0.0, dt, 0.0, 162 | 0.0, 1.0, 0.0, dt, 163 | 0.0, 0.0, 1.0, 0.0, 164 | 0.0, 0.0, 0.0, 1.0, 165 | ), 166 | // Ref.: Eq.(32) 167 | B: nalgebra::SMatrix::::new( 168 | 0.5 * dt.powi(2), 0.0, 169 | 0.0, 0.5 * dt.powi(2), 170 | dt, 0.0, 171 | 0.0, dt, 172 | ), 173 | // Ref.: Eq.(34) 174 | H: nalgebra::SMatrix::::new( 175 | 1.0, 0.0, 0.0, 0.0, 176 | 0.0, 1.0, 0.0, 0.0, 177 | ), 178 | // Ref.: Eq.(40) 179 | Q: nalgebra::SMatrix::::new( 180 | 0.25 * dt.powi(4), 0.0, 0.5 * dt.powi(3), 0.0, 181 | 0.0, 0.25 * dt.powi(4), 0.0, 0.5 * dt.powi(3), 182 | 0.5 * dt.powi(3), 0.0, dt.powi(2), 0.0, 183 | 0.0, 0.5 * dt.powi(3), 0.0, dt.powi(2), 184 | ) * std_dev_a.powi(2), 185 | // Ref.: Eq.(41) 186 | R: nalgebra::SMatrix::::new( 187 | std_dev_mx.powi(2), 0.0, 188 | 0.0, std_dev_my.powi(2), 189 | ), 190 | P: nalgebra::SMatrix::::new( 191 | 1.0, 0.0, 0.0, 0.0, 192 | 0.0, 1.0, 0.0, 0.0, 193 | 0.0, 0.0, 1.0, 0.0, 194 | 0.0, 0.0, 0.0, 1.0, 195 | ), 196 | x: nalgebra::SVector::::new( 197 | x, 198 | y, 199 | 0.0, 200 | 0.0 201 | ), 202 | } 203 | } 204 | /// Projects the state and the error covariance ahead 205 | /// Mutates the state vector and the error covariance matrix 206 | /// 207 | /// Basic usage: 208 | /// 209 | /// ``` 210 | /// use kalman_rust::kalman::Kalman2D; 211 | /// let dt = 0.1; // Single cycle time 212 | /// let ux = 2.0; // Control input for X 213 | /// let uy = 2.0; // Control input for Y 214 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 215 | /// let std_dev_mx = 1.2; // Standart deviation of measurement for X 216 | /// let std_dev_my = 1.2; // Standart deviation of measurement for Y 217 | /// let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my); 218 | /// let measurements = vec![(1.0, 2.0), (2.0, 3.0), (3.0, 4.0)]; 219 | /// for x in measurements.iter() { 220 | /// // get measurement 221 | /// kalman.predict(); 222 | /// // then do update 223 | /// } 224 | /// ``` 225 | pub fn predict(&mut self) { 226 | // Ref.: Eq.(5) 227 | self.x = (self.A*self.x) + (self.B*self.u); 228 | // Ref.: Eq.(6) 229 | self.P = self.A*self.P*self.A.transpose() + self.Q; 230 | } 231 | /// Computes the Kalman gain and then updates the state vector and the error covariance matrix 232 | /// Mutates the state vector and the error covariance matrix. 233 | /// 234 | /// Basic usage: 235 | /// 236 | /// ``` 237 | /// use kalman_rust::kalman::Kalman2D; 238 | /// let dt = 0.1; // Single cycle time 239 | /// let ux = 2.0; // Control input for X 240 | /// let uy = 2.0; // Control input for Y 241 | /// let std_dev_a = 0.25; // Standart deviation of acceleration 242 | /// let std_dev_mx = 1.2; // Standart deviation of measurement for X 243 | /// let std_dev_my = 1.2; // Standart deviation of measurement for Y 244 | /// let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my); 245 | /// let measurements = vec![(1.0, 2.0), (2.0, 3.0), (3.0, 4.0)]; 246 | /// for (mx, my) in measurements.iter() { 247 | /// kalman.predict(); 248 | /// kalman.update(*mx, *my).unwrap(); // assuming that there is noise in measurement 249 | /// } 250 | /// ``` 251 | pub fn update(&mut self, _zx: f32, _zy: f32) -> Result<(), Kalman2DError> { 252 | // Ref.: Eq.(7) 253 | let gain = match (self.H*self.P*self.H.transpose() + self.R).try_inverse() { 254 | Some(inv) => self.P*self.H.transpose()*inv, 255 | None => return Err(Kalman2DError{typ: 1}), 256 | }; 257 | // Ref.: Eq.(8) 258 | let z = nalgebra::SMatrix::::new( 259 | _zx, 260 | _zy 261 | ); 262 | let r = z - self.H*self.x; 263 | // Ref.: Eq.(9) 264 | self.x = self.x + gain*r; 265 | // Ref.: Eq.(10) 266 | self.P = (I - gain*self.H)*self.P; 267 | Ok(()) 268 | } 269 | /// Returns the current state (only X and Y, not Vx and Vy) 270 | pub fn get_state(&self) -> (f32, f32) { 271 | (self.x[0], self.x[1]) 272 | } 273 | /// Returns the current state (both (X, Y) and (Vx, Vy)) 274 | pub fn get_vector_tate(&self) -> nalgebra::SVector:: { 275 | self.x 276 | } 277 | } 278 | 279 | mod tests { 280 | use super::*; 281 | #[test] 282 | fn test_2d_kalman() { 283 | let dt = 0.04; // 1/25 = 25 fps - just an example 284 | let ux = 1.0; 285 | let uy = 1.0; 286 | let std_dev_a = 2.0; 287 | let std_dev_mx = 0.1; 288 | let std_dev_my = 0.1; 289 | 290 | // Sample measurements 291 | // Note: in this example Y-axis going from up to down 292 | let xs = vec![311, 312, 313, 311, 311, 312, 312, 313, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 311, 311, 311, 311, 311, 310, 311, 311, 311, 310, 310, 308, 307, 308, 308, 308, 307, 307, 307, 308, 307, 307, 307, 307, 307, 308, 307, 309, 306, 307, 306, 307, 308, 306, 306, 306, 305, 307, 307, 307, 306, 306, 306, 307, 307, 308, 307, 307, 308, 307, 306, 308, 309, 309, 309, 309, 308, 309, 309, 309, 308, 311, 311, 307, 311, 307, 313, 311, 307, 311, 311, 306, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312, 312]; 293 | let ys = vec![5, 6, 8, 10, 11, 12, 12, 13, 16, 16, 18, 18, 19, 19, 20, 20, 22, 22, 23, 23, 24, 24, 28, 30, 32, 35, 39, 42, 44, 46, 56, 58, 70, 60, 52, 64, 51, 70, 70, 70, 66, 83, 80, 85, 80, 98, 79, 98, 61, 94, 101, 94, 104, 94, 107, 112, 108, 108, 109, 109, 121, 108, 108, 120, 122, 122, 128, 130, 122, 140, 122, 122, 140, 122, 134, 141, 136, 136, 154, 155, 155, 150, 161, 162, 169, 171, 181, 175, 175, 163, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178, 178]; 294 | 295 | // Assume that initial X,Y coordinates match the first measurement 296 | let ix = xs[0] as f32; // Initial state for X 297 | let iy = ys[0] as f32; // Initial state for Y 298 | let mut kalman = Kalman2D::new_with_state(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my, ix, iy); 299 | kalman.x.x = xs[0] as f32; 300 | kalman.x.y = ys[0] as f32; 301 | let mut predictions: Vec> = vec![]; 302 | let mut updated_states: Vec> = vec![]; 303 | for (x, y) in xs.iter().zip(ys.iter()) { 304 | // Considering that the measurements are noisy 305 | let mx = *x as f32; 306 | let my = *y as f32; 307 | 308 | // Predict stage 309 | kalman.predict(); 310 | let state = kalman.get_vector_tate(); 311 | predictions.push(vec![state.x, state.y]); 312 | 313 | // Update stage 314 | kalman.update(mx, my).unwrap(); 315 | let updated_state = kalman.get_vector_tate(); 316 | updated_states.push(vec![updated_state.x, updated_state.y]); 317 | } 318 | 319 | // println!("measurement X;measurement Y;prediction X;prediction Y;updated X;updated Y"); 320 | // for i in 0..xs.len() { 321 | // println!("{};{};{};{};{};{}", xs[i], ys[i], predictions[i][0], predictions[i][1], updated_states[i][0], updated_states[i][1]); 322 | // } 323 | } 324 | } 325 | 326 | -------------------------------------------------------------------------------- /src/kalman/mod.rs: -------------------------------------------------------------------------------- 1 | //! Export contents of `kalman` folder 2 | mod kalman_1d; 3 | mod kalman_2d; 4 | 5 | pub use self::{ 6 | kalman_1d::*, 7 | kalman_2d::* 8 | }; -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | //! Export library 2 | pub mod kalman; --------------------------------------------------------------------------------