Udacity Self-Driving Car Engineer Nanodegree: Lidar and Radar Fusion with Kalman Filters in C++.
The task is to track a prdestrain moving in front of our autonomous vehicle.
This project can use multiple data sources originating from different sensors to estimate a more accurate object state.
The Kalman Filter algorithm will go through the following steps:
- First measurement.
- initialize state and covariance matrices.
- Predict.
- Update.
- Do another predict and update step (when receive another sensor measurement).
Because our state vector only tracks position and velocity, we are modeling acceleration as a random noise.
void KalmanFilter::Predict() {
x_ = F_ * x_;
MatrixXd Ft = F_.transpose();
P_ = F_ * P_ * Ft + Q_;
}zmeasurement vectorxstate vectorRmeasurement covariance matrixHmeasurement matrix: Find the right H matrix to project from a 4D state to a 2D observation space.
Because our state vector only tracks position and velocity, we are modeling acceleration as a random noise.
Details: Udacity Process Covariance Matrix
// compute the time elapsed between the current and previous measurements
// dt - expressed in seconds
float dt = (measurement_pack.timestamp_ - previous_timestamp_) / 1000000.0;
previous_timestamp_ = measurement_pack.timestamp_;
float dt_2 = dt * dt;
float dt_3 = dt_2 * dt;
float dt_4 = dt_3 * dt;
// 1. Modify the F matrix so that the time is integrated
kf_.F_(0, 2) = dt;
kf_.F_(1, 3) = dt;
// 2. Set the process covariance matrix Q
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;
// 3. Call the Kalman Filter predict() function
kf_.Predict();
// 4. Call the Kalman Filter update() function
// with the most recent raw measurements_
kf_.Update(measurement_pack.raw_measurements_);void KalmanFilter::Update(const VectorXd &z) {
VectorXd z_pred = H_ * x_;
VectorXd y = z - z_pred;
MatrixXd Ht = H_.transpose();
MatrixXd S = H_ * P_ * Ht + R_;
MatrixXd Si = S.inverse();
MatrixXd PHt = P_ * Ht;
MatrixXd K = PHt * Si;
//new estimate
x_ = x_ + (K * y);
long x_size = x_.size();
MatrixXd I = MatrixXd::Identity(x_size, x_size);
P_ = (I - K * H_) * P_;
}
It works quite well when the pedestrian is moving along the straght line.
However, our linear motion model is not perfect, especially for the scenarios when the pedestrian is moving along a circular path.
To solve this problem, we can predict the state by using a more complex motion model such as the circular motion.
The main differences betweem EKF and KF are:
- the
Fmatrix will be replaced byF_jwhen calculating ``P'. - the
Hmatrix in the Kalman filter will be replaced by the Jacobian matrixH_jwhen calculatingS,K, andP. - to calculate
x', the prediction update function,f, is used instead of theFmatrix. - to calculate
y, the hhh function is used instead of theHmatrix.
In the radar update step, the Jacobian matrix H_j is used to calculate S, K and P.
To calculate y, we use the equations that map the predicted location x' from Cartesian coordinates to polar coordinates
The radar sensor will output values in polar coordinates.
In order to calculate yyy for the radar sensor, we need to convert x′x'x′ to polar coordinates.
We are still using a linear model for the prediction step.
So, for the prediction step, we can still use the regular Kalman filter equations and the F matrix rather than the extended Kalman filter equations.
The measurement update for the radar sensor will use the extended Kalman filter equations.
The radar can directly measure the object range, bearing, radial velocity.
For radar, there is no H matrix that will map the state vector x into polar coordinates; instead, we need to calculate the mapping manually to convert from cartesian coordinates to polar coordinates.
We use the extended kalman filter in Radar Measurements for non-linear function.
What we change is we simply use non-linear function f(x) to predict the state, and h(X) to compute the measurement error.
So we first linearize the non-linear prediction and measurement functions, and then use the same mechanism to estimate the new state.
Extended Kalman filter (EKF) is the nonlinear version of the Kalman filter which linearizes about an estimate of the current mean and covariance.
To derive a linear approximation for the h function, we will only keep the expansion up to the Jacobian matrix Df(a).
We will ignore the Hessian matrix D^2f(a) and other higher order terms.
Assuming (x - a) is small, (x - a)^2 or the multi-dimensional equivalent will be even smaller;
Details: Calculate the Jacobian matrix
MatrixXd CalculateJacobian(const VectorXd& x_state) {
MatrixXd Hj(3,4);
// recover state parameters
float px = x_state(0);
float py = x_state(1);
float vx = x_state(2);
float vy = x_state(3);
// pre-compute a set of terms to avoid repeated calculation
float c1 = px*px+py*py;
float c2 = sqrt(c1);
float c3 = (c1*c2);
// check division by zero
if (fabs(c1) < 0.0001) {
cout << "CalculateJacobian () - 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;
}
VectorXd CalculateRMSE(const vector<VectorXd> &estimations,
const vector<VectorXd> &ground_truth) {
VectorXd rmse(4);
rmse << 0, 0, 0, 0;
// check the validity of the following inputs:
// * the estimation vector size should not be zero
// * the estimation vector size should equal ground truth vector size
if (estimations.size() != ground_truth.size()
|| estimations.size() == 0) {
cout << "Invalid estimation or ground_truth data" << endl;
return rmse;
}
//accumulate squared residuals
for (unsigned int i = 0; i < estimations.size(); ++i) {
VectorXd residual = estimations[i] - ground_truth[i];
//coefficient-wise multiplication
residual = residual.array()*residual.array();
rmse += residual;
}
//calculate the mean
rmse = rmse / estimations.size();
//calculate the squared root
rmse = rmse.array().sqrt();
//return the result
return rmse;
}