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) {
  MatrixXd Xsig = MatrixXd(n_x_, 2 * n_x_ + 1);
  MatrixXd P_sqrt = P_.llt().matrixL();
  Xsig.col(0) = x_;
  for (int i = 0; i < n_x_; i++) {
    Xsig.col(i+1)      = x_ + sqrt(lambda_+n_x_) * P_sqrt.col(i);
    Xsig.col(i+1+n_x_) = x_ - sqrt(lambda_+n_x_) * P_sqrt.col(i);
  }
  VectorXd x_aug = VectorXd(n_aug_);
  MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
  MatrixXd Xsig_aug = MatrixXd(n_aug_, 2*n_aug_+1);
  double lambda_aug = 3 - n_aug_;
  x_aug.head(5) = x_;
  x_aug(5) = 0.0;
  x_aug(6) = 0.0;
  P_aug.fill(0.0);
  P_aug.topLeftCorner(n_x_, n_x_) = P_;
  P_aug(5,5) = std_a_ * std_a_;
  P_aug(6,6) = std_yawdd_ * std_yawdd_;
  MatrixXd P_aug_sqrt = 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_aug+n_aug_) * P_aug_sqrt.col(i);
      Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt(lambda_aug+n_aug_) * P_aug_sqrt.col(i);
  }
  for (int i = 0; i < 2*n_aug_+1; i++) {
    double px       = Xsig_aug(0, i);
    double py       = 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;
    double v_p, yaw_p, yawd_p;
    if (fabs(yawd) > 0.001) {
      px_p = px + (v / yawd) * (sin(yaw + yawd * delta_t) - sin(yaw));
      py_p = py + (v / yawd) * (cos(yaw) - cos(yaw + (yawd * delta_t)));
    }
    else {
      px_p = px + (v * delta_t * cos(yaw));
      py_p = py + (v * delta_t * sin(yaw));
    }
    v_p = v;
    yaw_p = yaw + (yawd * delta_t);
    yawd_p = yawd;
    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);
    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;
  }
  weights_(0) = lambda_aug / (lambda_aug + n_aug_);
  for (int i = 1; i < 2*n_aug_+1; i++) {
    weights_(i) = 0.5 / (n_aug_ + lambda_aug);
  }
  x_.fill(0.0);
  for (int i = 0; i < 2*n_aug_+1; i++) {
    x_ = x_ + weights_(i) * Xsig_pred_.col(i);
  }
  P_.fill(0.0);
  for (int i = 0; i < 2*n_aug_+1; i++) {
    VectorXd x_diff = Xsig_pred_.col(i) - x_;
    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() ;
  }
  cout << "Predicted state" << endl << x_ << endl;
  cout << "Predicted covariance matrix" << endl << P_ << endl << endl;
}
void UKF :: Prediction ( double delta_t ) { MatrixXd Xsig = MatrixXd ( n_x_ , 2 * n_x_ + 1 ) ; MatrixXd P_sqrt = P_ . llt ( ) . matrixL ( ) ; Xsig . col ( 0 ) = x_ ; for ( int i = 0 ; i < n_x_ ; i ++ ) { Xsig . col ( i + 1 ) = x_ + sqrt ( lambda_ + n_x_ ) * P_sqrt . col ( i ) ; Xsig . col ( i + 1 + n_x_ ) = x_ - sqrt ( lambda_ + n_x_ ) * P_sqrt . col ( i ) ; } VectorXd x_aug = VectorXd ( n_aug_ ) ; MatrixXd P_aug = MatrixXd ( n_aug_ , n_aug_ ) ; MatrixXd Xsig_aug = MatrixXd ( n_aug_ , 2 * n_aug_ + 1 ) ; double lambda_aug = 3 - n_aug_ ; x_aug . head ( 5 ) = x_ ; x_aug ( 5 ) = 0.0 ; x_aug ( 6 ) = 0.0 ; P_aug . fill ( 0.0 ) ; P_aug . topLeftCorner ( n_x_ , n_x_ ) = P_ ; P_aug ( 5 , 5 ) = std_a_ * std_a_ ; P_aug ( 6 , 6 ) = std_yawdd_ * std_yawdd_ ; MatrixXd P_aug_sqrt = 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_aug + n_aug_ ) * P_aug_sqrt . col ( i ) ; Xsig_aug . col ( i + 1 + n_aug_ ) = x_aug - sqrt ( lambda_aug + n_aug_ ) * P_aug_sqrt . col ( i ) ; } for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { double px = Xsig_aug ( 0 , i ) ; double py = 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 ; double v_p , yaw_p , yawd_p ; if ( fabs ( yawd ) > 0.001 ) { px_p = px + ( v / yawd ) * ( sin ( yaw + yawd * delta_t ) - sin ( yaw ) ) ; py_p = py + ( v / yawd ) * ( cos ( yaw ) - cos ( yaw + ( yawd * delta_t ) ) ) ; } else { px_p = px + ( v * delta_t * cos ( yaw ) ) ; py_p = py + ( v * delta_t * sin ( yaw ) ) ; } v_p = v ; yaw_p = yaw + ( yawd * delta_t ) ; yawd_p = yawd ; 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 ) ; 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 ; } weights_ ( 0 ) = lambda_aug / ( lambda_aug + n_aug_ ) ; for ( int i = 1 ; i < 2 * n_aug_ + 1 ; i ++ ) { weights_ ( i ) = 0.5 / ( n_aug_ + lambda_aug ) ; } x_ . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { x_ = x_ + weights_ ( i ) * Xsig_pred_ . col ( i ) ; } P_ . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { VectorXd x_diff = Xsig_pred_ . col ( i ) - x_ ; 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 ( ) ; } cout << " " << endl << x_ << endl ; cout << " " << endl << P_ << endl << endl ; }
Predicts sigma points, the state, and the state covariance matrix.
Predicts sigma points the state and the state covariance matrix .
/**
  TODO:

  Complete this function! Estimate the object's location. Modify the state
  vector, x_. Predict sigma points, the state, and the state covariance matrix.
  */ // Generate Sigma Points /// Augment Sigma Points /// Create augmented mean state /// Create augmented covar matrix /// Create square root matrix /// Create augmented sigma points // Sigma Point Prediction /// Predicted values /// Incorporate noise /// Store sigma points // Predict /// Calculate predicted state mean /// Calculate predicted state covariance matrix /// state difference /// angle normalization