Self-driving Car ND B2, Unscented Kalman Filters

CTRV Model

constant turn rate and velocity magnitude model.

state vector x = , e.g. (2m, 4m, 7m/s, 0.5 rad, 0.6 rad/s)

the state transition function will involve integral.

When yaw rate , the car is going straight.

14 sigma points

the representative of the whole distribution.

number of sigma points: 2n+1

1st element is

2n elements is

int n_x = 5;  // state dimension
MatrixXd Xsig = MatrixXd(n_x, 2 * n_x + 1);
MatrixXd P = MatrixXd(n_x, n_x);
MatrixXd A = P.llt().matrixL();
Xsig.col(0)  = x;
for (int i = 0; i < n_x; i++){
    Xsig.col(i+1)     = x + sqrt(lambda+n_x) * A.col(i);
    Xsig.col(i+1+n_x) = x - sqrt(lambda+n_x) * A.col(i);
}

17 UKF Augmentation:

add process noise . Add previous P to the top left corner, add augmented covariance matrix in the bottom right corner:

int n_aug = 7;
VectorXd x_aug = VectorXd(n_aug);
MatrixXd P_aug = MatrixXd(n_aug, n_aug);
MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);
x_aug.head(5) = x;
x_aug(5) = 0;
x_aug(6) = 0;
P_aug.fill(0.0);
P_aug.topLeftCorner(5,5) = P;
P_aug(5,5) = std_a*std_a;
P_aug(6,6) = std_yawdd*std_yawdd;
MatrixXd L = P_aug.llt().matrixL();
Xsig_aug.col(0)  = x_aug;
double factor = sqrt(lambda+n_aug);
for (int i = 0; i< n_aug; i++){
    Xsig_aug.col(i+1)= x_aug + factor * L.col(i);
    Xsig_aug.col(i+1+n_aug) = x_aug - factor * L.col(i);
}

20 sigma point prediction

After delta_t

Xsig_pred_ = MatrixXd(n_x, 2 * n_aug + 1);
for (int i = 0; i< 2*n_aug+1; i++){
    //extract values for better readability
    double p_x = Xsig_aug(0,i);
    double p_y = Xsig_aug(1,i);
    double v = Xsig_aug(2,i);
    double yaw = Xsig_aug(3,i);
    double yawd = Xsig_aug(4,i);
    double nu_a = Xsig_aug(5,i);
    double nu_yawdd = Xsig_aug(6,i);
    double px_p, py_p;  //predicted state positions
    //avoid division by zero
    if (fabs(yawd) > 0.001) {
        px_p = p_x + v/yawd * ( sin (yaw + yawd*delta_t) - sin(yaw));
        py_p = p_y + v/yawd * ( cos(yaw) - cos(yaw+yawd*delta_t) );
    }
    else {
        px_p = p_x + v*delta_t*cos(yaw);
        py_p = p_y + v*delta_t*sin(yaw);
    }
    double v_p = v; // constant velocity magnitude
    double yaw_p = yaw + yawd*delta_t;
    double yawd_p = yawd;  // constant turn rate
    //add noise
    px_p = px_p + 0.5*nu_a*delta_t*delta_t * cos(yaw);
    py_p = py_p + 0.5*nu_a*delta_t*delta_t * sin(yaw);
    v_p = v_p + nu_a*delta_t;
    yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t;
    yawd_p = yawd_p + nu_yawdd*delta_t;
    //write predicted sigma point into right column
    Xsig_pred(0,i) = px_p;
    Xsig_pred(1,i) = py_p;
    Xsig_pred(2,i) = v_p;
    Xsig_pred(3,i) = yaw_p;
    Xsig_pred(4,i) = yawd_p;
  }

22 get state x and covariance by predicted sigma point

double weight_0 = lambda/(lambda+n_aug);
weights(0) = weight_0;
for (int i=1; i<2*n_aug+1; i++) {  //2n+1 weights
  double weight = 0.5/(n_aug+lambda);
  weights(i) = weight;
}
//predicted state mean
x.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; i++) {  //iterate over sigma points
  x = x+ weights(i) * Xsig_pred.col(i);
}
//predicted state covariance matrix
P.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; i++) {  //iterate over sigma points
  // state difference
  VectorXd x_diff = Xsig_pred.col(i) - x;
  //angle normalization
  while (x_diff(3)> M_PI) 
    x_diff(3)-=2.*M_PI;
  while (x_diff(3)<-M_PI) 
    x_diff(3)+=2.*M_PI;
  P = P + weights(i) * x_diff * x_diff.transpose() ;
}

26 measurement update: radar

get measurement covariance matrix S

int n_z = 3;
for (int i = 0; i < 2 * n_aug + 1; i++) {  //2n+1 simga points
  // extract values for better readibility
  double p_x = Xsig_pred(0,i);
  double p_y = Xsig_pred(1,i);
  double v  = Xsig_pred(2,i);
  double yaw = Xsig_pred(3,i);
  double v1 = cos(yaw)*v;
  double v2 = sin(yaw)*v;
  // measurement model
  Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y);   //r
  Zsig(1,i) = atan2(p_y,p_x);   //phi
  Zsig(2,i) = (p_x*v1 + p_y*v2 ) / sqrt(p_x*p_x + p_y*p_y);   //r_dot
}
//mean predicted measurement
VectorXd z_pred = VectorXd(n_z);
z_pred.fill(0.0);
for (int i=0; i < 2*n_aug+1; i++) {
    z_pred = z_pred + weights(i) * Zsig.col(i);
}
//measurement covariance matrix S
MatrixXd S = MatrixXd(n_z,n_z);
S.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; i++) {  //2n+1 simga points
  //residual
  VectorXd z_diff = Zsig.col(i) - z_pred;
  //angle normalization
  while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
  while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
  S = S + weights(i) * z_diff * z_diff.transpose();
}
//add measurement noise covariance matrix
MatrixXd R = MatrixXd(n_z,n_z);
R <<    std_radr*std_radr, 0, 0,
        0, std_radphi*std_radphi, 0,
        0, 0,std_radrd*std_radrd;
S = S + R;

Get cross correlation Tc and Kalman gain K, then update state:

//create matrix for cross correlation Tc
MatrixXd Tc = MatrixXd(n_x, n_z);
Tc.fill(0.0);
for (int i = 0; i < 2 * n_aug + 1; i++) {  //2n+1 simga points
  //residual
  VectorXd z_diff = Zsig.col(i) - z_pred;
  //angle normalization
  while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
  while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
  // state difference
  VectorXd x_diff = Xsig_pred.col(i) - x;
  //angle normalization
  while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
  while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
  Tc = Tc + weights(i) * x_diff * z_diff.transpose();
}
//Kalman gain K;
MatrixXd K = Tc * S.inverse();
//residual
VectorXd z_diff = z - z_pred;

//angle normalization
while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
//update state mean and covariance matrix
x = x + K * z_diff;
P = P - K*S*K.transpose();

project

There are 2 key points:

1. It’s quite amazing that only 15 points can represent that whole states and covariance so well.
   pay attention to the dimension difference:

   MatrixXd Xsig_aug = MatrixXd(n_aug, 2 * n_aug + 1);
   MatrixXd Xsig_pred_ = MatrixXd(n_x, 2 * n_aug + 1);
   

1. the Kalman gain K is somewhat more complicated to calculate than the extended Kalman filter. Luckily, this is something well established so you can copy some codes.

Detailed implementation is at https://github.com/jychstar/NanoDegreeProject/tree/master/CarND2/p2_unscented_kalman_filter

​