Kalman Filter

Kalman Filtering of the distance sensor

The Kalman Filter is an efficient algorithm used to estimate the state of a dynamic system from a series of noisy measurements. It is widely used in various fields, including control systems, robotics, computer vision, and navigation, to obtain accurate and reliable estimates of the system's true state even in the presence of measurement noise and uncertainty.

The Kalman Filter works by combining two sources of information:

1. Prediction: The filter predicts the current state of the system based on the previous state and the system's dynamics model.

2. Measurement Update: The filter incorporates the new measurement data and corrects the predicted state based on the measurement's accuracy and the state's uncertainty.

credits to kalmanfilter.net

Key Values in the Kalman Filter:

1. x (State Estimate):

   • x represents the current best estimate of the true state of the dynamic system being tracked or estimated by the Kalman Filter. It is a vector containing the state variables, such as position, velocity, orientation, etc., depending on the application. The prediction step uses the previous state estimate, and the measurement update step refines this estimate using the new measurement data.

2. P (Covariance Matrix):

   • P is the covariance matrix that represents the uncertainty or error in the state estimate (x). It describes how much each state variable is uncertain. P is typically an n x n matrix, where n is the number of state variables being estimated. The prediction step updates the covariance matrix to account for the process noise, while the measurement update step adjusts it based on the Kalman Gain.

3. u (Control Input):

   • u represents the control input or external force applied to the system. In some applications, the Kalman Filter may consider the effect of external forces that can be directly measured or estimated to improve state estimation accuracy. The control input is incorporated into the state transition equation during the prediction step.

4. z (Measurement):

   • z represents the actual measurement obtained from sensors or measurements from the environment. It may include noisy or imperfect readings of the true state of the system. The Kalman Filter uses this measurement to update the state estimate during the measurement update step.

5. Q (Process Noise Covariance):

   • Q is the covariance matrix that represents the uncertainty or error in the system dynamics model (process noise). It accounts for unpredicted disturbances or noise in the system, which the filter assumes to be Gaussian. The prediction step modifies the covariance matrix to account for the process noise.

6. R (Measurement Noise Covariance):

   • R is the covariance matrix that represents the uncertainty or error in the measurements obtained from sensors (measurement noise). It accounts for the noise present in the measurements and is typically provided by the sensor manufacturer or determined experimentally. The measurement update step adjusts the covariance matrix based on the measurement noise.

7. K (Kalman Gain):

   • K is the Kalman Gain, which is a crucial factor in the measurement update step. It determines how much the state estimate (x) is adjusted based on the new measurement (z) and is computed using the covariance matrices P and R. The Kalman Gain balances the impact of the predicted state and the measurement to achieve optimal state estimation.

By applying the Kalman Filter through prediction and measurement update steps (doing it again and again and again...), the state estimate becomes increasingly accurate and reliable (stabilizes) over time, even in the presence of noise and uncertainty. The filter's ability to handle noisy measurements and estimate the system's true state makes it a powerful tool in a wide range of applications.

The Kalman Filter involves several mathematical concepts and operations to estimate the state of a dynamic system from noisy measurements. Here's a comprehensive explanation of the math involved:

State Transition Equation:

• The state transition equation represents how the system evolves over time. It is typically expressed as a linear equation, such as: x(k) = F * x(k-1) + B * u(k) + w(k) where:

  • x(k) is the state vector at time step k.

  • F is the state transition matrix that describes how the state evolves from one time step to the next.

  • B is the control input matrix that represents the impact of external control inputs u(k) on the state.

  • u(k) is the control input vector applied to the system at time step k.

  • w(k) is the process noise vector representing the uncertainty in the system dynamics and is assumed to have a Gaussian distribution.

$$x(k) = F * x(k-1) + B * u(k) + w(k)$$

State Prediction:

• The prediction step estimates the state at the current time step based on the previous state and the state transition equation: x̂(k|k-1) = F * x̂(k-1|k-1) + B * u(k) where:

  • x̂(k|k-1) is the predicted state estimate at time step k based on the previous state estimate x̂(k-1|k-1).

  • F is the state transition matrix as mentioned earlier.

  • B is the control input matrix as mentioned earlier.

  • u(k) is the control input vector at time step k.

$$x̂(k|k-1) = F * x̂(k-1|k-1) + B * u(k)$$

State Covariance Prediction:

• The covariance matrix P represents the uncertainty in the state estimate. It evolves over time as the system is subject to process noise: P(k|k-1) = F * P(k-1|k-1) * F^T + Q where:

  • P(k|k-1) is the predicted covariance matrix at time step k based on the previous covariance matrix P(k-1|k-1).

  • F is the state transition matrix as mentioned earlier.

  • Q is the process noise covariance matrix representing the uncertainty in the state transition.

$$P(k|k-1) = F * P(k-1|k-1) * F^T + Q$$

Measurement Equation:

• The measurement equation relates the actual measurements z(k) to the true state x(k). It is typically represented as a linear equation, such as: z(k) = H * x(k) + v(k) where:

  • z(k) is the measurement vector obtained from sensors at time step k.

  • H is the measurement matrix that maps the state to the measurement space.

  • v(k) is the measurement noise vector representing the uncertainty in the measurements and is assumed to have a Gaussian distribution.

$$z(k) = H * x(k) + v(k)$$

Kalman Gain:

• The Kalman Gain (K) determines the weight given to the predicted state estimate and the actual measurement during the measurement update step. It is calculated to minimize the error in the state estimate: K(k) = P(k|k-1) * H^T * (H * P(k|k-1) * H^T + R)^(-1) where:

  • K(k) is the Kalman Gain at time step k.

  • P(k|k-1) is the predicted covariance matrix as calculated in the state covariance prediction step.

  • H is the measurement matrix as mentioned earlier.

  • R is the measurement noise covariance matrix representing the uncertainty in the measurements.

$$K(k) = P(k|k-1) * H^T * (H * P(k|k-1) * H^T + R)^-1$$

State Update:

• The state update step combines the predicted state estimate and the actual measurement to obtain an improved state estimate: x̂(k|k) = x̂(k|k-1) + K(k) * (z(k) - H * x̂(k|k-1)) where:

  • x̂(k|k) is the updated state estimate at time step k.

  • x̂(k|k-1) is the predicted state estimate as calculated in the state prediction step.

  • K(k) is the Kalman Gain as calculated in the Kalman Gain step.

  • z(k) is the measurement vector obtained from sensors at time step k.

$$x̂(k|k) = x̂(k|k-1) + K(k) * (z(k) - H * x̂(k|k-1))$$

Covariance Update:

• The covariance matrix is updated to account for the new information from the measurement: P(k|k) = (I - K(k) * H) * P(k|k-1) where:

  • P(k|k) is the updated covariance matrix at time step k.

  • K(k) is the Kalman Gain as calculated earlier.

  • H is the measurement matrix as mentioned earlier.

$$P(k|k) = (I - K(k) * H) * P(k|k-1)$$

By iteratively performing the prediction and measurement update steps, the Kalman Filter provides increasingly accurate and reliable estimates of the system's true state over time, even in the presence of noise and uncertainty in the measurements and system dynamics. The Kalman Filter's effectiveness lies in its ability to optimally combine prediction and measurement information to produce a more accurate state estimate. This is also explained by ctrlaltftc where you'll learn more of the math behind it! lets start with downloading the API (its super simple and only for this project like the others)for the kalman filter implementation, Ill show you how to use it later on! Remember to create a new class after importing this to the same directory as your main

and in the new class you can start coding NOTE : Kalman is not ONLY used for Distance Sensors...

4KB

KalmanFilter.java

Kalman Filter API

double initvalue = 0;
double filteredvalue = 0;
KalmanFilter kalmanFilter = new KalmanFilter(5, 100, 0, 0, 1, 1, KalmanFilter.calculateK(1, 1));

This created the initial value and the filtered value where the initial value will be your sensor input but in this case I decided to use a random number from 1 - 10 the kalman filter values are going to be given to you for distance sensor output below, filtered value will be equivelent to whatever we return.

// DONT USE THIS AS CODE
// DONT USE THIS AS CODE
// DONT USE THIS AS CODE
// DONT USE THIS AS CODE
x = 10   // Initial distance estimate
P(0) = 1.0   // Initial uncertainty in the distance estimate

H = [1]   // Measurement Matrix

F = [1]   // State Transition Matrix

Q = [0.01]   // Process Noise Covariance Matrix (Adjust this value based on sensor motion uncertainty)

R = [0.1]   // Measurement Noise Covariance Matrix (Adjust this value based on sensor measurement uncertainty)

B = []   // Control Input Matrix
u = 0   // Control Input (Assuming no external control input)

Z = 10 // Actual distance measurement obtained from the sensor
Initial State:
x(0) = [10.0]   // Initial state vector

these are rough estamites you have to fine tune these according to your sensor! Remember you will use them like this

KalmanFilter kalmanFilter = new KalmanFilter(10, 1, 0, 10, 0.01, 0.1, KalmanFilter.calculateK(1, 1));

remember its (x, p, u, z, q, r, k) as the kalman filter values on the function above

while(opModeIsActive){
    initvalue = DISTANCESENSORVALUE; // unit dose not matter
    filteredvalue = kalmanFilter.update(initvalue); // this is the filtered val
    telemetry.addData("x: ",kalmanFilter.getX()); // use telemetry.addData
    telemetry.addData("p: ",kalmanFilter.getP());
    telemetry.addData("u: ",kalmanFilter.getU());
    telemetry.addData("z: ",kalmanFilter.getZ());
    telemetry.addData("Q: ",kalmanFilter.getQ());
    telemetry.addData("R: ",kalmanFilter.getR());
    telemetry.addData("K: ",kalmanFilter.getK());
    telemetry.addData("x_previous: ",kalmanFilter.getX_previous());
    telemetry.addData("p_previous: ",kalmanFilter.getP_previous());
    telemetry.addData("initvalue: ",initvalue);
    telemetry.addData("filteredvalue: ",filteredvalue);
}

remember to use FTC dashboard to plot the telemetry values of initvalue and filteredvalue.