adskalman-rs

https://github.com/strawlab/adskalman-rs
Admin
A Rust implementation of Kalman filter and Rauch-Tung-Striebel smoothing that supports...
Tokens:6,723
Snippets:45
Trust Score:9.5
Update:6 months ago
Show doc for...
Tokens:
Raw
### Run Online Tracking Example

Source: https://github.com/strawlab/adskalman-rs/blob/main/examples/README.md

To run the online tracking example, navigate to the examples directory and execute the cargo command.

```bash
cd examples
cargo run --bin online_tracking
```

--------------------------------

### Run Offline Smoothing Example (Windows)

Source: https://github.com/strawlab/adskalman-rs/blob/main/examples/README.md

Compile and run the offline smoothing example on Windows, using Out-File to redirect output to results.csv.

```powershell
cargo run --bin offline_smoothing | Out-File -FilePath results.csv -Encoding utf8
```

--------------------------------

### Run Offline Smoothing Example (Linux/Mac)

Source: https://github.com/strawlab/adskalman-rs/blob/main/examples/README.md

Compile and run the offline smoothing example on Linux or Mac systems, redirecting output to results.csv.

```bash
cargo run --bin offline_smoothing > results.csv
```

--------------------------------

### Plot Results CSV

Source: https://github.com/strawlab/adskalman-rs/blob/main/examples/README.md

View the results of the offline smoothing example by plotting the generated CSV file using Python with Pandas and Matplotlib.

```python
python .\plot_csv.py .\results.csv
```

--------------------------------

### Implement Non-Linear Observation Model for EKF

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Implement `ObservationModel` with a custom `predict_observation()` method and re-linearize the Jacobian matrix `H` at each timestep around the current state estimate for Extended Kalman Filter (EKF) functionality. This example demonstrates a non-linear observation model `z = [x³, x*y]`.

```rust
use adskalman::{KalmanFilterNoControl, ObservationModel, StateAndCovariance};
use nalgebra::{Matrix2, Matrix2x4, Matrix4x2, Vector2, Vector4, U2, U4};

// Non-linear observation: z = [x³, x*y]
struct NonlinearObservationModel;

impl NonlinearObservationModel {
    fn linearize_at(&self, state: &Vector4<f64>) -> LinearizedObservationModel {
        // Jacobian of [x³, x*y] with respect to [x, y, vx, vy]
        let observation_matrix = Matrix2x4::<f64>::new(
            3.0 * state.x * state.x, 0.0, 0.0, 0.0,  // d(x³)/dx = 3x²
            state.y, state.x, 0.0, 0.0,               // d(xy)/dx = y, d(xy)/dy = x
        );
        let observation_noise_covariance = Matrix2::<f64>::new(0.01, 0.0, 0.0, 0.01);

        LinearizedObservationModel {
            state: state.clone(),
            observation_matrix,
            observation_matrix_transpose: observation_matrix.transpose(),
            observation_noise_covariance,
        }
    }
}

struct LinearizedObservationModel {
    state: Vector4<f64>,
    observation_matrix: Matrix2x4<f64>,
    observation_matrix_transpose: Matrix4x2<f64>,
    observation_noise_covariance: Matrix2<f64>,
}

impl ObservationModel<f64, U4, U2> for LinearizedObservationModel {
    fn H(&self) -> &Matrix2x4<f64> { &self.observation_matrix }
    fn HT(&self) -> &Matrix4x2<f64> { &self.observation_matrix_transpose }
    fn R(&self) -> &Matrix2<f64> { &self.observation_noise_covariance }

    fn predict_observation(&self, state: &Vector4<f64>) -> Vector2<f64> {
        Vector2::new(state.x * state.x * state.x, state.x * state.y)
    }
}

// EKF loop: re-linearize at each step
let motion_model = ConstantVelocity2DModel::new(0.01, 100.0);
let nonlinear_obs = NonlinearObservationModel;
let mut estimate = StateAndCovariance::new(
    Vector4::new(0.0, 0.0, 10.0, -5.0),
    Matrix4::identity() * 0.1,
);

let observations = vec![Vector2::new(0.001, 0.0), Vector2::new(0.008, 0.001)];
for obs in &observations {
    // Linearize around current estimate
    let linear_obs = nonlinear_obs.linearize_at(estimate.state());
    let kf = KalmanFilterNoControl::new(&motion_model, &linear_obs);
    estimate = kf.step(&estimate, obs).expect("EKF step failed");
}
```

--------------------------------

### Handle Kalman Filter Errors

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

The library defines `adskalman::Error` for filter operations. The primary error is `CovarianceNotPositiveSemiDefinite`, which occurs when the covariance matrix becomes non-positive-definite or is asymmetric. This example shows how to handle this error using a `match` statement.

```rust
use adskalman::{KalmanFilterNoControl, StateAndCovariance, Error};
use nalgebra::{Matrix4, Vector2, Vector4};

let kf = KalmanFilterNoControl::new(&motion_model, &observation_model);
let estimate = StateAndCovariance::new(
    Vector4::new(0.0, 0.0, 10.0, -5.0),
    Matrix4::identity() * 0.1,
);
let observation = Vector2::new(0.1, 0.05);

match kf.step(&estimate, &observation) {
    Ok(new_estimate) => {
        println!("Success: {:?}", new_estimate.state());
    }
    Err(Error::CovarianceNotPositiveSemiDefinite) => {
        eprintln!("Covariance matrix became invalid - consider using JosephForm");
        // Recovery: reset covariance or use more robust update method
    }
}
```

--------------------------------

### Configure Covariance Update Methods in Rust

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Demonstrates using step_with_options to select between JosephForm, OptimalKalman, and OptimalKalmanForcedSymmetric update methods.

```rust
use adskalman::{KalmanFilterNoControl, StateAndCovariance, CovarianceUpdateMethod};
use nalgebra::{Vector2, Vector4, Matrix4};

let kf = KalmanFilterNoControl::new(&motion_model, &observation_model);
let estimate = StateAndCovariance::new(
    Vector4::<f64>::new(0.0, 0.0, 10.0, -5.0),
    Matrix4::<f64>::identity() * 0.1,
);
let observation = Vector2::<f64>::new(0.1, 0.05);

// Use JosephForm for numerical stability (recommended)
let result_joseph = kf.step_with_options(
    &estimate,
    &observation,
    CovarianceUpdateMethod::JosephForm,
).unwrap();

// Use OptimalKalman for speed when numerical precision is less critical
let result_optimal = kf.step_with_options(
    &estimate,
    &observation,
    CovarianceUpdateMethod::OptimalKalman,
).unwrap();

// OptimalKalmanForcedSymmetric: fast with symmetry enforcement
let result_symmetric = kf.step_with_options(
    &estimate,
    &observation,
    CovarianceUpdateMethod::OptimalKalmanForcedSymmetric,
).unwrap();
```

--------------------------------

### Initialize StateAndCovariance

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Create and manipulate a 4D state estimate container using nalgebra matrices.

```rust
use adskalman::StateAndCovariance;
use nalgebra::{Matrix4, Vector4};

// Create a 4D state estimate with position (x, y) and velocity (vx, vy)
let state = Vector4::<f64>::new(0.0, 0.0, 10.0, -5.0);

// Initial covariance matrix (4x4 diagonal)
let covariance = Matrix4::<f64>::new(
    0.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,
);

let estimate = StateAndCovariance::new(state, covariance);

// Access state and covariance
println!("State: {:?}", estimate.state());
println!("Covariance: {:?}", estimate.covariance());

// Get mutable references for in-place modification
let mut estimate = estimate;
estimate.state_mut()[0] = 1.0;

// Decompose into inner components
let (state_vec, covar_mat) = estimate.inner();
```

--------------------------------

### Perform Kalman Filtering in Rust

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Initializes a KalmanFilterNoControl instance and performs a single-step update. Assumes motion and observation models are already defined.

```rust
use adskalman::{KalmanFilterNoControl, StateAndCovariance, CovarianceUpdateMethod};
use nalgebra::{Matrix4, Vector2, Vector4};

// Assuming motion_model and observation_model are defined as above
let dt = 0.01;
let motion_model = ConstantVelocity2DModel::new(dt, 100.0);
let observation_model = PositionObservationModel::new(0.01);

// Create the Kalman filter
let kf = KalmanFilterNoControl::new(&motion_model, &observation_model);

// Initial state estimate
let initial_state = Vector4::<f64>::new(0.0, 0.0, 10.0, -5.0);
let initial_covariance = Matrix4::<f64>::new(
    0.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,
);
let mut estimate = StateAndCovariance::new(initial_state, initial_covariance);

// Online tracking: process observations one at a time
let observation = Vector2::<f64>::new(0.12, -0.05);
estimate = kf.step(&estimate, &observation).expect("Filter step failed");
println!("Updated state: {:?}", estimate.state());
```

--------------------------------

### Configure log level for release builds

Source: https://github.com/strawlab/adskalman-rs/blob/main/README.md

Add this to your Cargo.toml to limit log trace output in release builds.

```toml
[dependencies]
log = { version = "0.4", features = ["release_max_level_debug"] }
```

--------------------------------

### Implement TransitionModelLinearNoControl

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Define a linear process model by implementing the TransitionModelLinearNoControl trait for a constant velocity 2D system.

```rust
use adskalman::TransitionModelLinearNoControl;
use nalgebra::{OMatrix, U4, convert};

// Constant velocity 2D model: state = [x, y, vx, vy]
pub struct ConstantVelocity2DModel {
    pub transition_model: OMatrix<f64, U4, U4>,
    pub transition_model_transpose: OMatrix<f64, U4, U4>,
    pub transition_noise_covariance: OMatrix<f64, U4, U4>,
}

impl ConstantVelocity2DModel {
    pub fn new(dt: f64, noise_scale: f64) -> Self {
        // State transition: x' = x + vx*dt, y' = y + vy*dt
        let transition_model = OMatrix::<f64, U4, U4>::new(
            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,
        );

        // Process noise covariance (from continuous white noise acceleration model)
        let t33 = dt * dt * dt / 3.0;
        let t22 = dt * dt / 2.0;
        let transition_noise_covariance = OMatrix::<f64, U4, U4>::new(
            t33,  0.0,  t22,  0.0,
            0.0,  t33,  0.0,  t22,
            t22,  0.0,  dt,   0.0,
            0.0,  t22,  0.0,  dt,
        ) * noise_scale;

        Self {
            transition_model,
            transition_model_transpose: transition_model.transpose(),
            transition_noise_covariance,
        }
    }
}

impl TransitionModelLinearNoControl<f64, U4> for ConstantVelocity2DModel {
    fn F(&self) -> &OMatrix<f64, U4, U4> { &self.transition_model }
    fn FT(&self) -> &OMatrix<f64, U4, U4> { &self.transition_model_transpose }
    fn Q(&self) -> &OMatrix<f64, U4, U4> { &self.transition_noise_covariance }
}
```

--------------------------------

### Implement ObservationModel Trait in Rust

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Defines a linear observation model by implementing the ObservationModel trait. Requires nalgebra matrices for the observation matrix, transpose, and noise covariance.

```rust
use adskalman::ObservationModel;
use nalgebra::{OMatrix, OVector, U2, U4, DimMin};

// Linear observation model: observe only position from [x, y, vx, vy]
pub struct PositionObservationModel {
    pub observation_matrix: OMatrix<f64, U2, U4>,
    pub observation_matrix_transpose: OMatrix<f64, U4, U2>,
    pub observation_noise_covariance: OMatrix<f64, U2, U2>,
}

impl PositionObservationModel {
    pub fn new(variance: f64) -> Self {
        // H extracts position: [x, y] from [x, y, vx, vy]
        let observation_matrix = OMatrix::<f64, U2, U4>::new(
            1.0, 0.0, 0.0, 0.0,
            0.0, 1.0, 0.0, 0.0,
        );
        let observation_noise_covariance = OMatrix::<f64, U2, U2>::new(
            variance, 0.0,
            0.0, variance,
        );

        Self {
            observation_matrix,
            observation_matrix_transpose: observation_matrix.transpose(),
            observation_noise_covariance,
        }
    }
}

impl ObservationModel<f64, U4, U2> for PositionObservationModel {
    fn H(&self) -> &OMatrix<f64, U2, U4> { &self.observation_matrix }
    fn HT(&self) -> &OMatrix<f64, U4, U2> { &self.observation_matrix_transpose }
    fn R(&self) -> &OMatrix<f64, U2, U2> { &self.observation_noise_covariance }
}
```

--------------------------------

### KalmanFilterNoControl Batch Filtering in Rust

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Processes a time series of observations at once. Handles missing data (NaN components) by skipping the update step for that timestep. Requires initialization with a KalmanFilterNoControl instance and an initial state estimate.

```rust
use adskalman::{KalmanFilterNoControl, StateAndCovariance};
use nalgebra::{Matrix4, Vector2, Vector4};

let kf = KalmanFilterNoControl::new(&motion_model, &observation_model);
let initial_estimate = StateAndCovariance::new(
    Vector4::<f64>::new(0.0, 0.0, 10.0, -5.0),
    Matrix4::<f64>::identity() * 0.1,
);

// Batch of observations (simulated noisy position measurements)
let observations = vec![
    Vector2::<f64>::new(0.10, -0.05),
    Vector2::<f64>::new(0.21, -0.10),
    Vector2::<f64>::new(0.30, -0.14),
    Vector2::<f64>::new(f64::NAN, f64::NAN),  // Missing observation
    Vector2::<f64>::new(0.52, -0.25),
];

// Run batch filter
let estimates = kf.filter(&initial_estimate, &observations)
    .expect("Filtering failed");

// Extract estimated positions
for (i, est) in estimates.iter().enumerate() {
    println!("t={}: pos=({:.3}, {:.3}), vel=({:.3}, {:.3})",
        i, est.state()[0], est.state()[1], est.state()[2], est.state()[3]);
}
```

--------------------------------

### KalmanFilterNoControl RTS Smoothing in Rust

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Performs Rauch-Tung-Striebel (RTS) smoothing using both past and future observations for optimal state estimates. It involves a forward filtering pass and a backward smoothing pass. Smoothed estimates typically have lower covariance than filtered estimates.

```rust
use adskalman::{KalmanFilterNoControl, StateAndCovariance};
use nalgebra::{Matrix4, Vector2, Vector4};

let kf = KalmanFilterNoControl::new(&motion_model, &observation_model);
let initial_estimate = StateAndCovariance::new(
    Vector4::<f64>::new(0.0, 0.0, 10.0, -5.0),
    Matrix4::<f64>::identity() * 0.1,
);

let observations = vec![
    Vector2::<f64>::new(0.10, -0.05),
    Vector2::<f64>::new(0.21, -0.10),
    Vector2::<f64>::new(0.30, -0.14),
    Vector2::<f64>::new(0.42, -0.20),
    Vector2::<f64>::new(0.52, -0.25),
];

// RTS smoothing uses all data for optimal estimates
let smoothed_estimates = kf.smooth(&initial_estimate, &observations)
    .expect("Smoothing failed");

// Smoothed estimates have lower covariance than filtered estimates
for (i, est) in smoothed_estimates.iter().enumerate() {
    println!("t={}: smoothed pos=({:.4}, {:.4})", i, est.state()[0], est.state()[1]);
}
```

--------------------------------

### KalmanFilterNoControl::filter - Batch Filtering

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Processes an entire time series of observations at once, returning state estimates for each timestep. Observations with NaN components are treated as missing data and skipped during the update step.

```APIDOC
## KalmanFilterNoControl::filter - Batch Filtering

### Description
Processes an entire time series of observations at once, returning state estimates for each timestep. Observations with NaN components are treated as missing data and skipped during the update step.

### Method
POST (or equivalent for batch processing)

### Endpoint
`/api/kalman/filter` (example endpoint)

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
- **initial_estimate** (StateAndCovariance) - Required - The initial state and covariance of the system.
- **observations** (Array<Vector2<f64>>) - Required - A batch of observations.

### Request Example
```json
{
  "initial_estimate": {
    "state": [0.0, 0.0, 10.0, -5.0],
    "covariance": [[0.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]]
  },
  "observations": [
    [0.10, -0.05],
    [0.21, -0.10],
    [0.30, -0.14],
    [null, null],
    [0.52, -0.25]
  ]
}
```

### Response
#### Success Response (200)
- **estimates** (Array<StateAndCovariance>) - An array of state estimates for each timestep.

#### Response Example
```json
{
  "estimates": [
    {
      "state": [0.09, -0.04, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.20, -0.09, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.29, -0.13, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.29, -0.13, 10.0, -5.0], 
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.51, -0.24, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    }
  ]
}
```
```

--------------------------------

### KalmanFilterNoControl::smooth - RTS Smoothing

Source: https://context7.com/strawlab/adskalman-rs/llms.txt

Performs Rauch-Tung-Striebel (RTS) smoothing, which uses both past and future observations to produce optimal state estimates. It first runs a forward filter pass, then a backward smoothing pass, providing better estimates than filtering alone.

```APIDOC
## KalmanFilterNoControl::smooth - RTS Smoothing

### Description
Performs Rauch-Tung-Striebel (RTS) smoothing, which uses both past and future observations to produce optimal state estimates. It first runs a forward filter pass, then a backward smoothing pass, providing better estimates than filtering alone.

### Method
POST (or equivalent for batch processing)

### Endpoint
`/api/kalman/smooth` (example endpoint)

### Parameters
#### Path Parameters
None

#### Query Parameters
None

#### Request Body
- **initial_estimate** (StateAndCovariance) - Required - The initial state and covariance of the system.
- **observations** (Array<Vector2<f64>>) - Required - A batch of observations.

### Request Example
```json
{
  "initial_estimate": {
    "state": [0.0, 0.0, 10.0, -5.0],
    "covariance": [[0.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]]
  },
  "observations": [
    [0.10, -0.05],
    [0.21, -0.10],
    [0.30, -0.14],
    [0.42, -0.20],
    [0.52, -0.25]
  ]
}
```

### Response
#### Success Response (200)
- **smoothed_estimates** (Array<StateAndCovariance>) - An array of smoothed state estimates for each timestep.

#### Response Example
```json
{
  "smoothed_estimates": [
    {
      "state": [0.0995, -0.0498, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.2090, -0.0990, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.2985, -0.1392, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.4180, -0.1990, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    },
    {
      "state": [0.5175, -0.2488, 10.0, -5.0],
      "covariance": [[...], [...], [...], [...]]
    }
  ]
}
```
```
Tokens:
Raw