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;

Monday, May 22, 2017

Java refresher

Java version of hello world is like this:
import java.util.*;
public class vanilla {
    public static void main(String args[]) {
    System.out.println("hello world");
  1. java.util is the basic package of Java, which contains many useful classes such as Arrays, ArrayList, Date, HashMap, Hashtable, LinkedList, Random, Set, List. A full description is here.
  2. class is usually public so it can be called. The class name is the same with the java file name. Because we usually have multiple classes work together, a project is build.
  3. There will be a main method, which is also usually public. So the java application runs by the main method.
  4. System is one of the classes in java.lang package, which is the default. A full list of the package is here.
Because Java is compile-run 2-step language, so:
  • we don’t have interactive IDE like Jupyter notebook, Matlab or R studio.
  • It is a production language. It is static type so you declare data type of each variable.
  • It trades development time for run time.
For convenience, I only write the code snippet inside the main method.

2 types of for-loop and array

int [] numbers = {10, 20, 30, 40, 50}; //list of int
for(int x : numbers ) {
    System.out.print( x +"\t");
String [] names = {"James", "Larry", "Tom", "Lacy"};  // list of String
for( String name : names ) {
    System.out.print( name + ",");
} // enumerate style: enhanced
for (int i=0; i<5;i++){
}  // incremental style: traditional
conditional operator Exp1 ? Exp2 : Exp3;


System.out.printf("The value of e is %.4f%n", Math.E);
System.out.printf("sqrt(%.3f) is %.3f%n", x, Math.sqrt(2));
note: printf is formatted print so %.3f means the float variable is formatted as 3 digits after the decimal point.%n is used to create a new line.


String Str = new String("");
for (String word: Str.split("-")) {
} // iterate to print an array of string
s.substring(0,2) // get first 2 letters.

pass array to method

public static void printArray(int[] array) {
   for (int a :array) {
      System.out.print(a + " ");
printArray(new int[]{3, 1, 2, 6, 4, 2});

regular expression

import java.util.regex.Matcher;
import java.util.regex.Pattern;
public class RegexMatches {
   public static void main( String args[] ) {
      String line = "This order was placed for QT3000! OK?";
      String pattern = "(.*)(\\d+)(.*)"; // 3 groups
      Pattern r = Pattern.compile(pattern);
      Matcher m = r.matcher(line);
      if (m.find( )) {
         System.out.println("Found value: " + );
         System.out.println("Found value: " + );
         System.out.println("Found value: " + );
      }else {
         System.out.println("NO MATCH");

args input

public class CommandLine {
   public static void main(String args[]) { 
      for(String s:args) {

file IO

public class vanilla {
    public static void main(String args[]) throws IOException{
        FileReader in = new FileReader("input.txt");
        FileWriter out = new FileWriter("output.txt");
        int c;
        while ((c = != -1) {
} package has a lot of paired classes to perform input and output in terms of streams.
  • Byte Streams: 8-bit bytes. FileInputStream, FileOutputStream
  • Character Streams: 16-bit unicode. FileReader, FileWriter
  • Standard Streams: InputStreamReader, System.out.println

Directories operation

File d = new File("/tmp/user/java/bin");
d.mkdirs(); //create a directory and all parent directory

File a = new File("/tmp");
String[] paths = a.list();  // list all files, directories
for (String path: paths){

read and parse csv file

This is for a simple case, which you use split() to parse the csv file.
String csvFile = "country.csv";
String line = "";
BufferedReader buffer = null;
try {FileReader file = new FileReader(csvFile);
     buffer= new BufferedReader(file);
    while ((line = buffer.readLine()) != null) {
        String[] words = line.split(",");
        for (String s:words ) {
            if (s.equals("\"China\"")){System.out.println("hello");}
}  catch (IOException e) {e.printStackTrace();}

array vs ArrayList

  1. array such as int[] nums= int[5] has fixed size, but arraylist such as ArrayList<Integer> nums = new ArrayList<Interger>() has elastic size.
  2. array is easy to use and storage efficient but has limited functionality
int[] nums= {1,2,3};

ArrayList<Integer> n = new ArrayList<Integer>();
double array size:
int [] nums = new int[100];
int size = 0;  // track of current size
value [size] = value;
if (size>= nums.length){
  nums = Arrays.copyOf(nums, 2*nums.length);
}  // from java.util.Arrays package
System.out.println(Arrays.toString(nums)) // use string to print
  1. String can be converted to chars by s.toCharArray(), or you access individual char using s.chartAt(i)
  2. length of string is s.length() but length of array is a.length, length of ArrayList is al.size()


import java.util.Scanner;
System.out.println("Enter your username: ");
Scanner scanner = new Scanner(;
String username = scanner.nextLine();
System.out.println("Your username is " + username);
You can use while(scanner.hasNextDouble()) to check user input for continuous input.

add external jar in intell j

file ->project structure -> modules -> dependecne => +JARs

java system property

java.util.Properties props = System.getProperties();


IntelliJ is my favourite. BlueJ is also nice due to its class-module visualization.

Downloading BlueJ