src/ukf.cpp
/**
 * @param {MeasurementPackage} meas_package The latest measurement data of
 * either radar or laser.
 */
@param {MeasurementPackage} meas_package The latest measurement data of
either radar or laser.
@param { MeasurementPackage } meas_package The latest measurement data of either radar or laser .
void UKF::ProcessMeasurement(MeasurementPackage meas_package) {
	if (!is_initialized_) {
		cout << "UKF first measurement initializing..." << endl;
		x_.fill(1.0);
		P_ = MatrixXd::Identity(5,5);
		P_ *= 0.7;
		if (meas_package.sensor_type_ == MeasurementPackage::LASER && use_laser_) {
			x_(0) = meas_package.raw_measurements_(0);
			x_(1) = meas_package.raw_measurements_(1);	
		}
		else if (meas_package.sensor_type_ == MeasurementPackage::RADAR && use_radar_) {
			double px = cos(meas_package.raw_measurements_(1))*meas_package.raw_measurements_(0);
			double py = sin(meas_package.raw_measurements_(1))*meas_package.raw_measurements_(0);
			x_(0) = px;
			x_(1) = py;	
		}
		is_initialized_ = true;
		time_us_ = meas_package.timestamp_;
		return;
	}
	double delta_t = (meas_package.timestamp_ - time_us_) / 1000000.0;
	time_us_ = meas_package.timestamp_;
	Prediction(delta_t);
	if (meas_package.sensor_type_ == MeasurementPackage::RADAR && use_radar_) {
		UpdateRadar(meas_package);	
	}
	else if (meas_package.sensor_type_ == MeasurementPackage::LASER && use_laser_) {
		UpdateLidar(meas_package);
	}
}
void UKF :: ProcessMeasurement ( MeasurementPackage meas_package ) { if ( ! is_initialized_ ) { cout << " " << endl ; x_ . fill ( 1.0 ) ; P_ = MatrixXd :: Identity ( 5 , 5 ) ; P_ *= 0.7 ; if ( meas_package . sensor_type_ == MeasurementPackage :: LASER && use_laser_ ) { x_ ( 0 ) = meas_package . raw_measurements_ ( 0 ) ; x_ ( 1 ) = meas_package . raw_measurements_ ( 1 ) ; } else if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR && use_radar_ ) { double px = cos ( meas_package . raw_measurements_ ( 1 ) ) * meas_package . raw_measurements_ ( 0 ) ; double py = sin ( meas_package . raw_measurements_ ( 1 ) ) * meas_package . raw_measurements_ ( 0 ) ; x_ ( 0 ) = px ; x_ ( 1 ) = py ; } is_initialized_ = true ; time_us_ = meas_package . timestamp_ ; return ; } double delta_t = ( meas_package . timestamp_ - time_us_ ) / 1000000.0 ; time_us_ = meas_package . timestamp_ ; Prediction ( delta_t ) ; if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR && use_radar_ ) { UpdateRadar ( meas_package ) ; } else if ( meas_package . sensor_type_ == MeasurementPackage :: LASER && use_laser_ ) { UpdateLidar ( meas_package ) ; } }
@param {MeasurementPackage} meas_package The latest measurement data of
 either radar or laser.
@param { MeasurementPackage } meas_package The latest measurement data of either radar or laser .
/**
  TODO:

  Complete this function! Make sure you switch between lidar and radar
  measurements.
  */ // check if first measurement // initialize state vector x_ as 1's // initialize covariance P_ as identity matrix // check measurement type and use_ variable // set state vector px and py with laser measurements // convert radar measurements from polar to cartesian // set state vector px and py with radar measurements // finish first measurement initialization // calculate time change // call prediction update // call measurement update