Iteratively Saturated Kalman Filter

A Python implementation of the iteratively saturated Kalman filter (ISKF), for state estimation in the presence of outliers in both the measurements and dynamics. This code accompanies the paper: https://stanford.edu/~boyd/papers/iskf.html

The ISKF is about as efficient as the standard Kalman filter (KF), and the steady-state ISKF is about as efficient as the steady-state KF (just matrix-vector multiplies).

To install dependencies, run:

bash pip install -r requirements.txt

Optionally, create a virtual environment first.

Filter Implementations

The iskf.filters module provides various robust Kalman filter implementations:

Basic Filters

• KalmanFilter: Standard Kalman filter
• SteadyKalmanFilter: Steady-state Kalman filter

Huber-based Robust Filters

• HuberKalmanFilter: Huber-based robust Kalman filter
• SteadyHuberKalmanFilter: Steady-state Huber Kalman filter
• IterSatKalmanFilter: Iteratively saturated Kalman filter
• SteadyIterSatKalmanFilter: Steady-state version of ISKF

Iteratively Saturated Kalman Filters (ISKF)

• SteadyOneStepIterSatFilter: Single-step iterative filter
• SteadyTwoStepIterSatFilter: Two-step iterative filter (ISKF with $\tilde k=2$)
• SteadyThreeStepIterSatFilter: Three-term Huber filter

Other Robust Filters

• SteadyRegularizedKalmanFilter: Regularized Kalman filter
• WeightedLikelihoodFilter: Weighted likelihood filter (WoLF)

Filter Usage

All filters inherit from BaseFilter and share a common interface:

```python from iskf.filters import SteadyTwoStepIterSatFilter from iskf.models.vehicle import vehicle_ss

Create system model

systemmodel = vehicless(gamma=0.05, dt=0.05)

Initialize filter

filterinstance = SteadyTwoStepIterSatFilter( systemmodel=systemmodel, covinput=processnoisecov, covmeasurement=measurementnoisecov, coefs=1.345, # Huber parameter for state coefo=1.345, # Huber parameter for observation stepsize=1.0 # Step size for iterations )

Run filter

estimates = filterinstance.estimate( Tout=timevector, Yout=measurements, xinitialestimate=initialstate, Pinitial=initial_covariance ) ```

System Models

The iskf.models module provides system models for simulation and filtering:

Vehicle Model (vehicle_ss)

A 2D vehicle model with position and velocity states.

```python from iskf.models.vehicle import vehicle_ss

Create vehicle model

model = vehicle_ss(gamma=0.05, dt=0.05)

States: [xpos, ypos, xvel, yvel]

Outputs: xpos, ypos

```

CSTR Model (cascaded_cstr_ss)

A cascaded Continuously Stirred Tank Reactor model.

```python from iskf.models.cstr import cascadedcstrss

Create CSTR model with n reactors

model = cascadedcstrss(n=3, dt=0.05)

States: CA1, T1, CA2, T2, C_A3, T3

Outputs: T1, T2, T3

```

Simulation Scripts

Vehicle Simulation (simulate_vehicle.py)

Simulates a 2D vehicle model and saves simulation data for hyperparameter tuning.

```bash

Generate simulation data only

python simulatevehicle.py --randomseed 42 --outlierpercent 10 --numsimulation_steps 1000

Generate data and run a filter

python simulatevehicle.py --randomseed 42 --outlierpercent 10 --filtertype steadytwostep_iskf ```

Key Arguments:

• --random_seed: Random seed for reproducibility (default: 42)
• --outlier_percent: Percentage of outliers in noise (default: 10)
• --num_simulation_steps: Number of simulation steps (default: 1000)
• --filter_type: Optional filter to run immediately
• --filter_kwargs: JSON string of filter parameters

Output: Saves data to results/simulation_data/vehicle_p{outlier_percent}_sp{scale_proc}_sm{scale_meas}_steps{steps}_seed{seed}.pkl

CSTR Simulation (simulate_cstr.py)

Simulates a cascaded CSTR model with 3 reactors.

```bash

Generate CSTR simulation data

python simulatecstr.py --randomseed 0 --outlierpercent 15 --numsimulation_steps 1000

With filter

python simulatecstr.py --outlierpercent 10 --filtertype steadyiskf ```

Output: Saves data to results/simulation_data/cstr_n{n_reactors}_p{outlier_percent}_sp{scale_proc}_sm{scale_meas}_steps{steps}_seed{seed}.pkl

Hyperparameter Tuning

Filter Parameter Tuning (tune_filter.py)

Performs grid search over filter hyperparameters using simulation data. It is recommended to use separate simulation files for fitting and testing to avoid overfitting.

```bash

Tune ISKF (k̃=2) parameters (with separate fit and test data)

python tunefilter.py --filtertype steadytwostepiskf \ --simdatapath results/simulationdata/vehiclep10sp10sm10steps1000seed42.pkl \ --testdatapath results/simulationdata/vehiclep10sp10sm10steps1000_seed0.pkl \ --metric rmse --optimistic

Tune Huber filter (with separate fit and test data)

python tunefilter.py --filtertype steadyhuber \ --simdatapath results/simulationdata/cstrn3p10sp10sm10steps1000seed0.pkl \ --testdatapath results/simulationdata/cstrn3p10sp10sm10steps1000_seed1.pkl ```

Key Arguments:

• --filter_type: Filter to tune (steady_two_step_iskf, steady_huber, wolf, etc.)
• --sim_data_path: Path to simulation data for fitting (required)
• --test_data_path: Path to separate test data for evaluation (recommended)
• --metric: Evaluation metric (rmse, mne, rmedse, etc.)
• --optimistic: Use true states for evaluation (vs. predicted measurements)
• --sweep_resolution: Grid search resolution (default: 15)

Output: Saves results to results/parameter_search_data/{filter_type}_{data_info}_{metric}_results.pkl

Note: For robust evaluation, always use different random seeds for --sim_data_path and --test_data_path to ensure the filter is not tuned and tested on the same data.

Iteration Count Tuning (tune_num_iters.py)

Specifically tunes the number of iterations for iterative filters.

```bash

Tune iteration count for ISKF

python tunenumiters.py \ --simdatapath results/simulationdata/vehiclep10sp10sm10steps1000seed42.pkl \ --metric rmse --optimistic ```

Output: Saves results to results/parameter_search_data/steady_iskf_iter_count_{data_info}_{metric}_results.pkl

Visualization Tools

Vehicle Results Plotting (plot_vehicle_results.py)

Visualizes filter performance on vehicle trajectory data.

```bash

Plot trajectory with tuned filter

python plotvehicleresults.py --tunefilterresults \ results/parametersearchdata/steadytwostepiskfvehiclep10sp10sm10steps1000seed42realisticrmseresults.pkl

Plot iteration sweep results

python plotvehicleresults.py --sweepitersresults \ results/parametersearchdata/steadyiskfitercountvehiclep10sp10sm10steps1000seed42realisticrmseresults.pkl ```

Features:

• Vehicle trajectory plots showing true path, measurements, and filter estimates
• State trajectory plots with error analysis
• Iteration performance plots

CSTR Results Plotting (plot_cstr_results.py)

Visualizes filter performance on CSTR data.

```bash

Plot CSTR results

python plotcstrresults.py --tunefilterresults \ results/parametersearchdata/steadyiskfcstrn3p10sp10sm10steps1000seed0optimisticrmse_results.pkl ```

Features:

• CSTR state trajectories (concentrations and temperatures)
• State error plots
• Comparative performance visualization

Usage Examples

Basic Filter Usage

```python import numpy as np from iskf.models.vehicle import vehicle_ss from iskf.filters import SteadyTwoStepIterSatFilter from iskf.simulator import Simulator

1. Create system model

model = vehicle_ss(gamma=0.05, dt=0.05)

2. Setup simulation with outlier parameters

sim = Simulator( systemmodel=model, processnoisecov=np.eye(2) * 10, measurementnoisecov=np.eye(2) * 5, poutlierprocess=0.1, outlierscaleprocess=10.0, poutliermeasurement=0.1, outlierscale_measurement=10.0 )

3. Generate data

Tout, Ymeas, Xtrue, _ = sim.simulate( x0=np.array([0, 0, 5, 5]), Tfinal=(1000 - 1) * 0.05, # Ensure Tfinal is a multiple of dt numsteps=1000, returnnoiseinputs=True )

4. Create and run filter

filterinstance = SteadyTwoStepIterSatFilter( systemmodel=model, covinput=np.eye(2) * 10, covmeasurement=np.eye(2) * 5, coefs=1.345, coefo=1.345, step_size=1.0 )

Xest = filterinstance.estimate( Tout, Ymeas, xinitialestimate=np.array([0, 0, 5, 5]), P_initial=np.eye(4) )

5. Evaluate performance

from iskf.metrics import rmse error = rmse(Xest, Xtrue[:, 1:]) # Align dimensions print(f"RMSE: {error:.4f}") ```

Available Metrics

The iskf.metrics module provides several evaluation metrics:

• rmse: Root Mean Squared Error
• rmedse: Root Median Squared Error
• mne: Mean Norm Error
• median_ne: Median Norm Error
• max_ne: Maximum Norm Error

Workflow Overview

The typical experimental workflow consists of three main steps:

1. Generate Simulation Data

```bash

Vehicle model

python simulatevehicle.py --outlierpercent 10 --random_seed 42

CSTR model

python simulatecstr.py --outlierpercent 10 --random_seed 0 ```

2. Tune Filter Parameters

```bash

Tune ISKF parameters

python tunefilter.py --filtertype steadytwostepiskf \ --simdatapath results/simulationdata/vehiclep10sp10sm10steps1000_seed42.pkl

Tune iteration count

python tunenumiters.py \ --simdatapath results/simulationdata/vehiclep10sp10sm10steps1000seed42.pkl ```

3. Visualize Results

```bash

Plot tuned filter performance

python plotvehicleresults.py --tunefilterresults \ results/parametersearchdata/steadytwostepiskfvehiclep10sp10sm10steps1000seed42realisticrmseresults.pkl

Plot iteration analysis

python plotvehicleresults.py --sweepitersresults \ results/parametersearchdata/steadyiskfitercountvehiclep10sp10sm10steps1000seed42realisticrmseresults.pkl ```