src/ukf.cpp
/**
 * Updates the state and the state covariance matrix using a radar measurement.
 * @param {MeasurementPackage} meas_package
 */
Updates the state and the state covariance matrix using a radar measurement.
Updates the state and the state covariance matrix using a radar measurement .
void UKF::UpdateRadar(MeasurementPackage meas_package) {
	VectorXd z_meas = meas_package.raw_measurements_;
	int n_z = 3; 
	MatrixXd Zsig = MatrixXd(n_z, 2*n_aug_+1);
	MatrixXd R = MatrixXd(n_z,n_z);
	R << std_radr_*std_radr_, 0, 0,
		 0, std_radphi_*std_radphi_, 0,
		 0, 0, std_radrd_*std_radrd_;
	for (int i = 0; i < 2*n_aug_+1; i++)
	{
		double px = Xsig_pred_(0,i);
		double py = Xsig_pred_(1,i);
		double v = Xsig_pred_(2,i);
		double yaw = Xsig_pred_(3,i);
		double vx = v*cos(yaw);
		double vy = v*sin(yaw);
		Zsig(0,i) = sqrt(px*px + py*py);
		Zsig(1,i) = atan2(py,px);
		if (fabs(Zsig(0,i)) > 0.001)
		{
			Zsig(2,i) = (px*vx + py*vy)/Zsig(0,i);
		}
		else
			Zsig(2,i) = 0;
	}
	VectorXd z_pred = VectorXd(n_z);
	MatrixXd S = MatrixXd(n_z, n_z);
	measMeanAndCovariance(Zsig, R, z_pred, S, true);
	MatrixXd Tc = MatrixXd(n_x_, n_z);
	Tc.fill(0.0);
	for (int i = 0; i < 2*n_aug_+1; i++)
	{
		VectorXd xdiff = Xsig_pred_.col(i) - x_;
		VectorXd zdiff = Zsig.col(i) - z_pred;
		Tc += weights_(i)*xdiff*zdiff.transpose();
	}
	MatrixXd K = Tc*S.inverse();
	VectorXd z_diff = z_meas - z_pred;
	x_ = x_ + K * z_diff;
	P_ = P_ - K * S * K.transpose();
}
void UKF :: UpdateRadar ( MeasurementPackage meas_package ) { VectorXd z_meas = meas_package . raw_measurements_ ; int n_z = 3 ; MatrixXd Zsig = MatrixXd ( n_z , 2 * n_aug_ + 1 ) ; MatrixXd R = MatrixXd ( n_z , n_z ) ; R << std_radr_ * std_radr_ , 0 , 0 , 0 , std_radphi_ * std_radphi_ , 0 , 0 , 0 , std_radrd_ * std_radrd_ ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { double px = Xsig_pred_ ( 0 , i ) ; double py = Xsig_pred_ ( 1 , i ) ; double v = Xsig_pred_ ( 2 , i ) ; double yaw = Xsig_pred_ ( 3 , i ) ; double vx = v * cos ( yaw ) ; double vy = v * sin ( yaw ) ; Zsig ( 0 , i ) = sqrt ( px * px + py * py ) ; Zsig ( 1 , i ) = atan2 ( py , px ) ; if ( fabs ( Zsig ( 0 , i ) ) > 0.001 ) { Zsig ( 2 , i ) = ( px * vx + py * vy ) / Zsig ( 0 , i ) ; } else Zsig ( 2 , i ) = 0 ; } VectorXd z_pred = VectorXd ( n_z ) ; MatrixXd S = MatrixXd ( n_z , n_z ) ; measMeanAndCovariance ( Zsig , R , z_pred , S , true ) ; MatrixXd Tc = MatrixXd ( n_x_ , n_z ) ; Tc . fill ( 0.0 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { VectorXd xdiff = Xsig_pred_ . col ( i ) - x_ ; VectorXd zdiff = Zsig . col ( i ) - z_pred ; Tc += weights_ ( i ) * xdiff * zdiff . transpose ( ) ; } MatrixXd K = Tc * S . inverse ( ) ; VectorXd z_diff = z_meas - z_pred ; x_ = x_ + K * z_diff ; P_ = P_ - K * S * K . transpose ( ) ; }
Updates the state and the state covariance matrix using a radar measurement.
Updates the state and the state covariance matrix using a radar measurement .
/**
  TODO:

  Complete this function! Use radar data to update the belief about the object's
  position. Modify the state vector, x_, and covariance, P_.

  You'll also need to calculate the radar NIS.
  */ //compute predicted measurement mean z and covariance S //radar measurement //compute mean and covariance of measurement prediction /*isRadar*/ //update measurement mean and covariance