Techniques for estimating the state of a system:

- Kalman filter, for continuous state and unimodal distribution
- Moute Carlo localization, for discrete state

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

- measurement update, Bayes rule
- 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:`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_`

.- create a
`Tracking`

instance, in which the constructor initialize all the 6 parameters of`KalmanFilter`

instance, as well as noise, initialized status and timestamp. - 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. - call
`KalmanFilter::Predict()`

- 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;
}
```