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.
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 
XAxis Random Noise:
0

YAxis Random Noise:
0

FadeOut Time (sec):
2.0

Prediction Amount (sec):
1.0

Click on an image to enlarge.
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 * A^{T}) + Q ← A^{T} is the matrix transpose of A
Correction Step:
S = (H * P * H^{T}) + R ← H^{T} is the matrix transpose of H
K = P * H^{T} * 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
c = [0, 0, 0, 0] ← control vector (not used here)
Prediction Step:
x = (A * x) + (B * c)
P = (A * P * A^{T}) + Q ← A^{T} is the matrix transpose of A
Correction Step:
S = (H * P * H^{T}) + R ← H^{T} is the matrix transpose of H
K = P * H^{T} * 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)
predX = (A * predX) + (B * c)
The estimated position of the cursor is in the x vector:
x ← [xPos, yPos, xVel, yVel]
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.