Struct Kalman2D

Source

pub struct Kalman2D { /* private fields */ }

Expand description

Implementation of Discrete Kalman filter for case when there are two variables: X and Y.

Implementations§

Source §

impl Kalman2D

Source

pub fn new( dt: f32, ux: f32, uy: f32, std_dev_a: f32, std_dev_mx: f32, std_dev_my: f32, ) -> Self

Creates new Kalman2D

Basic usage:

use kalman_rust::kalman::Kalman2D;
let dt = 0.1; // Single cycle time
let ux = 2.0; // Control input for X
let uy = 2.0; // Control input for Y
let std_dev_a = 0.25; // Standart deviation of acceleration
let std_dev_mx = 1.2; // Standart deviation of measurement for X
let std_dev_my = 1.2; // Standart deviation of measurement for Y
let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my);

Source

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

Creates new Kalman2D with initial state

Why is it needed to set the initial state to the actual first observed coordinates of an object (sometimes)? When the first state vector is initialized with zeros, it assumes that the object is at the origin and the filter needs to estimate the position of the object from scratch, which can result in some initial inaccuracies. On the other hand, initializing the first state vector with the actual observed coordinates of the object can provide a more accurate estimate from the beginning, which can improve the overall tracking performance of the filter

Basic usage:

use kalman_rust::kalman::Kalman2D;
let dt = 0.1; // Single cycle time
let ux = 2.0; // Control input for X
let uy = 2.0; // Control input for Y
let std_dev_a = 0.25; // Standart deviation of acceleration
let std_dev_mx = 1.2; // Standart deviation of measurement for X
let std_dev_my = 1.2; // Standart deviation of measurement for Y
let ix = 1.0; // Initial state for X
let iy = 5.0; // Initial state for Y
let mut kalman = Kalman2D::new_with_state(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my, ix, iy);

Source

pub fn predict(&mut self)

Projects the state and the error covariance ahead Mutates the state vector and the error covariance matrix

Basic usage:

use kalman_rust::kalman::Kalman2D;
let dt = 0.1; // Single cycle time
let ux = 2.0; // Control input for X
let uy = 2.0; // Control input for Y
let std_dev_a = 0.25; // Standart deviation of acceleration
let std_dev_mx = 1.2; // Standart deviation of measurement for X
let std_dev_my = 1.2; // Standart deviation of measurement for Y
let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my);
let measurements = vec![(1.0, 2.0), (2.0, 3.0), (3.0, 4.0)];
for x in measurements.iter() {
    // get measurement
    kalman.predict();
    // then do update
}

Source

pub fn update(&mut self, _zx: f32, _zy: f32) -> Result<(), Kalman2DError>

Computes the Kalman gain and then updates the state vector and the error covariance matrix Mutates the state vector and the error covariance matrix.

Basic usage:

use kalman_rust::kalman::Kalman2D;
let dt = 0.1; // Single cycle time
let ux = 2.0; // Control input for X
let uy = 2.0; // Control input for Y
let std_dev_a = 0.25; // Standart deviation of acceleration
let std_dev_mx = 1.2; // Standart deviation of measurement for X
let std_dev_my = 1.2; // Standart deviation of measurement for Y
let mut kalman = Kalman2D::new(dt, ux, uy, std_dev_a, std_dev_mx, std_dev_my);
let measurements = vec![(1.0, 2.0), (2.0, 3.0), (3.0, 4.0)];
for (mx, my) in measurements.iter() {
    kalman.predict();
    kalman.update(*mx, *my).unwrap(); // assuming that there is noise in measurement
}