Sigma Point Prediction Assignment 1.cpp

/*******************************************************************************
 * Student part begin
 ******************************************************************************/

  //predict sigma points
  //avoid division by zero
  //write predicted sigma points into right column
  
  for (int i = 0; i < 2 * n_aug + 1; i++) {
      VectorXd x = Xsig_aug.col(i);
      double p_x = x[0];
      double p_y = x[1];
      double v = x[2];
      double yaw = x[3];
      double yaw_dot = x[4];
      double nu_a = x[5];
      double nu_yaw_dot_dot = x[6];
      
      if (fabs(yaw_dot) > 0.001) {
        p_x = p_x + v/yaw_dot*(sin(yaw+yaw_dot*delta_t)-sin(yaw));
        p_y = p_y + v/yaw_dot*(-cos(yaw+yaw_dot*delta_t)+cos(yaw));
        yaw = yaw + yaw_dot*delta_t;
      } else {
        p_x = p_x + v*delta_t*cos(yaw);
        p_y = p_y + v*delta_t*sin(yaw);
        yaw = yaw + yaw_dot*delta_t;
      }
      
      p_x = p_x+0.5*delta_t*delta_t*cos(yaw)*nu_a;
      p_y = p_y+0.5*delta_t*delta_t*sin(yaw)*nu_a;
      v = v+delta_t*nu_a;
      yaw = yaw+0.5*delta_t*delta_t*v*nu_yaw_dot_dot;
      yaw_dot = yaw_dot+delta_t*nu_yaw_dot_dot;
      
      VectorXd x_k1 = VectorXd(5);
      x_k1 << p_x, p_y, v, yaw, yaw_dot;

      Xsig_pred.col(i) = x_k1;
  }
  

/*******************************************************************************
 * Student part end
 ******************************************************************************/