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) {
	MatrixXd H = MatrixXd(2,5);
	H << 1,0,0,0,0,
	  0,1,0,0,0;
	MatrixXd Ht = H.transpose();
	MatrixXd R = MatrixXd(2,2);
	R << std_laspx_*std_laspx_, 0,
	  0, std_laspy_*std_laspy_;
	VectorXd z = meas_package.raw_measurements_;
	VectorXd z_pred = H * x_;
	VectorXd y = z - z_pred;
	MatrixXd S = H * P_ * Ht + R;
	MatrixXd K = P_ * Ht * S.inverse();
	x_ = x_ + K*y;
	long x_size = x_.size();
	MatrixXd I = MatrixXd::Identity(x_size,x_size);
	P_ = (I - K * H) * P_;
	double NIS = y.transpose() * S.inverse() * y;
	if (5.991 < NIS) {
		l_nis_(1)++;
		l_nis_(2)++;
		l_nis_(3)++;
		l_nis_(4)++;
	}
	else if (4.605 < NIS) {
		l_nis_(1)++;
		l_nis_(2)++;
		l_nis_(3)++;
	}
	else if (0.211 < NIS) {
		l_nis_(1)++;
		l_nis_(2)++;
	}
	else if (0.103 < NIS) {
		l_nis_(1)++;
	}
	l_nis_(0)++;
}
void UKF :: UpdateLidar ( MeasurementPackage meas_package ) { MatrixXd H = MatrixXd ( 2 , 5 ) ; H << 1 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 ; MatrixXd Ht = H . transpose ( ) ; MatrixXd R = MatrixXd ( 2 , 2 ) ; R << std_laspx_ * std_laspx_ , 0 , 0 , std_laspy_ * std_laspy_ ; VectorXd z = meas_package . raw_measurements_ ; VectorXd z_pred = H * x_ ; VectorXd y = z - z_pred ; MatrixXd S = H * P_ * Ht + R ; MatrixXd K = P_ * Ht * S . inverse ( ) ; x_ = x_ + K * y ; long x_size = x_ . size ( ) ; MatrixXd I = MatrixXd :: Identity ( x_size , x_size ) ; P_ = ( I - K * H ) * P_ ; double NIS = y . transpose ( ) * S . inverse ( ) * y ; if ( 5.991 < NIS ) { l_nis_ ( 1 ) ++ ; l_nis_ ( 2 ) ++ ; l_nis_ ( 3 ) ++ ; l_nis_ ( 4 ) ++ ; } else if ( 4.605 < NIS ) { l_nis_ ( 1 ) ++ ; l_nis_ ( 2 ) ++ ; l_nis_ ( 3 ) ++ ; } else if ( 0.211 < NIS ) { l_nis_ ( 1 ) ++ ; l_nis_ ( 2 ) ++ ; } else if ( 0.103 < NIS ) { l_nis_ ( 1 ) ++ ; } l_nis_ ( 0 ) ++ ; }
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.
  */ // USE STANDARD KALMAN FILTER EQUATIONS // measurement matrix // noise matrix // equations // new mean and covariance // calculate NIS and put into correct bucket // increment total NIS calculations