src/ukf.cpp
/**
 * Predicts sigma points, the state, and the state covariance matrix.
 * @param {double} delta_t the change in time (in seconds) between the last
 * measurement and this one.
 */
Predicts sigma points, the state, and the state covariance matrix.
@param {double} delta_t the change in time (in seconds) between the last
measurement and this one.
Predicts sigma points the state and the state covariance matrix . @param { double } delta_t the change in time ( in seconds ) between the last measurement and this one .
void UKF::Prediction(double delta_t) {
  VectorXd x_aug = VectorXd(n_aug_);
  x_aug.fill(0.0);
  MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
  P_aug.fill(0.0);
  MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);
  Xsig_aug.fill(0.0);
  x_aug.head(5) = x_;
  x_aug(5) = 0;
  x_aug(6) = 0;
  MatrixXd proc_noise = MatrixXd(2,2);
  proc_noise << std_a_ * std_a_, 0,
         0, std_yawdd_ * std_yawdd_;
  P_aug.topLeftCorner(n_x_, n_x_) = P_;
  P_aug.bottomRightCorner(2, 2) = proc_noise;
  MatrixXd A_aug = P_aug.llt().matrixL();
  Xsig_aug.col(0) = x_aug;
  for(int i = 0; i < n_aug_; i++)
  {
      Xsig_aug.col(i+1) = x_aug + (sqrt(lambda_ + n_aug_) * A_aug.col(i));
      Xsig_aug.col(i+1+n_aug_) = x_aug - (sqrt(lambda_ + n_aug_) * A_aug.col(i));
  }
  for(int i = 0; i < 2*n_aug_+1; i++)
  {
      float px      = Xsig_aug(0, i);
      float py      = Xsig_aug(1, i);
      float v       = Xsig_aug(2, i);
      float u       = Xsig_aug(3, i);
      float u_dot   = Xsig_aug(4, i);
      float va      = Xsig_aug(5, i);
      float v_dot   = Xsig_aug(6, i);
      float delta_t_squared = (0.5 * (delta_t * delta_t));
      if(fabs(u_dot) < 0.001)
      {
          Xsig_pred_(0,i) = (v * cos(u) * delta_t) + (delta_t_squared * cos(u) * va);
          Xsig_pred_(1,i) = (v * sin(u) * delta_t) + (delta_t_squared * sin(u) * va);
      }
      else
      {
          Xsig_pred_(0,i) = (v/u_dot)*(sin(u + u_dot * delta_t) - sin(u));
          Xsig_pred_(0,i) += (delta_t_squared * cos(u) * va);
          Xsig_pred_(1,i) = (v/u_dot)*((-1.0 * cos(u + u_dot * delta_t)) + cos(u));
          Xsig_pred_(1,i) += (delta_t_squared * sin(u) * va);
      }
      Xsig_pred_(2,i) = delta_t * va;
      Xsig_pred_(3,i) = (delta_t * u_dot) + delta_t_squared * v_dot;
      Xsig_pred_(4,i) = (delta_t * v_dot);
      Xsig_pred_(0,i) += px;
      Xsig_pred_(1,i) += py;
      Xsig_pred_(2,i) += v;
      Xsig_pred_(3,i) += u;
      Xsig_pred_(4,i) += u_dot;
  }
  VectorXd x_pred = VectorXd(n_x_);
  x_pred.fill(0.0);
  for(int i = 0; i < (2*n_aug_+1); i++)
  {
      x_pred = x_pred + weights_(i)*Xsig_pred_.col(i);
  }
  MatrixXd P_pred = MatrixXd(n_x_, n_x_);
  P_pred.fill(0.0);
  for(int i = 0; i < (2*n_aug_+1); i++)
  {
      VectorXd x_diff_temp = Xsig_pred_.col(i) - x_pred;
      while(x_diff_temp(3) > M_PI)
      {
        x_diff_temp(3) -= 2.0 * M_PI;
      }
      while(x_diff_temp(3) < -M_PI)
      {
        x_diff_temp(3) += 2.0 * M_PI;
      }
      P_pred = P_pred + weights_(i) * (x_diff_temp) * (x_diff_temp.transpose());
  }
  x_ = x_pred;
  P_ = P_pred;
}
void UKF :: Prediction ( double delta_t ) { VectorXd x_aug = VectorXd ( n_aug_ ) ; x_aug . fill ( 0.0 ) ; MatrixXd P_aug = MatrixXd ( n_aug_ , n_aug_ ) ; P_aug . fill ( 0.0 ) ; MatrixXd Xsig_aug = MatrixXd ( n_aug_ , 2 * n_aug_ + 1 ) ; Xsig_aug . fill ( 0.0 ) ; x_aug . head ( 5 ) = x_ ; x_aug ( 5 ) = 0 ; x_aug ( 6 ) = 0 ; MatrixXd proc_noise = MatrixXd ( 2 , 2 ) ; proc_noise << std_a_ * std_a_ , 0 , 0 , std_yawdd_ * std_yawdd_ ; P_aug . topLeftCorner ( n_x_ , n_x_ ) = P_ ; P_aug . bottomRightCorner ( 2 , 2 ) = proc_noise ; MatrixXd A_aug = P_aug . llt ( ) . matrixL ( ) ; Xsig_aug . col ( 0 ) = x_aug ; for ( int i = 0 ; i < n_aug_ ; i ++ ) { Xsig_aug . col ( i + 1 ) = x_aug + ( sqrt ( lambda_ + n_aug_ ) * A_aug . col ( i ) ) ; Xsig_aug . col ( i + 1 + n_aug_ ) = x_aug - ( sqrt ( lambda_ + n_aug_ ) * A_aug . col ( i ) ) ; } for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { float px = Xsig_aug ( 0 , i ) ; float py = Xsig_aug ( 1 , i ) ; float v = Xsig_aug ( 2 , i ) ; float u = Xsig_aug ( 3 , i ) ; float u_dot = Xsig_aug ( 4 , i ) ; float va = Xsig_aug ( 5 , i ) ; float v_dot = Xsig_aug ( 6 , i ) ; float delta_t_squared = ( 0.5 * ( delta_t * delta_t ) ) ; if ( fabs ( u_dot ) < 0.001 ) { Xsig_pred_ ( 0 , i ) = ( v * cos ( u ) * delta_t ) + ( delta_t_squared * cos ( u ) * va ) ; Xsig_pred_ ( 1 , i ) = ( v * sin ( u ) * delta_t ) + ( delta_t_squared * sin ( u ) * va ) ; } else { Xsig_pred_ ( 0 , i ) = ( v / u_dot ) * ( sin ( u + u_dot * delta_t ) - sin ( u ) ) ; Xsig_pred_ ( 0 , i ) += ( delta_t_squared * cos ( u ) * va ) ; Xsig_pred_ ( 1 , i ) = ( v / u_dot ) * ( ( -1.0 * cos ( u + u_dot * delta_t ) ) + cos ( u ) ) ; Xsig_pred_ ( 1 , i ) += ( delta_t_squared * sin ( u ) * va ) ; } Xsig_pred_ ( 2 , i ) = delta_t * va ; Xsig_pred_ ( 3 , i ) = ( delta_t * u_dot ) + delta_t_squared * v_dot ; Xsig_pred_ ( 4 , i ) = ( delta_t * v_dot ) ; Xsig_pred_ ( 0 , i ) += px ; Xsig_pred_ ( 1 , i ) += py ; Xsig_pred_ ( 2 , i ) += v ; Xsig_pred_ ( 3 , i ) += u ; Xsig_pred_ ( 4 , i ) += u_dot ; } VectorXd x_pred = VectorXd ( n_x_ ) ; x_pred . fill ( 0.0 ) ; for ( int i = 0 ; i < ( 2 * n_aug_ + 1 ) ; i ++ ) { x_pred = x_pred + weights_ ( i ) * Xsig_pred_ . col ( i ) ; } MatrixXd P_pred = MatrixXd ( n_x_ , n_x_ ) ; P_pred . fill ( 0.0 ) ; for ( int i = 0 ; i < ( 2 * n_aug_ + 1 ) ; i ++ ) { VectorXd x_diff_temp = Xsig_pred_ . col ( i ) - x_pred ; while ( x_diff_temp ( 3 ) > M_PI ) { x_diff_temp ( 3 ) -= 2.0 * M_PI ; } while ( x_diff_temp ( 3 ) < - M_PI ) { x_diff_temp ( 3 ) += 2.0 * M_PI ; } P_pred = P_pred + weights_ ( i ) * ( x_diff_temp ) * ( x_diff_temp . transpose ( ) ) ; } x_ = x_pred ; P_ = P_pred ; }
Predicts sigma points, the state, and the state covariance matrix.
Predicts sigma points the state and the state covariance matrix .
// L14 - create augmented sigma points // augmented state covariance // augmented sigma point matrix // augmented mean state // augmented covariance   // augmented sigma points // Augmented sigma points created... predict sigma points //std::cout << px << " " << py << " " << v << " " << u << " " << u_dot << " " << va << std::endl;  // Predict Mean and Covariance //std::cout << "elm: " << std::endl << Xsig_pred(0,i) << std::endl; // normalize // KDA don't forget to normalize angle difference      // Update mean and covariance with predicitions //std::cout << "Prediction" << std::endl; //std::cout << x_ << std::endl; //std::cout << P_ << std::endl;