Multi-Variable Kalman Filter

Kalman FIlter but better?

State-Space Model:

The Multivariable Kalman Filter works on a "state-space model", which shows the system's dynamic behavior using a set of state variables and state transition equations. In a general form, the state-space model can be represented as follows:

• State Vector (n x 1): x[k] = [x₁[k], x₂[k], ..., xₙ[k]]ᵀ Represents the system's state at time step k. It includes all the variables needed to describe the system's behavior, such as position, velocity, orientation, etc.

$$(n x 1): x[k] = [x₁[k], x₂[k], ..., xₙ[k]]ᵀ$$

• State Transition Matrix (n x n): A Describes how the state evolves over time. It represents the dynamics of the system and is used to predict the state at the next time step.

• Control Input Vector $(m x 1): u[k] = [u₁[k], u₂[k], ..., uₘ[k]]ᵀ$ (optional) Represents external inputs or controls that affect the system's behavior.

• Control Input Matrix (n x m): B (optional) Describes how the control inputs affect the state transition.

• Process Noise Vector (n x 1): w[k] Represents the uncertainty or disturbances in the state transition model. It is assumed to be a zero-mean Gaussian noise with a known covariance matrix.

State Transition Equation: x[k+1] = A * x[k] + B * u[k] + w[k]

$$x[k+1] = A * x[k] + B * u[k] + w[k]$$

Measurement Model:

In addition to the state-space model, the Multivariable Kalman Filter also uses a measurement model to relate sensor measurements to the system's state. The measurement model can be represented as follows:

• Measurement Vector (m x 1): z[k] = [z₁[k], z₂[k], ..., zₘ[k]]ᵀ Represents the sensor measurements at time step k.

$$z[k] = [z₁[k], z₂[k], ..., zₘ[k]]ᵀ$$

• Measurement Matrix (m x n): C Describes how the state variables are related to the sensor measurements.

• Measurement Noise Vector (m x 1): v[k] Represents the measurement noise or errors, assumed to be a zero-mean Gaussian noise with a known covariance matrix.

Measurement Equation: z[k] = C * x[k] + v[k]

$$z[k] = C * x[k] + v[k]$$

Kalman Filter Equations:

The Kalman Filter operates in two main steps: the prediction step (time update) and the correction step (measurement update).

Prediction Step:

1. State Prediction: x̂[k|k-1] = A * x̂[k-1|k-1] + B * u[k]

$$x̂[k|k-1] = A * x̂[k-1|k-1] + B * u[k]$$

• $x̂[k|k-1]$ is the predicted state at time k based on the previous estimate.

• $x̂[k-1|k-1]$is the previous state estimate at time k-1.

1. Error Covariance Prediction: P[k|k-1] = A * P[k-1|k-1] * Aᵀ + Q

$$P[k|k-1] = A * P[k-1|k-1] * Aᵀ + Q$$

• $P[k|k-1]$ is the predicted error covariance matrix at time k based on the previous estimate.

• $P[k-1|k-1]$ is the error covariance matrix of the previous estimate at time k-1.

• Q is the process noise covariance matrix.

Correction Step:

1. Kalman Gain Calculation: K[k] = P[k|k-1] * Cᵀ * (C * P[k|k-1] * Cᵀ + R)⁻¹

$$K[k] = P[k|k-1] * Cᵀ * (C * P[k|k-1] * Cᵀ + R)⁻¹$$

• $K[k]$ is the Kalman Gain at time k.

• R is the measurement noise covariance matrix.

1. State Update: x̂[k|k] = x̂[k|k-1] + K[k] * (z[k] - C * x̂[k|k-1])

$$x̂[k|k] = x̂[k|k-1] + K[k] * (z[k] - C * x̂[k|k-1])$$

• $x̂[k|k]$ is the updated state estimate at time k.

• $z[k]$ is the actual sensor measurement at time k.

1. Error Covariance Update: P[k|k] = (I - K[k] * C) * P[k|k-1]

$$P[k|k] = (I - K[k] * C) * P[k|k-1]$$

• $P[k|k]$ is the updated error covariance matrix at time k.

• I is the identity matrix.

5. Iterative Process:

The Kalman Filter operates in an iterative process (updates itself again and again...), where the prediction and correction steps are repeated for each time step (k) using the updated state estimate $(x̂[k|k])$ and error covariance matrix $(P[k|k])$ from the previous time step.

Advantages:

• Optimal Estimation: The Kalman Filter provides the best linear unbiased estimate of the system state given the measurements and process model.

• Real-Time Capability: It operates in real-time, continuously updating the state estimate based on new measurements.

• Noise Robustness: The filter handles sensor noise and disturbances effectively, providing a stable and accurate state estimation.

Challenges:

• Computational Complexity: Implementing the Kalman Filter, especially for high-dimensional systems, can be computationally intensive.

• Modeling Errors: The filter assumes linearity and Gaussian noise, which may not hold true for all systems.

Alright Enough theory, lets start coding! 🎉

Lets first download the API! NOTE : MVKF can be used many other ways and methods for different reasons what is stated here doesn't mean you are limited to this...

7KB

MultiVariableKalmanFilter.java

MVKF API

this API has a dependency of

org.apache.commons.math3.linear

to import it into your build.dependencies.gradle file do this

dependencies {
    implementation 'org.apache.commons:commons-math3:3.6.1'
}

then rebuild your project

double initialDistanceSensor1 = 10; // init dsensor pos
double initialDistanceSensor2 = 10; // init dsensor pos

// Create the Kalman filter for each sensor
double[][] initialX1 = { { initialDistanceSensor1 },{ initialDistanceSensor2 } }; // Initial state for sensor 1 (distance)
double[][] modelCovariance = { { 0.1 } }; // Model covariance (adjust this based on the system dynamics)
double[][] sensorCovariance = { { 1.0 } }; // Sensor covariance (adjust this based on sensor noise)
double[][] initialCovarianceGuess = { { 0.1 } }; // Initial covariance guess
double[][] initialKalmanGainGuess = { { 0.0 } }; // Initial Kalman gain guess
double[][] controlInput = { { 0.0 } }; // Control input (change in distance, assuming 0 here)
double[][] modelMatrix = { { 1.0 } }; // Model matrix (state transition matrix, assuming 1 here)
double[][] controlMatrix = { { 1.0 } }; // Control matrix (assuming 1 here)
double[][] sensorMatrix = { { 1.0 } }; // Sensor matrix (assuming 1 here)
double[][] identityMatrix = { { 1.0 } }; // Identity matrix (for covariance updates)

this creates all the basic variables that we talked about above. In this project we are going to be implementing a Kalman filter of 2 different distance sensors simultaneously.

MultiVariableKalmanFilter kalmanFilter = new MultiVariableKalmanFilter(
        initialX1, // Both sensors start with their initial distances
        modelCovariance,
        sensorCovariance,
        initialCovarianceGuess,
        initialKalmanGainGuess,
        controlInput,
        initialX1, // Initialize measurement with initial distance for sensor 1
        modelMatrix,
        controlMatrix,
        sensorMatrix,
        identityMatrix
);

here we create a new instance of our API and give in our parameters in this order

while(opModeIsActive){
    double measuredDistanceSensor1 = DSENSORVAL;
    double measuredDistanceSensor2 = DESONSOR2VAL; // unit dose not matter

    // Update the Kalman filter with the measurements from both sensors
    double[][] measurements = { { measuredDistanceSensor1 }, { measuredDistanceSensor2 } };
    kalmanFilter.update(measurements);

    // Get the filtered distance estimates for each sensor
    double[][] filteredState = kalmanFilter.getFilteredValue();

    // Print the filtered distance estimates for each sensor
    telemetry.addData("Distance of Sensor 1: ",DSENSORVAL);
    telemetry.addData("Distance of Sensor 2: ",DSENSOR2VAL);
    telemetry.addData("Filtered Distance Estimate Sensor 1: ",filteredState[0][0]);
    telemetry.addData("Filtered Distance Estimate Sensor 2: ",filteredState[1][0]);
    
}

This has a loop where it measures the values of both sensors and updates the Kalman filter and then gets the values and this will happen until we stop the opmode. The output should look something like this.

Distance of Sensor 1: 9.8
Distance of Sensor 2: 9.9
Filtered Distance Estimate Sensor 1: 9.8
Filtered Distance Estimate Sensor 2: 9.9
Distance of Sensor 1: 10.0
Distance of Sensor 2: 9.8
Filtered Distance Estimate Sensor 1: 9.9
Filtered Distance Estimate Sensor 2: 9.85
Distance of Sensor 1: 10.09
Distance of Sensor 2: 11.5
Filtered Distance Estimate Sensor 1: 10.0
Filtered Distance Estimate Sensor 2: 10.05

Remember to use FTC dashboard to graph you telemetry data 😄

NOTE : this is a simple implementation of MVKF if you want something more advanced look at kalmanfilter.net