## Unscented Kalman Filter

This page will describe how to create and program a C++Unscented Kalman Filter (UKF) to accurately track a dynamic object motions in two-dimensions modelling system and sensor errors using lidar and radar measurements. The information supplied is not exhaustive, and it is recommended to view the source code and comments on GitHub for a deeper understanding of the code. A link to the GitHub repository is here. A video of the project in action can be found here.

This project page will describe the following system sections

Process model

Generate sigma points

Predict sigma points

Predict mean and covariance

Predict measurement

Update state

Consistency Verification

## Process Model

Before we can design a UKF, we need to define the objects motion model that we wish to track for the filters state vector. There is numerous model to choose from that will work with the UKF such as constant velocity model (CV), constant turn rate and velocity magnitude model (CTRV), constant turn rate and acceleration (CTRA), constant steering angle and velocity (CSAV) and constant curvature and acceleration (CCA). Each model makes different assumptions about an object's motion. We will work with the CTRV model which is shown below.

The two-dimensional position of the object at time k is px and py. The velocity at k is Vt and yaw angle is represented by ψ. The function f is the process model that predicts the parameters of the object at time step k + 1

For the state vector (Sv), we will track the position, velocity, yaw angle and the yaw rate or;

The process model predicts the position of the object using non-linear mathematical models of each parameter in the system. The process model is;

Where µk is the objects mean state at time step k and νk is the process noise at time step k. One way to solve for the CTRV model involves calculating the differential equations of the state vector. We will assume that the CTRV model has constant velocity and ψdot. Later we will account for process noise in the system to account for the distribution of acceleration and sensor noise. From the CTRV diagram above we cannot derive the model directly, but we can say the following from its change in state.

To get from time step k to k+1, we will use the integral of the differential equations from k to k+1, where the difference between k and k+1 is Δt.

This is our process model for predicting the object at k+1. However, If the vehicle is driving in a straight line, there will be a divide by zero as the yaw rate ψdot is zero. For this particular case, we can derive process model just by looking at the diagram above.

We now need to introduce the process noise (νk) into the model. We will describe the noise vector as;

Where νa,k is the longitudinal acceleration noise and νψ,k is the yaw acceleration noise, and they are accompanied by their statistical properties which normally distributed with a mean of zero and a distribution of σ squared.

The position will be affected by the noise of the yaw acceleration, but if we have a fast update rate, the effect will be minimal compared to other factors so we will neglect it from our positional noise. To simplify things, even more, we can assume that between timesteps the positional variations of a small yaw angle will be close to that of no yaw angle so we can use an approximation and neglect the yaw rate from the noise process. With these assumptions, we can integrate the zero yaw rate process models position with respect to Δt, and we get the following.

The noise influence on velocity can be solved by assuming that acceleration is constant between timesteps, then the acceleration noise will linearly add up to the velocity over time. This is also the case for the yaw rate acceleration.

νx and νare the process noise on the x and y positions respectively. ​The noise influence on velocity can be solved by assuming that acceleration is constant between timesteps, then the acceleration noise will linearly add up to the velocity over time. This is also the case for the yaw rate acceleration.

νv is the velocity process noise and νψ is the yaw acceleration. The yaw rate is integral of νψ above.

νψ is the yaw rate process noise. The final process model is;

## Generate Sigma Points

If we use the process model above to predict the objects mean and distribution of uncertainty one time-step in advance the distribution will not be normally distributed and it is also not easy to calculate this distribution as it is a non-linear processes model. With this unknown distribution, we can not accurately estimate the objects state parameters, and they become only approximations. A UKF solves this problem by using sigma points to represent the mean and distribution or covariance or the object. The sigma points are chosen from the mean state and in a certain relation to the mean and standard deviations of the state vector. These sigma points serve as a representation of the whole distribution. See the figure below.

The purple circle represents the mean (µ) of the objects state and the blue the covariance (P). The green star is the sigma points mean and the red starts represent the covariance distribution.

The first k in the |(k pipe k) is the estimated state vector at the present time step and is called the posterior. The second t is the latest measurement values at the present time step. Later when we update the sigma points, we will see k+1 |k where the k + 1 is the estimated state vector for the next time step and is called the Prior.

The number of sigma points to generate is dependant on the dimensions of the state vector nx and is calculated with the following.

nσ = 2nx + 1

One for the mean and two for each state dimension. In our case, so far, nx is five, so we will need to produce 11 sigma points. The first sigma point is the mean of the objects first position. The covariance sigma points are calculated using the following "Rule for sigma points" matrix.

The result of the square root of the covariance matrix will result in a matrix where we can consider it as nx vectors. Each vector is the direction in which we will place a sigma point. It is, however, λ defines how far about the mean the point is. λ is a design parameter that we can choose to put the sigma points in relation to the covariance ellipse. For our example, we will use λ = 3 - nx. The larger λ, the further away the sigma points are away from the mean.

Another benefit of the UKF is that we can model independent process noise into the calculation. The process noise doesn't express effects on the state vector and is independent of the changes in time. We will define this process noise into the covariance matrix Q below.

To calculate the covariance matrix Q, all we need to know is the stochastic properties of the noise processes. We derived these earlier but here they are as a reminder.

Where N is a normal distributed, white noise with zero mean and distribution of σ squared. The covariance matrix Q is;

We can now update the state vector. When including the covariance matrix in the sigma points calculation, it is called Augmentation. The new state vector is;​

We now require 15 sigma points as the augmented state matrix, nx, is seven.

To add the process noise matrix Q, we extend P and add Q to the bottom right corner and pad the remaining positions with zero. This is called the Augmented covariance matrix.

With these augmented equations we can now program the sigma points calculations.

// update the state augmentation vector
VectorXd x_aug = VectorXd(n_aug_);
x_aug
<< x_, 0, 0;

// Process noise standard deviation longitudinal acceleration in m/s^2
double std_a_ = 3.0;
// Process noise standard deviation yaw acceleration in rad/s^2
double std_yawdd_ = 1.6;

// Calculate the augmentation matrix
MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
P_aug.fill(0.0);
P_aug.topLeftCorner(n_x_, n_x_) = P_;
P_aug(5,5) = std_a_ * std_a_;
P_aug(6,6) = std_yawdd_ * std_yawdd_;

// n_aug_ = 7  n_aug_size_ = 2 * n_aug_ + 1
MatrixXd sigma_pts = MatrixXd(n_aug_, n_aug_size_);
sigma_pts.col(0) = x_aug;

// create square root matrix
MatrixXd sqrt_P = P_aug.llt().matrixL();

/*****************************************************************************
*  Generate sigma points
****************************************************************************/

for (int i = 0; i < n_aug_; i++){

// columns 1 -> n_aug_ = x + sqrt((lambda + n_aug_) * P_)
sigma_pts.col(i+1) = x_aug + sqrt(lambda_ + n_aug_) * sqrt_P.col(i);

// columns n_aug_+1 -> 2*n_aug_+1 = x - sqrt((lambda + n_aug_) * P_)
sigma_pts.col(i+1+n_aug_) = x_aug - sqrt(lambda_ + n_aug_) * sqrt_P.col(i);
}

## Predict Sigma Points

From the above, we now have the augmented sigma points at time step k. To predict the object state at k+1 all we do is pass each sigma point into the process model. It is worthy to note that in this step we pass in the points matrix that has a row size the same as the augmented state vector, seven in our case, but the returned predicted matrix will have a row size the same as the state vector, five for this system.

The code below predicts the sigma points at k+1.

/**
* Predicts the sigma points at the next time step from the current timestep.
* {sigma_pts} - matrix containing the previous sigma points
* {pred_sigma_pts} - matrix containing the predicted sigma points
* {delta_t} - change in time between UKF updates
*/

void UKF::SigmaPointPrediction(MatrixXd &sigma_pts, MatrixXd &pred_sigma_pts, double delta_t)

{
pred_sigma_pts.fill(0.0);
// iterate and predict each sigma point
for (int i = 0; i< n_aug_size_; i++){
double pos_x = sigma_pts(0,i);
double pos_y = sigma_pts(1,i);
double vel = sigma_pts(2,i);
double yaw = sigma_pts(3,i);
double yaw_dot = sigma_pts(4,i);
double a_pos = sigma_pts(5,i); // acceleration process noise
double a_yaw_dot = sigma_pts(6,i); // change in angle process noise

// Check that the yaw acceleration is not zero to prevent divide by zero
if (fabs(yaw_dot) > 0.001) {
pos_x += vel/yaw_dot * (sin(yaw + yaw_dot * delta_t) - sin(yaw));
pos_y += vel/yaw_dot * (cos(yaw) - cos(yaw + yaw_dot * delta_t));
} else {
pos_x += vel * delta_t * cos(yaw);
pos_y += vel * delta_t * sin(yaw);
}

// predict the next timestep sigma points using the process model
pred_sigma_pts(0,i) = pos_x + 0.5 * a_pos * delta_t * delta_t * cos(yaw);
pred_sigma_pts(1,i) = pos_y + 0.5 * a_pos * delta_t * delta_t * sin(yaw);
pred_sigma_pts(2,i) = vel + a_pos * delta_t;
pred_sigma_pts(3,i) = yaw + yaw_dot * delta_t + 0.5 * a_yaw_dot * delta_t * delta_t;
pred_sigma_pts(4,i) = yaw_dot + a_yaw_dot * delta_t;
}
}

## Predict Mean and Covariance

Now that we have predicted the objects state with sigma points we need to convert them back to a mean and covariance so we can get it's anticipated state. Lets first look back at what we have so far. We earlier predicted the sigma points, the diagram above is repeated here for clarity, and now we have the process models prediction of the sigma points, which might be similar to the second diagram, and we wish to calculate the mean and covariance as outlined in green.

The equation to calculate the mean and covariance of state samples is;

Where the index i is the ith column of the matrix, and wi is the mean sigma point weight. The weight performs the inverse of the spreading factor that we completed earlier with λ. There are multiple ways to calculate the weights, but we will use the following.

We can program the mean and covariance calculation code as follows;

/**
* Predicts the mean and covariance from the provided sigma points matrix.
* {x} - the output vector with the predicted mean values
* {P} - the output Matrix with the predicted covariance values
* {pred_sigma_pts} - the matrix containing the sigma points
* {yaw_pos} - the vector position of the stored yaw value. -1 if yaw
*                    not present in the sigma points.
*/

void UKF::PredictMeanAndCovariance(VectorXd &x, MatrixXd &P,
MatrixXd &pred_sigma_pts, const int yaw_pos=-1) {

//predict state mean
x.fill(0.0);

for (int i = 0; i < n_aug_size_; i++) {  //iterate over sigma points
x += weights_(i) * pred_sigma_pts.col(i);
}

//predicted state covariance matrix
P.fill(0.0);
for (int i = 0; i < n_aug_size_; i++) {  //iterate over sigma points

// state difference
VectorXd x_diff = pred_sigma_pts.col(i) - x;
if(yaw_pos >= 0) {
//angle normalization
while (x_diff(yaw_pos)> M_PI) x_diff(yaw_pos) -= 2.*M_PI;
while (x_diff(yaw_pos)<-M_PI) x_diff(yaw_pos) += 2.*M_PI;
}

P += weights_(i) * x_diff * x_diff.transpose();
}
}

The above function, PredictMeanAndCovariance, will be used later for predicting the mean and covariance of the sensor data and the final state and covariance matrix. We will use the parameter for Lidar measurements which does not contain yaw measurements.

This is the final step in the UKF prediction stage. We can now move onto updating the state with sensor measurements.

## Predict Measurements

Up to now, we have predicted the objects state based on the derived process model. It is now time to use sensor measurements of the model to refine our prediction. But first, we will predict where the measurement will be. To do this, we will use two sensors for analysis, a Lidar and a Radar. Both sensors measure the target object differently. The Lidar measures along a plane, in our case the x,y plane, and the Radar measures distance to the object (ρ) at an angle ψ with a change in the distance-velocity of ρdot. The most important part of predicting measurements is that we have the exact time that the measurement occurred.

To update the predicted state to the measurement state we need to consider which sensor made the measurement and used the associated measurement model. We then perform similar steps to the prediction stage above but with two shortcuts. The first we will reuse the predicted sigma points at timestep k+1, as this eliminates the need to generate new sigma points from the predicted mean and covariance at k+1. The second shortcut is that we do not need to produce an augmented matrix because sensor process noise is purely additive, even if the sensor measurement is non-linear, whereas the above process noise had a non-linear effect on the state. In this case, there is an easier way to consider measurement noise which we will consider during the covariance calculations later. The measurement model is a defined as;

Zk+1 |k  is the measurement space mean, and wk+1 is the measurement covariance noise.

The next step we require to do is to transform the predicted sigma points into the measurement space and using them to calculate the mean and covariance matrix of the expected measurement. Below is an example of the measurement space with transformed sigmoid points which we will then use to calculate the mean Zk+1 |k and the covariance Sk+1 |k.

Transforming the predicted sigma points into measurement space is not required for lidar measurements. This is because the measurements returned from the simulator are the x and y position points. However, should this be used with a Lidar that returns distance (d) at an angle (β), we will need to transform the predicted sigma points into the Lidars measurement space d & β.

As Radar measurements return distance, angle and velocity, we will need to transform the sigma points into the Radar measurement state. We do this with the follow equations.

The equation to calculate the mean and covariance of the sigma points in the measurement space are;

Where R is the measurement covariance noise. This is where we account for the measurement noise and it is defined as;

Implement the above Radar measurement prediction we can use the following code.

/**
* Updates the state and the state covariance matrix using a radar measurement.
* {MeasurementPackage} meas_package
* below is modified from the Unscented Kalman filter assignments 26, 27, 29, 30
*/​​

int n_z = 3; // number of sensor output parameters. (range, angle, vel)
//create matrix for sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, n_aug_size_);

/*****************************************************************************
*  transform predicted sigma points into measurement space
****************************************************************************/

for (int i = 0; i < n_aug_size_; i++) {
// extract values for better readibility
double pos_x = predicted_sigma_pts_(0,i);
double pos_y = predicted_sigma_pts_(1,i);
double vel  = predicted_sigma_pts_(2,i);
double yaw = predicted_sigma_pts_(3,i);

// measurement model
// range = sqrt(px^2 + py^2)

Zsig(0,i) = sqrt(fabs(pos_x*pos_x + pos_y*pos_y));
Zsig(1,i) = atan2(pos_y,pos_x); //angle
// velocity = (px*cos(ψ)v+py*sin(ψ)v)/sqrt(px^2 + py^2)
Zsig(2,i) = (pos_x*cos(yaw)*vel + pos_y*sin(yaw)*vel) / Zsig(0,i);
}

//mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
//measurement covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);

/*****************************************************************************
*  Calculate the mean and covariance
****************************************************************************/

UKF::PredictMeanAndCovariance(z_pred, S, Zsig, 1);

MatrixXd R = MatrixXd(n_z,n_z);
S = S + R;

/*****************************************************************************
*  Update the unscented kalman filter
****************************************************************************/

UKF::UpdateState(x_, P_, predicted_sigma_pts_, S, Zsig, z_pred, n_z, meas_package);
}

And the Lidar measurements is the following.

/**
* Updates the state and the state covariance matrix using a laser measurement.
* {MeasurementPackage} meas_package
*/

void UKF::UpdateLidar(MeasurementPackage meas_package) {

const int n_z = 2; // number of sensor output parameters (pos_x, pos_y)
//create matrix for sigma points in measurement space
MatrixXd Zsig = MatrixXd(n_z, n_aug_size_);

/*****************************************************************************
*  convert predicted sigma points into measurement space
****************************************************************************/

Zsig.row(0) = predicted_sigma_pts_.row(0);
Zsig.row(1) = predicted_sigma_pts_.row(1);

//mean predicted measurement
VectorXd z_pred = VectorXd(n_z);

//measurement covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);

/*****************************************************************************
*  Calculate the mean and covariance
****************************************************************************/

UKF::PredictMeanAndCovariance(z_pred, S, Zsig, -1);

MatrixXd R = MatrixXd(n_z,n_z);
R <<    std_laspx_*std_laspx_, 0,
0, std_laspy_*std_laspy_;
S = S + R;

/*****************************************************************************
*  Update the unscented kalman filter
****************************************************************************/

UKF::UpdateState(x_, P_, predicted_sigma_pts_, S, Zsig, z_pred, n_z, meas_package);
}

## Update State

The final step in the UKF is to use the predicted mean and covariance as well as the measurement's mean and covariance to update the objects estimated state vector and covariance matrix. To do this we require one more thing and that is the actual measurement Zk+1.

The state and covariance are updated with the following equations.

The Kk+1 |k is the Kalman gain. To calculate the Kalman gain, we will need to calculate the cross-correlation between the sigma points in the state space and the measurement space Tk+1 |k, and they are derived from the equations.

The final step in the UKF is coded below.

/**
* Updates the mean and covariance with a proprtion of the predicted and
* sensor measured position.
* {x} - output vector with the predicted mean values
* {P} - output Matrix with the predicted covariance values
* {S} - measurement covariance matrix
* {Zsig} - Matrix with the sensor predicted sigma points
* {z_pred} - Vector with the current predicted mean values
* {n_z} - Number of measured sensor parameters, lidar = 2, Radar = 3.
* {meas_package} - Measurement sensor class
*/

void UKF::UpdateState(VectorXd &x, MatrixXd &P, MatrixXd &pred_sigma_pts,
MatrixXd &S, MatrixXd &Zsig, VectorXd &z_pred, const int &n_z,
MeasurementPackage &meas_package) {

//create matrix for cross correlation Tc
MatrixXd Tc = MatrixXd(n_x_, n_z);
//calculate cross correlation matrix
Tc.fill(0.0);
for (int i = 0; i < n_aug_size_; i++) {

//residual
VectorXd z_diff = Zsig.col(i) - z_pred;
// state difference
VectorXd x_diff = pred_sigma_pts.col(i) - x;

Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
}

//Kalman gain K;
MatrixXd K = Tc * S.inverse();
//residual
VectorXd z_diff = meas_package.raw_measurements_ - z_pred;

if(use_nis_) {
// check the error using Normalised Innovation Squared
UKF::NISState(S, z_diff, n_z);
}

//update state mean and covariance matrix
x += K * z_diff;
P = P - K * S * K.transpose();
}

That is it for using a UKF for tracking an object, but to verify that our chosen process values are providing consistent predictions for the application, we will need to measure the system consistency.

## Consistency Verification

That is it for using a UKF for tracking an object but to verify that our chosen process values are providing consistent predictions for the application, we will need to measure the system consistency. Examples of consistent and inconsistent measurement prediction are;

In the above inconsistency, we are always under or overestimating the uncertainty of the predicted measurement. While we are dealing with stochastic measurements that can on occasion produce these inconsistencies, if it is happening on a regular basis, it is likely due to the noise parameters. The filter is consistent if it provides a realistic estimation of uncertainty. For our CTRV model, two parameters define the process noise, σa^2  representing the linear acceleration noise and σψ^2  representing yaw acceleration or angular acceleration. Tuning will involve guessing appropriate parameter values and tweak the parameters during testing until the results are good enough. If the application requires a fast response time, you will need to choose a noise parameter slightly higher or for a smoother prediction marginally lower than the optimal parameters.

There are multiple methods to verify consistency, but we will use the "Normalised Innovation Squared" or NIS method. The equation for NIS is;

This will return a scalar value of the relationship between the predicted measurement against the actual measurement, normalised in relation to the covariance matrix S. The value follows a distribution called "Chi-squared distribution", and it is the distribution of the sum of squared standard normal deviations relative to the system's degree of freedom. A web search of a Chi-squared table will produce a table like the one here from this site. This table informs us of the number we should expect for our NIS.

In our model, we are using a Radar that has three degrees of freedom so the value that we will compare our prediction against. We can compare our value against any of the numbers in the third row, but for simplicity, we will use  0.950 column, and we can expect that 95% of the NIS values will be lower than 7.815. If this is the case, we will say that we have a consistent filter. If more than 5% of the values are larger than 7.815, the filter is underestimating the uncertainty in the system, and if they are less than 5%, the filter is overestimating the uncertainty.

We can compute the NIS of our system with the following code.

/**
* Normalised Innovation Squared for checking noise parameter values,
* {S} - measurement covariance matrix
* {z_diff} - difference between predicted and measured values
* {n_z} - Number of measured sensor parameters, lidar = 2, Radar = 3.
*/

void UKF::NISState(MatrixXd &S, VectorXd &z_diff, const int &n_z) {

update_count_ += 1;

// compare error with the Chi-Squared 0.050 distribution
if(n_z == 2) { // Lidar measurement check
if(z_diff.transpose() * S.inverse() * z_diff > 5.991) {
nis_thresh_count_ += 1;
}
} else if(n_z == 3) { // Radar measurement check
if(z_diff.transpose() * S.inverse() * z_diff > 7.815) {
nis_thresh_count_ += 1;
}
}

std::cout << "Chi Squared 0.05 error = ";
std::cout << (float(nis_thresh_count_) / float(update_count_)) * 100.0 << "%";
std::cout << std::endl;
}

That is it. The UKF project code can be viewed and downloaded from GitHub here.

A video of the UKF filter in action can be viewed below. The red and blue circles indicate Lidar and Radar sensor measurements and the green triangles show the Kalman filters predicted position.

The

## CONTACT

I hope you enjoyed reviewing the project. - Let's get in touch. LinkedIn messages work best.