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_) {
    time_us_ = meas_package.timestamp_;
    x_ << 1, 1, 1, 1, 0.1;
    P_ << 0.15,   0,  0, 0, 0,
             0, 0.15, 0, 0, 0,
             0,   0,  1, 0, 0,
             0,   0,  0, 1, 0,
             0,   0,  0, 0, 1;
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      cout << "Initialize with Radar" << endl;
      double rho = meas_package.raw_measurements_(0);
      double phi = meas_package.raw_measurements_(1);
      x_(0) = rho * cos(phi);
      x_(1) = rho * sin(phi);
    }
    else if (meas_package.sensor_type_ == MeasurementPackage::LASER) {
      cout << "Initialize with Laser" << endl;
      x_(0) = meas_package.raw_measurements_(0);
      x_(1) = meas_package.raw_measurements_(1);
    }
    is_initialized_ = true;
    return;
  }
  double dt = (meas_package.timestamp_ - time_us_) / 1000000.0;	
	time_us_ = meas_package.timestamp_;
  Prediction(dt);
  if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
    UpdateRadar(meas_package);
  }
  else if (meas_package.sensor_type_ == MeasurementPackage::LASER) {
    UpdateLidar(meas_package);
  }
  cout << "x_ = " << x_ << endl;
  cout << "P_ = " << P_ << endl;
}
void UKF :: ProcessMeasurement ( MeasurementPackage meas_package ) { if ( ! is_initialized_ ) { time_us_ = meas_package . timestamp_ ; x_ << 1 , 1 , 1 , 1 , 0.1 ; P_ << 0.15 , 0 , 0 , 0 , 0 , 0 , 0.15 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 1 , 0 , 0 , 0 , 0 , 0 , 1 ; if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR ) { cout << " " << endl ; double rho = meas_package . raw_measurements_ ( 0 ) ; double phi = meas_package . raw_measurements_ ( 1 ) ; x_ ( 0 ) = rho * cos ( phi ) ; x_ ( 1 ) = rho * sin ( phi ) ; } else if ( meas_package . sensor_type_ == MeasurementPackage :: LASER ) { cout << " " << endl ; x_ ( 0 ) = meas_package . raw_measurements_ ( 0 ) ; x_ ( 1 ) = meas_package . raw_measurements_ ( 1 ) ; } is_initialized_ = true ; return ; } double dt = ( meas_package . timestamp_ - time_us_ ) / 1000000.0 ; time_us_ = meas_package . timestamp_ ; Prediction ( dt ) ; if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR ) { UpdateRadar ( meas_package ) ; } else if ( meas_package . sensor_type_ == MeasurementPackage :: LASER ) { UpdateLidar ( meas_package ) ; } cout << " " << x_ << endl ; cout << " " << P_ << endl ; }
@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.
  */ //dt - expressed in seconds