## 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(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++){
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
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() ;
}


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();
}
MatrixXd R = MatrixXd(n_z,n_z);
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.