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);
  x_aug.head(n_x_) = x_;
  MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
  P_aug.fill(0.0);
  P_aug.topLeftCorner(n_x_, n_x_) = P_;
  P_aug(n_x_,n_x_) = std_a_*std_a_;
  P_aug(n_x_+1,n_x_+1) = std_yawdd_*std_yawdd_;
  PredictSigmaPoints(x_aug, P_aug, delta_t); 
  weights_(0) = lambda_/(lambda_+n_aug_);
  for (int i = 1; i < 2*n_aug_+1; i++)
  {
	  weights_(i) = 0.5/(lambda_+n_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 xdiff = Xsig_pred_.col(i) - x_;
		  if (xdiff(3) > M_PI) xdiff(3) -= 2*M_PI;
		  else if (xdiff(3) < -M_PI) xdiff(3) += 2*M_PI;
		  P_ = P_ + weights_(i)*xdiff*xdiff.transpose();
	  }
  }
void UKF :: Prediction ( double delta_t ) { VectorXd x_aug = VectorXd ( n_aug_ ) ; x_aug . fill ( 0.0 ) ; x_aug . head ( n_x_ ) = x_ ; MatrixXd P_aug = MatrixXd ( n_aug_ , n_aug_ ) ; P_aug . fill ( 0.0 ) ; P_aug . topLeftCorner ( n_x_ , n_x_ ) = P_ ; P_aug ( n_x_ , n_x_ ) = std_a_ * std_a_ ; P_aug ( n_x_ + 1 , n_x_ + 1 ) = std_yawdd_ * std_yawdd_ ; PredictSigmaPoints ( x_aug , P_aug , delta_t ) ; weights_ ( 0 ) = lambda_ / ( lambda_ + n_aug_ ) ; for ( int i = 1 ; i < 2 * n_aug_ + 1 ; i ++ ) { weights_ ( i ) = 0.5 / ( lambda_ + n_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 xdiff = Xsig_pred_ . col ( i ) - x_ ; if ( xdiff ( 3 ) > M_PI ) xdiff ( 3 ) -= 2 * M_PI ; else if ( xdiff ( 3 ) < - M_PI ) xdiff ( 3 ) += 2 * M_PI ; P_ = P_ + weights_ ( i ) * xdiff * xdiff . transpose ( ) ; } }
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.
  */ //populates Xsig_pred_ //Predict state mean and covariance