Self-driving Car ND B1, Kalman Filter

Techniques for estimating the state of a system:

1. Kalman filter, for continuous state and unimodal distribution
2. Moute Carlo localization, for discrete state

Kalman Filter represents our distributions by Gaussians and iterates on 2 main cycles:

1. measurement update, Bayes rule
2. motion update(prediction), convolution, total probability

Distribution is described by a Guassian function:

1 measurement update

The new information will always decrease your covariance, meaning more certainty. Think about it this way: at , so the smaller sigma, the larger probability you have.

2 motion update

position is observable, speed is hidden and can be inferred from the location.

3 Kalman Matrices 1D

This is interesting. similar to the matrix representation of Quantum physics, we have matrix representation of Kalman filter. For convenience, location and velocity is wrapped together as x, the corresponding uncertainty is represented as P.

def kalman_filter(x, P):
    for n in range(len(measurements)):
        # measurement update
        Z = matrix([[measurements[n]]])
        y = Z-(H *x)  # error
        S = H * P * H.transpose() + R
        K = P * H.transpose() * S.inverse()  # Kalman gain
        x = x + (K*y)
        P = (I- (K*H)) *P
        # motion update
        x = (F*x) + u
        P = F*P * F.transpose()
    return x,P
measurements = [1, 2, 3]
x = matrix([[0.], [0.]]) # initial state (location and velocity)
P = matrix([[1000., 0.], [0., 1000.]]) # initial uncertainty
u = matrix([[0.], [0.]]) # external motion
F = matrix([[1., 1.], [0, 1.]]) # next state function
H = matrix([[1., 0.]]) # measurement function
R = matrix([[1.]]) # measurement uncertainty
I = matrix([[1., 0.], [0., 1.]]) # identity matrix
print kalman_filter(x, P)

In each iteration, we first have measurement update and then motion update. The key element in both updates is a very simple transfer matrix F=[[1,1],[0,1]]. For measurement update, because we can only directly measure location, so we extract the location from the state x by an even simpler matrix H =[1,0]. By comparing the old location and new measured location, we get error y. Combine with measurement uncertainty, we get a middle variable S and then Kalman gain K. Now we are ready to update the measurement state.

6 Kalman Matrices 1D by C++

#include <iostream>
#include <Eigen/Dense>
#include <vector>
using namespace std;
using namespace Eigen;
//Kalman Filter variables
VectorXd x,u;    // object state, external motion
MatrixXd P;    // object covariance matrix
MatrixXd F; // state transition matrix: 1, 1, 0, 1;
MatrixXd H, R;    // measurement matrix and covariance
MatrixXd I; // Identity matrix: 1,0,0,1
MatrixXd Q;    // process covariance matrix(noise):0,0,0,0

void filter(VectorXd &x, MatrixXd &P) {
    for (int n = 0; n < measurements.size(); ++n) {
        VectorXd z = measurements[n];
        VectorXd y = z- (H * x);
        VectorXd S = H * P* H.transpose() + R;
        VectorXd K = P * H.transpose() * S.inverse();
        // state update
          x = x + K * y;
        P = (I- K*H) * P;
          // motion update
        x = F *x +u;
        P = F*P* F.transpose()+Q;
    }
}

12 laser measurment 2D

The assignment is to implement Tracking::ProcessMeasurement(), which is only a very small part of the whole story. From bird’s eye view, there are several steps:

1. main.cpp first parse the input file and get the corresponding information into vector<MeasurementPackage>. Each “MeasurementPackage” includes long timestamp_, sensor_type_ and VectorXd raw_measurements_.
2. create a Tracking instance, in which the constructor initialize all the 6 parameters of KalmanFilter instance, as well as noise, initialized status and timestamp.
3. use the Tracking instance to process the “MeasurementPackage” by calling ProcessMeasurement(). The first measurement is used to initialize state and timestamp, others are used to modify transfer matrix F and process covariance Q.
4. call KalmanFilter::Predict()
5. call KalmanFilter::Update()

Note that class Kaman_filter and Tracking have been written in separate .h file and .cpp file. So the linking statement add_executable(kf main.cpp kalman_filter.cpp tracking.cpp)must be added into CMakeLIsts.txt.

Now it’s ready to get to the meat:

// Process a single measurement
void Tracking::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
    if (!is_initialized_) {
        kf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
        previous_timestamp_ = measurement_pack.timestamp_;
        is_initialized_ = true;
        return;
    }
    float dt = (measurement_pack.timestamp_ - previous_timestamp_) * 1e-6;    //dt - expressed in seconds
    previous_timestamp_ = measurement_pack.timestamp_;
    float dt_2 = dt *dt;
    float dt_3 = dt_2 *dt;
    float dt_4 = dt_3 *dt;
    kf_.F_(0, 2) = dt;
    kf_.F_(1, 3) = dt;
    kf_.Q_ = MatrixXd(4, 4);
    kf_.Q_ <<  dt_4/4*noise_ax, 0, dt_3/2*noise_ax, 0,
            0, dt_4/4*noise_ay, 0, dt_3/2*noise_ay,
            dt_3/2*noise_ax, 0, dt_2*noise_ax, 0,
            0, dt_3/2*noise_ay, 0, dt_2*noise_ay;
    //predict
    kf_.Predict();
    //measurement update
    kf_.Update(measurement_pack.raw_measurements_);
    std::cout << "x_= " << kf_.x_ << std::endl;
    std::cout << "P_= " << kf_.P_ << std::endl;
}

18 Radar and the polar coordinates

Radar sees a different world. It measures .

Extended Kalman Filter (EKF) uses a linear approximation of h(x), which is the first order approximation of Taylor expansion.

Because the original spaces have 4 variables: x, y, vx,vy. We will use a Jacobian matrix to represent the partial derivatives.

MatrixXd CalculateJacobian(const VectorXd& x_state) {
    MatrixXd Hj(3,4); // set size
    float px = x_state(0), py = x_state(1);
    float vx = x_state(2), vy = x_state(3);
    float c1 = px*px+py*py;
    float c2 = sqrt(c1);
    float c3 = (c1*c2);
    //check division by zero
    if(fabs(c1) < 0.0001){ // c-styple abs only for int
        cout << "Error - Division by Zero" << endl;
        return Hj;
    }
    //compute the Jacobian matrix
    Hj << (px/c2), (py/c2), 0, 0,
          -(py/c1), (px/c1), 0, 0,
          py*(vx*py - vy*px)/c3, px*(px*vy - py*vx)/c3, px/c2, py/c2;
    return Hj;
}

22 Calculate Root Mean Square Error

VectorXd CalculateRMSE(const vector<VectorXd> &estimations,
        const vector<VectorXd> &ground_truth){
    VectorXd rmse(4) = VectorXd::Zero(); //initialize
    if(estimations.size() != ground_truth.size()
            || estimations.size() == 0){
        cout << "Invalid data" << endl;
        return rmse;
    }
    //accumulate squared residuals
      VectroXd residual(4);
    for(unsigned int i=0; i < estimations.size(); ++i){
        residual = estimations[i] - ground_truth[i];
        rmse += residual.array()*residual.array();
    }
    rmse = rmse/estimations.size(); //mean
    rmse = rmse.array().sqrt();  // square root
    return rmse;
}

​