Extended Kalman Filter Project

The objective of this project is to estimate the state of a moving object with noisy measurements from lidar and radar sensors. The state of the moving object is represented by two dimensional position p_x, p_y and two dimensional velocity v_x, v_y. The state vector is therefore:

Udacity provided simulated radar and lidar measurements from a cyclist who moves around the target vehicle. In addition, Udacity also provided a software framework which :

1. Reads in the data (sensor values as well as ground truth) from the simulator
2. Encapsulates the data into instances of class MeasurementPackage
3. Provide a software design with entry points to:

• Initialize Kalman filter matrices and variables
• Predict where the cyclist is going to be after a time step
• Update where the cyclist is based on sensor measurements
• Measure how the Kalman filter performs by calculating the root mean squared error

4. Returns the results back to the simulator

The actual contribution from the project is the Step 3, where the standard and extended Kalman Filter functions for predicting and updating the data from Lidar and Radar respectively are implemented.

---

Kalman Filter

To track the cyclist around the vehicle, it is required to define a :

1. State Transition function that models how the state of the cyclist changes between two sensor timestamps.
2. Measurement function that models how the measurement is calculated and how close it is to the predicted state.

State Transition function

Based on the kinematics equations of an object in motion, the new 2D position and 2D velocity can be estimated with the following equations:

where and are random acceleration vectors with mean 0 and standard deviations and respectively. In the project , both values are configured as 9.0 as recommended in the project instructions.

The state transition function shown above consists of deterministic part as well as stochastic part represented by the random acceleration vectors. A generalized version of the state transition function with deterministic and stochastic parts is shown below:

Prediction step

This step predicts the new state (x') and uncertainty (P') of the cyclist's position and velocity. The prediction calculation for new state is

where F is the state-transition matrix whose product with the initial state vector gives the state vector at a later time and refers to the uncertainty in the prediction step or simply process noise which is a gaussian distribution with mean 0 and covariance Q . The F matrix can be derived from the generalized state function:

During the transition of state, the cyclist might have changed direction and/or accelerated/decelerated. This changes the uncertainty of the prediction. The change in uncertainty is given by the following equation:

The noise variance matrix Q is calculated as :

Update step

Define H matrix for Lidar

Lidar sensor measures only two of the four object states (p_x, p_y). The H matrix projects the belief about the object's current state into the measurement space of the sensor. Multiplying H matrix with x, enables a one to one comparison with new sensor data , z.

H_laser_ << 1, 0, 0, 0,
            0, 1, 0, 0;

Define H Matrix for Radar

Incase of Radar sensors, the predicted position and speed are mapped to the polar coordinates of range, bearing and range rate.

The range ρ , is the distance to the pedestrian. The bearing φ is the angle between ρ and the x-axis. The range rate, is the projection of the velocity, v, onto the line, L. In a simplified form, the h function that specifies how the predicted position and speed get mapped to the polar coordinates of range, bearing and range rate is given by the following equation:

The above matrix consists of non-linear function (Eg:arctan()).

In order to apply Gaussian distributions, this needs to be linearized by a linear function which is tangent to h at the mean location of the original gaussian distribution. This is done with the help of multivariate Taylor series expansion. A general form of Taylor series expansion is:

As shown in the figure above, when the Taylor expansion is applied, the non-linear function is evaluated first at the mean location μ, followed by a extrapolation with the slope along μ. This slope is given by the first derivate of h, which is called the Jacobian matrix which contains the partial derivatives of H_j:

After calculating all the derivates, the final H_j looks like :

The function MatrixXd Tools::CalculateJacobian(const VectorXd& x_state) performs this calculation. The code for the implementation is taken over from the class notes as-is.

Calculate Error

The next step in the measurement process is to calculate the difference between the predicted states from the previous step with what the sensor measurement says.

Calculate Kalman Gain

The K matrix, referred to as the Kalman filter gain, combines the uncertainty of the prediction (P′) with the uncertainty of the sensor measurement R. The R matrix are generally provided by sensor suppliers. For the current project, the R values were provided by Udacity.

//measurement covariance matrix - laser
R_laser_ << 0.0225, 0,
            0, 0.0225;

//measurement covariance matrix - radar
R_radar_ << 0.09, 0, 0,
            0, 0.0009, 0,
            0, 0, 0.09;

Calculate updated object state

With the Kalman gain and the error components, the new state and the uncertainty of the cyclist is derived from the following equations:

Evaluate Performance

The Root Mean Squared Error (RMSE) method is used to evaluate the performance of the Kalman filter by measuring the deviation of the estimated state from the ground truth. The lower the RMSE, the higher is the accuracy. The performance evaluation is done in the tools.cpp. The code is taken over as-is from the class notes.

VectorXd Tools::CalculateRMSE(const vector<VectorXd> &estimations,
                              const vector<VectorXd> &ground_truth)

Final results

To evaluate the impact of radar and laser sensor in the Kalmar filter, the program is extended to support two command line flags --no-radar and --no-laser.

Kalman Filter with only Laser data

If the Kalman Filter is run with only laser data (--no-radar), the prediction accuracy of position measurement is good (with reference to the performance KPI mentioned in the project), but the accuracy of the velocity measurement is not good.

./ExtendedKF --no-radar

Kalman Filter with only Radar data

If the Kalman Filter is run with only radar data(--no-laser), the prediction accuracy of position measurement is comparatively lower, with a slight improvement in accuracy of the velocity measurement.

./ExtendedKF --no-laser

Kalman Filter with Radar and Laser

When the Kalman filter uses the data from both laser and radar, the accuracy of both the position and velocity of the cyclists is improved. The results on dataset1 is shown below:

Similar performance is also seen with the dataset2 as shown below:

---

How to run the Kalman Filter?

The Simulator can be downloaded here.

This repository includes two files that can be used to set up and install uWebSocketIO for either Linux or Mac systems. For windows you can use either Docker, VMware, or even Windows 10 Bash on Ubuntu to install uWebSocketIO. Please see the uWebSocketIO Starter Guide page in the classroom within the EKF Project lesson for the required version and installation scripts.

Once the install for uWebSocketIO is complete, the main program can be built and run by doing the following from the project top directory.

• mkdir build
• cd build
• cmake ..
• make
• ./ExtendedKF [--no-laser] [--no-radar]

---