Kalman Filter to Navigate Obstacles Based on Noisy Senor Data

I've implemented a Kalman filter that takes noisy sensor data regarding the x and y positions of the asteroids in this simulation to infer the velocity and acceleration based on the bivariate normal distribution. This data is used to navigate a ship from the bottom to the top of the screen, avoiding all of the asteroids by using their predicted future positions. This is part of the Artificial Intelligence for Robotics course for the CS Master's program at Georgia Tech.

The Kalman filter is ideal for this application because of its ability to handle noisy data by using the Kalman gain to gradually transition from relying on the sensor to the internal model and because of its ability to infer correlated variables such as velocity and acceleration using a transition function that can define the physical constraints of the system. Because this system uses 6 variables it requires the use of linear algebra matrix operations vs the more 1D application of the Kalman filter.

The ship uses the predicted future positions of the asteroids to, at any given timestep, determine if steering left, right, or center, would bring to nearer or father from the squared euclidean distance of the future position of the nearest asteroid.

Below are samples of the ship using the Kalman filter to avoid the asteroids and reach the goal. The code for the asteroid field and rendering was provided by GaTech; the code for the Kalman filter and ship's navigation strategy was written by me.