Kalman Filter Simulation

A Kalman filter can be used to predict the state of a system where there is a lot of input noise. In this case, my partner and I used it for a class project for our Autonomous Robots class. We predicted the location of a ball as it was kicked towards the robot in an effort to stop the ball. To implement the Kalman filter on the robot, we first created this simulation that allowed us to easily tweak the settings and debug our algorithm.


Your browser does not support the canvas element.

Move your mouse around the screen. The Kalman Filter will reduce input noise and predict your movement.

               Show Prediction:

A Matrix
State Transition
B Matrix
Input Control
H Matrix
Measurement
Q Matrix
Action Uncertainty
R Matrix
Sensor Noise

X-Axis Random Noise:
0
Y-Axis Random Noise:
0
Fade-Out Time (sec):
2.0
Prediction Amount (sec):
1.0



Click on an image to enlarge.

The Kalman Filter estimates the true state of an object given noisy input (input with some inaccuracy). In the case of this simulation, the Kalman Filter estimates the true position of your cursor when there is random input noise. It can also predict the future state using past readings (i.e. the most likely position of your cursor after n seconds).



Below is the Kalman Filter equation. A, B, H, Q, and R are the matrices as defined above. Lowercase variables are vectors, and uppercase variables are matrices. x and P start out as the 0 vector and matrix, respectively. m, the measurement vector, contains the position and velocity readings from the sensor. In the simulation, sensor noise is added by randomly offsetting the actual position values of the cursor. c is the control vector, which would contain estimated changes from direct action commands (for example, if a robot's motor was instructed to move forward). In the simulation, there are no control actuators such as motors, so the control vector and matrix (B) are ignored.


m = [measuredX, measuredY, xVelocity, yVelocity]       ← measurement vector
c = [0, 0, 0, 0]       ← control vector (not used here)

Prediction Step:
x = (A * x) + (B * c)
P = (A * P * AT) + Q       ← AT is the matrix transpose of A

Correction Step:
S = (H * P * HT) + R       ← HT is the matrix transpose of H
K = P * HT * S-1       ← S-1 is the matrix inverse of S
y = m - (H * x)
x = x + (K * y)
P = (I - (K * H)) * P       I is the Identity matrix


If prediction is enabled, the prediction step is looped for n more frames after the above code is executed:

predX = x
predX = (A * predX) + (B * c)


The estimated position of the cursor is in the x vector:

x ← [xPos, yPos, xVel, yVel]


The blue points represent the sensor reading of the cursor's position. If there is no input noise, these points reflect the true position of the cursor. Input noise can be adjusted with the Random Noise sliders. The green line represents the Kalman Filter estimate of the true position. When there is a lot of input noise, the Kalman Filter estimate is much more accurate than a direct reading. If prediction is enabled, the red line shows the predicted path of your movement (how far the prediction goes is adjustable by the Prediction Amount slider).

The five matrices have been preset to work with this simulation. A is the state transition matrix. This matrix influences the measurement vector. B is the control matrix. This matrix influences the control vector (unused in this simulation). H is the measurement matrix. This matrix influences the Kalman Gain. Q is the action uncertainty matrix. This matrix implies the process noise covariance. R is the sensor noise matrix. This matrix implies the measurement error covariance, based on the amount of sensor noise. In this simulation, Q and R are constants, but some implementations of the Kalman Filter may adjust them throughout execution.

We used the Kalman Filter on an Aldebaran NAO humanoid robot as part of a class project. The Kalman Filter helped predict the location of a ball being tracked by the NAO's onboard cameras. The robot acted as an autonomous goalie in a game of soccer, tasked with blocking incoming balls from going into the goal. The Kalman Filter provided a decent estimate of the ball's future location, allowing the NAO to block it in time.



This video is a very good reference to learn more about Kalman Filters. It helped me understand the theory and math. For our class assignment, we used the paper, An Introduction to the Kalman Filter, as a guide to implement the algorithm.

This simulation uses Sylvester, an open source JavaScript library for doing vector and matrix calculations.

The Kalman Filter simulator source code can be viewed here.




Back to Top