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
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 * 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)
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 is available under the MIT Licence and can be downloaded here.
The script that defines the matrices and interfaces with the
HTML is also available here.