src/ukf.cpp
void UKF::ProcessMeasurement(MeasurementPackage meas_package) {
  /**
  TODO:

  Complete this function! Make sure you switch between lidar and radar
  measurements.
  */
  if (!is_initialized_) {
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      double rho = meas_package.raw_measurements_[0]; // range
      double phi = meas_package.raw_measurements_[1]; // bearing
      double rho_dot = meas_package.raw_measurements_[2]; // velocity of rh
      double x = rho * cos(phi);
      double y = rho * sin(phi);
      double vx = rho_dot * cos(phi);
      double vy = rho_dot * sin(phi);
      double v = sqrt(vx * vx + vy * vy);
      x_ << x, y, v, 0, 0;
    }
    else {
      x_ << meas_package.raw_measurements_[0], meas_package.raw_measurements_[1], 0, 0, 0;
    }

    previous_timestamp_ = meas_package.timestamp_;

    //Done initializing, no need to predict or update
    is_initialized_ = true;
    return;
  }

  double dt = (meas_package.timestamp_ - previous_timestamp_) / 1000000.0;
  previous_timestamp_ = meas_package.timestamp_;

  //PREDICTION STEP
  Prediction(dt);

  //UPDATE STEP
  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.
@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::RADAR) {
      double rho = meas_package.raw_measurements_[0]; 
      double phi = meas_package.raw_measurements_[1]; 
      double rho_dot = meas_package.raw_measurements_[2]; 
      double x = rho * cos(phi);
      double y = rho * sin(phi);
      double vx = rho_dot * cos(phi);
      double vy = rho_dot * sin(phi);
      double v = sqrt(vx * vx + vy * vy);
      x_ << x, y, v, 0, 0;
    }
    else {
      x_ << meas_package.raw_measurements_[0], meas_package.raw_measurements_[1], 0, 0, 0;
    }
    previous_timestamp_ = meas_package.timestamp_;
    is_initialized_ = true;
    return;
  }
  double dt = (meas_package.timestamp_ - previous_timestamp_) / 1000000.0;
  previous_timestamp_ = meas_package.timestamp_;
  Prediction(dt);
  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_ ) { if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR ) { double rho = meas_package . raw_measurements_ [ 0 ] ; double phi = meas_package . raw_measurements_ [ 1 ] ; double rho_dot = meas_package . raw_measurements_ [ 2 ] ; double x = rho * cos ( phi ) ; double y = rho * sin ( phi ) ; double vx = rho_dot * cos ( phi ) ; double vy = rho_dot * sin ( phi ) ; double v = sqrt ( vx * vx + vy * vy ) ; x_ << x , y , v , 0 , 0 ; } else { x_ << meas_package . raw_measurements_ [ 0 ] , meas_package . raw_measurements_ [ 1 ] , 0 , 0 , 0 ; } previous_timestamp_ = meas_package . timestamp_ ; is_initialized_ = true ; return ; } double dt = ( meas_package . timestamp_ - previous_timestamp_ ) / 1000000.0 ; previous_timestamp_ = meas_package . timestamp_ ; Prediction ( dt ) ; 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.
  */ // range // bearing // velocity of rh //Done initializing, no need to predict or update //PREDICTION STEP //UPDATE STEP