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_)
	{
		if (meas_package.sensor_type_ == MeasurementPackage::LASER)
		{
			x_ << meas_package.raw_measurements_[0], meas_package.raw_measurements_[1], 0, 0, 0;
		}
		else
		{
			float ro = meas_package.raw_measurements_[0];
			float theta = meas_package.raw_measurements_[1];
			float rodot = meas_package.raw_measurements_[2];
			float v = sqrt(rodot*cos(theta)*rodot*cos(theta) + rodot*sin(theta)*rodot*sin(theta));
			x_ << ro*cos(theta), ro*sin(theta), v, 0, 0;
		}
		is_initialized_ = true;
		time_us_ = meas_package.timestamp_;
		return;
	}
	if ((meas_package.sensor_type_ == MeasurementPackage::LASER && use_laser_) ||
		(meas_package.sensor_type_ == MeasurementPackage::RADAR && use_radar_))
	{
		double delta_t = (meas_package.timestamp_ - time_us_)/1000000.0;
		time_us_ = meas_package.timestamp_;
		Prediction(delta_t);
	}
	if (meas_package.sensor_type_ == MeasurementPackage::LASER && use_laser_)
	{
		UpdateLidar(meas_package);
	}
	else if (meas_package.sensor_type_ == MeasurementPackage::RADAR && use_radar_)
	{
		UpdateRadar(meas_package);
	}
}
void UKF :: ProcessMeasurement ( MeasurementPackage meas_package ) { if ( ! is_initialized_ ) { if ( meas_package . sensor_type_ == MeasurementPackage :: LASER ) { x_ << meas_package . raw_measurements_ [ 0 ] , meas_package . raw_measurements_ [ 1 ] , 0 , 0 , 0 ; } else { float ro = meas_package . raw_measurements_ [ 0 ] ; float theta = meas_package . raw_measurements_ [ 1 ] ; float rodot = meas_package . raw_measurements_ [ 2 ] ; float v = sqrt ( rodot * cos ( theta ) * rodot * cos ( theta ) + rodot * sin ( theta ) * rodot * sin ( theta ) ) ; x_ << ro * cos ( theta ) , ro * sin ( theta ) , v , 0 , 0 ; } is_initialized_ = true ; time_us_ = meas_package . timestamp_ ; return ; } if ( ( meas_package . sensor_type_ == MeasurementPackage :: LASER && use_laser_ ) || ( meas_package . sensor_type_ == MeasurementPackage :: RADAR && use_radar_ ) ) { double delta_t = ( meas_package . timestamp_ - time_us_ ) / 1000000.0 ; time_us_ = meas_package . timestamp_ ; Prediction ( delta_t ) ; } if ( meas_package . sensor_type_ == MeasurementPackage :: LASER && use_laser_ ) { UpdateLidar ( meas_package ) ; } else if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR && use_radar_ ) { UpdateRadar ( 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.
  */ //initialize state with first measurement //RADAR measurement //call prediction function //call update function based on type of sensor