Coding Project
Kalman Filter Demo
Demo
A Kalman Filter can be used to predict the state of a system with a lot of input noise. This demo simulates a ball being observed by a camera (or some other sensor) – you can think of this as a camera viewing the field from above and a computer vision algorithm trying to determine the ball’s x and y position on the field. The ground truth position of the ball is determined by the user’s cursor position, and random noise is added to the measured sensor reading. The Kalman Filter is then run on the noisy position reading to give an estimated position. If prediction is enabled, the Kalman Filter can also predict the where the ball will be after n seconds given the current sensor reading.
Basic Controls
- Move your mouse cursor around the black canvas screen. The soccer ball will follow your cursor position, the ground truth position of the ball.
- You can click anywhere in the canvas area to lock the ball’s position. Click again to make the ball follow your cursor again.
- Click START SIMULATION to start the Kalman Filter. Once it is running, you can click PAUSE or RESUME to toggle the Kalman Filter simulation on or off. You can still move the ball around even when the simulation is off.
The Kalman Filter
- The green circle represents the Kalman Filter’s estimate of the ball’s position.
- If the Show Prediction checkbox is enabled, the faded soccer ball represents the Kalman Filter’s predicted future position of the ball after n seconds, where n is adjusted with the Prediction slider.
- You can control the amount of simulated sensor noise by moving the Noise slider.
- The matrix values for matrices A, H, Q, and R can be individually adjusted. Simply edit any of the values in any cell and it will update in the simulation in real time.
- Matrix B is the control matrix, which is disabled for this simulation. The control matrix does not apply here since we’re only simulating a sensor and have no control over the ball itself. This would come into play if we had control over the state, e.g. if the ball was a robot to which we were giving movement commands. Your cursor movement is not controlling anything as far as the simulation is concerned, it’s simply providing the ground truth position.
Breakdown
This video and this paper are great resources to get a deeper understanding of the algorithm.
The Kalman Filter is broken into two main steps: prediction and correction. The equation uses the control matrices
The Kalman Filter Equation
To start, we define four variables, all initialized to zero: P is the predicted covariance matrix (4×4), x is the predicted state column vector, mt is the measurement vector at current time t, and mt-1 is the measurement at the previous time t-1. The x and m vectors are of the form [xpos, ypos, xvelocity, yvelocity]. There’s also the c vector (control) which takes the same form, but this simulation just keeps it at 0 since it’s not used. This demo runs at 60 frames per second.
At each time t, we first determine mt. Its xpos and ypos are simply the actual cursor (ball) pixel position with random noise added to each axis. xvelocity and yvelocity are computed as the delta between the positions of mt and mt-1. We can then set mt-1 = mt for the next iteration.
Once we get the measurement, we run the actual Kalman Filter.
Prediction Step
x’ = Ax + Bc
P’ = APAT + Q
x’ is now our predicted state vector and P’ is our predicted covariance matrix.
Correction Step
K = P’HT(HP’HT + R)-1
x” = x’ + K(m – Hx’)
P” = (I – KH)P’
K is the Kalman Gain matrix. I is the identity matrix. x” and P” are now the corrected state vector and covariance matrix, respectively. We can update the x and P variables defined in the beginning to x” and P” to use for the next iteration at time t+1.
Future State prediction
If Show Prediction is enabled, we can use part of the prediction equation to step forward in time and predict the state at time t+n. Iterate this part n times, where n is the number of iterations into the future (e.g. at 60 FPS, 2 seconds into the future means n = 120).
xt+0 = x”
Iterate n times: xt+i+1 = Axt+i + Bc
That's All There Is
The xpos and ypos of the newly updated state x represent the Kalman Filter’s estimated position of the ball, shown in the simulation by the green circle. Similarly, xt+n represents the predicted future position in n iterations, represented by the faded ball.
The Control Matrices
A, B, H, Q, and R are the control matrices.
A is the state transition matrix. It influences the prediction step’s computation of x’. It controls how much the velocity measurement influences the predicted position.
B is the control matrix. This is the same as the A matrix but for the control vector. This simulation does not use a control vector, since it’s only simulating an observed environment that we have no influence over.
Q is the action uncertainty matrix. It is applied as a correction to the covariance matrix P in the prediction step.
H is the measurement matrix. It influences the Kalman Gain (K matrix) computation of the correction step and can be used for correcting or offsetting the measurement from your sensor (e.g. if your sensor always detects the position as too far to the left).
R is the sensor noise matrix. It also influences the Kalman Gain (K matrix), and controls how noise affects the system. Higher values of R can counteract higher sensor noise, but will also reduce the speed at which the Kalman Filter can converge on an corrected state.