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:
%20%3D%20%5Cfrac%20%7B1%7D%7B%5Csqrt%7B2*%5Cpi*%5Csigma%5E2%7D%7D%20exp(-%5Cfrac%7B1%7D%7B2%7D%5Cfrac%7B(x-%5Cmu)%5E2%7D%7B%5Csigma%5E2%7D))
1 measurement update
The new information will always decrease your covariance, meaning more certainty. Think about it this way: at %3D%5Cfrac%20%7B1%7D%7B%5Csqrt%7B2*%5Cpi*%5Csigma%5E2%7D%7D)
, 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.cppfirst parse the input file and get the corresponding information intovector<MeasurementPackage>. Each “MeasurementPackage” includeslong timestamp_,sensor_type_andVectorXd raw_measurements_.- create a
Trackinginstance, in which the constructor initialize all the 6 parameters ofKalmanFilterinstance, as well as noise, initialized status and timestamp. - use the
Trackinginstance to process the “MeasurementPackage” by callingProcessMeasurement(). 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;
}
