Wednesday, May 31, 2017

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:
f(x) = \frac {1}{\sqrt{2*\pi*\sigma^2}} exp(-\frac{1}{2}\frac{(x-\mu)^2}{\sigma^2})

1 measurement update

The new information will always decrease your covariance, meaning more certainty. Think about it this way: at f(\mu)=\frac {1}{\sqrt{2*\pi*\sigma^2}}, 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;
    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;
    //measurement update
    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 \rho, \phi, \dot{\rho}.
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;

C++ libraries, boost and eigen

c++ check point: Bjarne Stroutstrup, file io, class, template
c++ check point 2: vector/array, char/string, map, auto, lambda, sort, unique
I came to realize the huge blindspot in my “Data Structure” class 5 years ago. The professor, head of our CS department, never taught us how to use C++ libraries. He asked us to implement array, vector, linked list and tree from scratch. These assignments did help us understand the building block of data structure. But we should never end there.
Just a few days ago when I was preparing for a C++ interview, I learned to use vector and sort, and appreciated the beauty of pointer. I came to realize that Python has hidden so many implementation details. On one hand it is easy to use and reach more users, but on the other hand it slow down the calculation speed and it is less flexible to make some fundamental changes.
Python has package management tools such as pip and conda. But for c++, it only use brew. And the default C++ STL such as vector, algorithm is located at /Library/Developer/CommandLineTools/usr/include/c++/v1
I am confused that several places storing somewhat differently command lines:
For the same command, there may be multiple alias, such as: c++, clang, cc.
What I really want to talk about is these 2 important c++ libraries:
Boost and Eigen.

how to use Boost

$ brew install boost
🍺  /usr/local/Cellar/boost/1.64.0_1: 12,628 files, 398.7MB
Boost is a collection of extensive libraries. And the most popular ones has been migrated into C++11, C++14, etc.
In my “warmup” project, my CMakeLists.txt file is as follows:
cmake_minimum_required(VERSION 3.7)

find_package(Boost 1.64.0 COMPONENTS system filesystem REQUIRED)  # library + version
    message(STATUS "Boost_INCLUDE_DIRS: ${Boost_INCLUDE_DIRS}")
    message(STATUS "Boost_LIBRARIES: ${Boost_LIBRARIES}")
    message(STATUS "Boost_VERSION: ${Boost_VERSION}")

set(SOURCE_FILES main.cpp)
add_executable(warmup ${SOURCE_FILES})

    target_link_libraries(warmup ${Boost_LIBRARIES})  # link
Note: find_package() can automatically find the include and library dir: /usr/local/lib/*, which is actually the alias of the real ones that brew has installed /usr/local/Cellar/boost/1.64.0_1. Surprisingly, manually setting the dir will cause … version.hpp" cannot be read error.
Alternatively, you can copy all the library files to your project folder and use #include "boost/xxx", which is more space costly.
The following cpp codes can be used to test whether the boost library is properly configured.
#include <boost/lambda/lambda.hpp>
#include <iostream>
int main(){
    typedef std::istream_iterator<int> in;
    std::cout << "Type in any number: ";
            in(std::cin), in(), std::cout
                    << (boost::lambda::_1 * 10)
                    << "\nType in another number: " );

how to use Eigen

$ brew install eigen
$ brew list eigen
/usr/local/Cellar/eigen/3.3.3/include/eigen3/ (477 files)
/usr/local/Cellar/eigen/3.3.3/share/eigen3/ (4 files)
The whole Eigen folder is only 5.5 /3.8 MB (before/after compilation), which contains a batch of headers and their corresponding cpp files stored in src folder.
There are 2 ways to use eigen libraries:
  1. copy the whole eigen folder to the project folder. write #include "Eigen/Dense" in main.cpp.
  2. keep eigen folder where brew put it. write #include <Eigen/Dense> in main.cpp. Add include_directories (/usr/local/Cellar/eigen/3.3.3/include/eigen3/) in “CMakeLists.txt”.
We are using the Dense here, which is a header condensing 7 headers together, such as Coure, LU, QR, EigenValues, Geometry.

data type

In STL, list is implemented as vector<int> a (5,0) or array<int, 3> ={0,0,0}.
In Eigen, it is interesting that the data class is named as [Matrix/Vector][1234X][ifdc]. For example, MatrixXf means Matrix with a dynamic size and filled with float numbers.Vector3i means a Vector of size 3 and filled with integers.
#include <Eigen/Dense>
#include <iostream>
using namespace std;
using namespace Eigen;
int main(){
    MatrixXf m1;
    m1 << 1.3, 4,-8,
            0,0.9,2; // manual input, match existing size
    cout<< m1(1,1) <<endl; // access by ()
    m1 = MatrixXf:: Zero(2,5); // change sizes
    cout<<"m1_4=\n" <<m1 <<endl;
      m1 = m1.array() * m1.array(); // element-wise operation
    cout<<"m1_5=\n" <<m1 <<endl;

    Vector3i v1 = Vector3i::Ones();
    cout<< v1(0)<<v1[0]<<endl;

    MatrixXf m2(5,2);
    cout << m1*m2 <<endl;
    cout << <<endl;
    cout << m1.maxCoeff() <<endl;
    cout << m1.mean() <<endl;