Unscented Kalman Filters (follow)

Sigma Point Prediction

For the prediction step, we insert every sigma point into the process model. We store every predicted sigma point as a column of a matrix.

The input to the process model is a 7 dimensional augmented vector (5 state dimensions + 2 noise dimensions). The output is a 5 dimensional vector, because this is what the process model returns.

Predicted Mean and Covariance

First, we generated the sigma points. Then, we predicted them. Now, we want to use the sigma points to calculate the mean and the covariance of the predicted state.

We need a weight for every sigma point. The weights depend on the spreading parameter Lambda. (We used Lambda to set how far we want to spread the sigma point). We need to consider Lambda again here because before we had a covariance and generated sigma points. Now (at k+1|k), we are doing the inverse step, we have predicted sigma points and we want to recover the covariance matrix. So we also have to invert the spreading of the sigma points. This is what the weights do.

Measurement Prediction

We have to transform the predicted state to the measurement space. The function that defines this transformation is the measurement model. Sure we have to consider what type of sensor produced the current measurement, and use the correspondent measurement model.

This problem is very similar to the one in the prediction step. We need to transform a distribution through a non linear function, so we can apply exactly the same unscented transformation approach as we did before.

We can do a shortcut here to make things easier; the first thing we did in the prediction step is that we generated sigma points, we would make this again using the predicted mean and covariance matrix → But we can just reuse the sigma points we already have from the prediction step.

We can also skip the augmentation step...Why? We needed the augmentation because the process noise had a non linear effect on the state. But here the measurement noise has a purely additive effect, in this case we don't need to perform augmentation; since there is an easier way to consider the measurement noise which we will tackle later.

Now we just have to transform the sigma points we already have to the measurement space, and use them to calculate the mean and the covariance matrix of the predicted measurement.

The measurement space of the radar was a 3D space.

UKF Update

What we have:

There is one more thing that we need; the actual measurement Z|k+1, that we receive for the time step k+1. This is the first time that we really need to know the measurement values. What we needed right from the beginning was the time of the measurement, so we can predict to the correct time, and the later the sensor type so we can use the correct measurement model in the measurement prediction step.

Parameters and Consistency

🔥

The video in this lesson is Impressive 🤩

Yaw Acceleration Noise Parameter Intuition

Measurement Noise Parameters

Measurement noise parameters represent uncertainty in sensor measurements. In general, the manufacturer will provide these values in the sensor manual. In the UKF project, you will not need to tune these parameters.

Consistency: Normalized Innovation Squared (NIS)

A measure to know if you set the noise parameters correctly. In the following case, the actual measurement occurs somewhere inside the error ellipse, so everything looks Ok.

If the results kept looking like the cases on the right, something is wrong. These results tells you that you underestimate the uncertainty in your system. Your estimate is less precise than you think.

Here you overestimate the uncertainty in your system. It means that your estimate is actually more precise than you think. This filter is also inconsistent.

Normalized Innovation Squared Consistency Check

NIS is just a scalar number and so easy to calculate. But what number should we expect?

df: degrees of freedom, the dimension of our measurement space. The first column means that for 95% of all the cases, your NIS values should be higher than the given number based on the df.

We should always plot the 95% line and check for the shape of the NIS values. The next curve is actually a good one and means that our noise parameters are good.

Unfortunately, the NIS doesn't tell you where the mistake comes from, but at least it gives you some feedback.

What to Expect from the Project

Initializing the Kalman Filter

As discussed previously, the process noise parameters have an important effect on your Kalman filter; you will need to tune the longitudinal and yaw acceleration noise parameters as part of the project. The initial values for your state variables will also affect your Kalman filter's performance. Both the:

state vector x and the

state covariance matrix P

need to be initialized for the unscented Kalman filter to work properly.

Initializing the State Vector x

The state vector x contains x = [px, py, v, ψ, ψ˙]. You won't know where the bicycle is until you receive the first sensor measurement. Once the first sensor measurement arrives, you can initialize px and py. For the other variables in the state vector x, you can try different initialization values to see what works best.

Note that although radar does include velocity information, the radar velocity and the CTRV velocity are not the same. Radar velocity is measured from the autonomous vehicle's perspective. If you drew a straight line from the vehicle to the bicycle, radar measures the velocity along that line.

In the CTRV model, the velocity is from the object's perspective, which in this case is the bicycle; the CTRV velocity is tangential to the circle along which the bicycle travels. Therefore, you cannot directly use the radar velocity measurement to initialize the state vector.

Initializing the State Covariance Matrix P

To initialize the state covariance matrix P, one option is to start with the identity matrix. For the CTRV model, P is a 5x5 matrix. The identity matrix would be:

Think back to what the state covariance matrix represents: take for example the value in the first row, second column. The value at (1, 2) would be the covariance σpx,py measuring the linear relationship between the two variables. The diagonal values represent the variances for each of the variables in the x state vector.

Why is the identity matrix a good place to start? Since the non-diagonal values represent the covariances between variables, the P matrix is symmetrical. The identity matrix is also symmetrical. The symmetry comes from the fact that the covariance between two variables is the same whether you look at (x, y) or (y, x).

If you print out the P matrix in your UKF code, you will see that P remains symmetrical even after the matrix gets updated. If your solution is working, you will also see that P starts to converge to small values relatively quickly. Instead of setting each of the diagonal values to 1, you can try setting the diagonal values by how much difference you expect between the true state and the initialized x state vector. For example, in the project, we assume the standard deviation of the lidar x and y measurements is 0.15. If we initialized px with a lidar measurement, the initial variance or uncertainty in px would probably be less than 1. You will have to experiment with different initialization values to find a working solution.

Additional Project Details

Highway Parameters

In highway.h there are a number of parameters that can be modified to help with testing and understanding.

// Parameters 
// --------------------------------
// Set which cars to track with UKF
std::vector<bool> trackCars = {true,true,true};
// Visualize sensor measurements
bool visualize_lidar = true;
bool visualize_radar = true;
bool visualize_pcd = false;
// Predict path in the future using UKF
double projectedTime = 0;
int projectedSteps = 0;
// --------------------------------

The trackCars list can be used to toggle on/off cars for the UKF objects to track. The default is to track all three cars on the road, but for testing it can be nice to toggle to only track one at a time. For instance to only track the first car {true,false,false}.

The image above shows what it looks like if the projectedTime and projectedSteps variables are used. Also in the animation above the visualization for lidar and radar has been set to false and projectedTime = 2 and projectedSteps = 6. The green spheres then show the predicted position for the car in the future over a 2 second interval. The number of steps increases the number of positions to interpolate the time interval. It's interesting to see the predicted path which is constrained by the motion model. In this project the motion model used is CTRV, which assumes constant velocity and turning rate. Since our cars do not have constant turning rates you can see the predicted paths swing around and take a while to correct after the car begins moving straight again.

The visualize_pcd parameter can be set to true to visualize the cars from the lidar's perspective from point clouds. The traffic pcd data is available in src/sensors/data/pcd directory. As a bonus assignment this data could be used to cluster the individual cars using techniques from Lidar Obstacle Detection and then bounding boxes could be fitted around the car clusters. The bounding box centre (x,y) point could then be used to represent the lidar marker that would be fed into the UKF instead of the project pre-generated lidar marker from tools.cpp lidarSense function.