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);
	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_;
	MatrixXd A = P_aug.llt().matrixL();
	MatrixXd Xsig_aug = MatrixXd(n_aug_, 2*n_aug_+1);
	Xsig_aug.fill(0.0);
	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.col(i);
		Xsig_aug.col(i+n_aug_+1) = x_aug - sqrt(lambda_+n_aug_)*A.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 yaw_dot = Xsig_aug(4,i);
		double noise_a = Xsig_aug(5,i);
		double noise_yaw = Xsig_aug(6,i);
		double px_pred, py_pred, v_pred, yaw_pred, yaw_dot_pred;
		if (fabs(yaw_dot) > 0.001) {
			px_pred = px + v/yaw_dot*(sin(yaw + yaw_dot*delta_t) - sin(yaw));
			py_pred = py + v/yaw_dot*(-cos(yaw + yaw_dot*delta_t) + cos(yaw));
		}
		else {
			px_pred = px + v*cos(yaw)*delta_t;
			py_pred = py + v*sin(yaw)*delta_t;
		}
		v_pred = v;
		yaw_pred = yaw + yaw_dot*delta_t;
		yaw_dot_pred = yaw_dot;
		px_pred += 0.5*(delta_t*delta_t)*cos(yaw)*noise_a;
		py_pred += 0.5*(delta_t*delta_t)*sin(yaw)*noise_a;
		v_pred += delta_t*noise_a;
		yaw_pred += 0.5*(delta_t*delta_t)*noise_yaw;
		yaw_dot_pred += delta_t*noise_yaw;
		Xsig_pred_(0,i) = px_pred;
		Xsig_pred_(1,i) = py_pred;
		Xsig_pred_(2,i) = v_pred;
		Xsig_pred_(3,i) = yaw_pred;
		Xsig_pred_(4,i) = yaw_dot_pred;
	}
	//////
	//////
	VectorXd x_pred = VectorXd(n_x_);
	x_pred.fill(0.0);
	for (int i=0; i < 2*n_aug_+1; ++i) {
		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 = Xsig_pred_.col(i) - x_pred;
		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_pred += weights_(i)*x_diff*x_diff.transpose();
	}
	x_ = x_pred;
	P_ = P_pred;
}
void UKF :: Prediction ( double delta_t ) { VectorXd x_aug = VectorXd ( n_aug_ ) ; x_aug . fill ( 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_ ; MatrixXd A = P_aug . llt ( ) . matrixL ( ) ; MatrixXd Xsig_aug = MatrixXd ( n_aug_ , 2 * n_aug_ + 1 ) ; Xsig_aug . fill ( 0.0 ) ; 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 . col ( i ) ; Xsig_aug . col ( i + n_aug_ + 1 ) = x_aug - sqrt ( lambda_ + n_aug_ ) * A . 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 yaw_dot = Xsig_aug ( 4 , i ) ; double noise_a = Xsig_aug ( 5 , i ) ; double noise_yaw = Xsig_aug ( 6 , i ) ; double px_pred , py_pred , v_pred , yaw_pred , yaw_dot_pred ; if ( fabs ( yaw_dot ) > 0.001 ) { px_pred = px + v / yaw_dot * ( sin ( yaw + yaw_dot * delta_t ) - sin ( yaw ) ) ; py_pred = py + v / yaw_dot * ( - cos ( yaw + yaw_dot * delta_t ) + cos ( yaw ) ) ; } else { px_pred = px + v * cos ( yaw ) * delta_t ; py_pred = py + v * sin ( yaw ) * delta_t ; } v_pred = v ; yaw_pred = yaw + yaw_dot * delta_t ; yaw_dot_pred = yaw_dot ; px_pred += 0.5 * ( delta_t * delta_t ) * cos ( yaw ) * noise_a ; py_pred += 0.5 * ( delta_t * delta_t ) * sin ( yaw ) * noise_a ; v_pred += delta_t * noise_a ; yaw_pred += 0.5 * ( delta_t * delta_t ) * noise_yaw ; yaw_dot_pred += delta_t * noise_yaw ; Xsig_pred_ ( 0 , i ) = px_pred ; Xsig_pred_ ( 1 , i ) = py_pred ; Xsig_pred_ ( 2 , i ) = v_pred ; Xsig_pred_ ( 3 , i ) = yaw_pred ; Xsig_pred_ ( 4 , i ) = yaw_dot_pred ; } VectorXd x_pred = VectorXd ( n_x_ ) ; x_pred . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; ++ i ) { 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 = Xsig_pred_ . col ( i ) - x_pred ; 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_pred += weights_ ( i ) * x_diff * x_diff . 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 .
/**
  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 //////////////////////// // create augmented state vector // create augmented covariance matrix // calculate square root matrix // create augmented sigma points //////////////////////// // PREDICTE SIGMA POINTS //////////////////////// // loop through each sigma point // define intermediate state variables for readability // predicted location variables // set px and py predictions and avoid 0 division error // set v, yaw, and yaw_dot predictions // add noise // add predicted sigma points to Xsig_pred matrix ////////////////////////////// // PREDICT MEAN AND COVARIANCE ////////////////////////////// // predict mean // predict covariance // calculate state diff // normalize angle // calculate P  // set x_ and P_ to predicted mean and covariance