In this project I implemented the Unscented Kalman Filter to estimate the state of multiple cars on a highway using noisy lidar and radar measurements.
-
- first measurement - the filter will receive initial measurements of the car's position relative to our car. These measurements will come from a radar or lidar sensor.
-
- initialize state and covariance matrices - the filter will initialize the car's position based on the first measurement.
-
- then the car will receive another sensor measurement after a time period Δt.
-
- predict - the algorithm will predict where the cars will be after time Δt using Unscented Kalman filter.
-
- Measurement update – the measurement update step depends on sensor type. If the current measurements are generated by a Laser sensor, then we just apply a standard Kalman filter to update the car’s state. However, Radar measurements involve a non-linear measurement function. So, when we receive Radar measurements, we may use Unscented Kalman filter.
-
- then the car will receive another sensor measurement after a time period Δt. The algorithm then does another predict and update step.
there are many models including:
- constant velocity model (CV).
- constant turn rate and velocity magnitude model (CTRV).
- constant turn rate and acceleration (CTRA).
- constant steering angle and velocity (CSAV).
- constant curvature and acceleration (CCA).
Each model makes different assumptions about an object's motion. In our project, we will work with the CTRV model. With the assumption that the velocity is constant, you’re simplifying the way vehicles actually move because most roads have turns. But this is a problem because a process model with the assumption of constant velocity and directions (assumes objects can move straight) will predict turning vehicles incorrectly.
The state vector:
- Clone this repo.
- Make a build directory:
mkdir build && cd build - Compile:
cmake .. && make - Run it:
./ukf_highway