src/ukf.cpp
/**
 * Updates the state and the state covariance matrix using a laser measurement.
 * @param {MeasurementPackage} meas_package
 */
Updates the state and the state covariance matrix using a laser measurement.
Updates the state and the state covariance matrix using a laser measurement .
void UKF::UpdateLidar(MeasurementPackage meas_package) {
	int nx = 2;
	VectorXd z_meas = meas_package.raw_measurements_;
	MatrixXd R = MatrixXd(nx,nx);
	R << std_laspx_*std_laspx_, 0,
		 0, std_laspy_*std_laspy_;
	MatrixXd Zsig = MatrixXd(nx, 2*n_aug_+1);
	for (int i = 0; i < 2*n_aug_+1; i++)
	{
		Zsig.col(i) = Xsig_pred_.col(i).head(nx);
	}
	VectorXd z_pred = VectorXd(nx);
	MatrixXd S = MatrixXd(nx, nx);
	measMeanAndCovariance(Zsig, R, z_pred, S, false);
	MatrixXd Tc = MatrixXd(n_x_, nx);
	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();
	}
	VectorXd z_diff = z_meas - z_pred;
	MatrixXd K = Tc*S.inverse();
	x_ = x_ + K*z_diff;
	P_ = P_ - K*S*K.transpose();
}
void UKF :: UpdateLidar ( MeasurementPackage meas_package ) { int nx = 2 ; VectorXd z_meas = meas_package . raw_measurements_ ; MatrixXd R = MatrixXd ( nx , nx ) ; R << std_laspx_ * std_laspx_ , 0 , 0 , std_laspy_ * std_laspy_ ; MatrixXd Zsig = MatrixXd ( nx , 2 * n_aug_ + 1 ) ; for ( int i = 0 ; i < 2 * n_aug_ + 1 ; i ++ ) { Zsig . col ( i ) = Xsig_pred_ . col ( i ) . head ( nx ) ; } VectorXd z_pred = VectorXd ( nx ) ; MatrixXd S = MatrixXd ( nx , nx ) ; measMeanAndCovariance ( Zsig , R , z_pred , S , false ) ; MatrixXd Tc = MatrixXd ( n_x_ , nx ) ; 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 ( ) ; } VectorXd z_diff = z_meas - z_pred ; MatrixXd K = Tc * S . inverse ( ) ; x_ = x_ + K * z_diff ; P_ = P_ - K * S * K . transpose ( ) ; }
Updates the state and the state covariance matrix using a laser measurement.
Updates the state and the state covariance matrix using a laser measurement .
/**
  TODO:

  Complete this function! Use lidar 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 lidar NIS.
  */ //transform sigma points to measurement space //compute mean and covariance of measurement /*isRadar*/