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

  Complete this function! Make sure you switch between lidar and radar
  measurements.
  */

  if (!use_laser_ && meas_package.sensor_type_ == MeasurementPackage::LASER) {
    return;
  }

  if (!use_radar_ && meas_package.sensor_type_ == MeasurementPackage::RADAR) {
    return;
  }

  /*****************************************************************************
   *  Initialization
   ****************************************************************************/
  if (!is_initialized_) {

    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      /**
      Convert radar from polar to cartesian coordinates and initialize state.
      */
      float rho = meas_package.raw_measurements_(0);
      float phi = meas_package.raw_measurements_(1);
      float rho_dot = meas_package.raw_measurements_(2);

      x_(0) = rho * cos(phi);
      x_(1) = rho * sin(phi);
      x_(2) = 0;
      x_(3) = 0;
      x_(4) = 0;
    }
    else if (meas_package.sensor_type_ == MeasurementPackage::LASER) {
      /**
      Initialize state.
      */

      x_(0) = meas_package.raw_measurements_(0);
      x_(1) = meas_package.raw_measurements_(1);
      x_(2) = 0;
      x_(3) = 0;
      x_(4) = 0;
    }

    P_ = MatrixXd::Identity(n_x_, n_x_);
    /*
    P_ << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1000, 0,
        0, 0, 0, 1000;
    */

    previous_timestamp_ = meas_package.timestamp_;

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

  double delta_t = (meas_package.timestamp_ - previous_timestamp_) / 1000000.0; //dt - expressed in seconds
  previous_timestamp_ = meas_package.timestamp_;

  if (delta_t > 0) {
    Prediction(delta_t);

    /*****************************************************************************
     *  Update
     ****************************************************************************/

    UpdateGeneric(meas_package, meas_package.sensor_type_);
    /*
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      UpdateRadar(meas_package);
    } else {
      UpdateLidar(meas_package);
    }*/
  }

  return;
}
/**
 * @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 (!use_laser_ && meas_package.sensor_type_ == MeasurementPackage::LASER) {
    return;
  }
  if (!use_radar_ && meas_package.sensor_type_ == MeasurementPackage::RADAR) {
    return;
  }
  if (!is_initialized_) {
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      float rho = meas_package.raw_measurements_(0);
      float phi = meas_package.raw_measurements_(1);
      float rho_dot = meas_package.raw_measurements_(2);
      x_(0) = rho * cos(phi);
      x_(1) = rho * sin(phi);
      x_(2) = 0;
      x_(3) = 0;
      x_(4) = 0;
    }
    else if (meas_package.sensor_type_ == MeasurementPackage::LASER) {
      x_(0) = meas_package.raw_measurements_(0);
      x_(1) = meas_package.raw_measurements_(1);
      x_(2) = 0;
      x_(3) = 0;
      x_(4) = 0;
    }
    P_ = MatrixXd::Identity(n_x_, n_x_);
    previous_timestamp_ = meas_package.timestamp_;
    is_initialized_ = true;
    return;
  }
  double delta_t = (meas_package.timestamp_ - previous_timestamp_) / 1000000.0; 
  previous_timestamp_ = meas_package.timestamp_;
  if (delta_t > 0) {
    Prediction(delta_t);
    UpdateGeneric(meas_package, meas_package.sensor_type_);
  }
  return;
}
void UKF :: ProcessMeasurement ( MeasurementPackage meas_package ) { if ( ! use_laser_ && meas_package . sensor_type_ == MeasurementPackage :: LASER ) { return ; } if ( ! use_radar_ && meas_package . sensor_type_ == MeasurementPackage :: RADAR ) { return ; } if ( ! is_initialized_ ) { if ( meas_package . sensor_type_ == MeasurementPackage :: RADAR ) { float rho = meas_package . raw_measurements_ ( 0 ) ; float phi = meas_package . raw_measurements_ ( 1 ) ; float rho_dot = meas_package . raw_measurements_ ( 2 ) ; x_ ( 0 ) = rho * cos ( phi ) ; x_ ( 1 ) = rho * sin ( phi ) ; x_ ( 2 ) = 0 ; x_ ( 3 ) = 0 ; x_ ( 4 ) = 0 ; } else if ( meas_package . sensor_type_ == MeasurementPackage :: LASER ) { x_ ( 0 ) = meas_package . raw_measurements_ ( 0 ) ; x_ ( 1 ) = meas_package . raw_measurements_ ( 1 ) ; x_ ( 2 ) = 0 ; x_ ( 3 ) = 0 ; x_ ( 4 ) = 0 ; } P_ = MatrixXd :: Identity ( n_x_ , n_x_ ) ; previous_timestamp_ = meas_package . timestamp_ ; is_initialized_ = true ; return ; } double delta_t = ( meas_package . timestamp_ - previous_timestamp_ ) / 1000000.0 ; previous_timestamp_ = meas_package . timestamp_ ; if ( delta_t > 0 ) { Prediction ( delta_t ) ; UpdateGeneric ( meas_package , meas_package . sensor_type_ ) ; } return ; }
@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.
  */ /*****************************************************************************
   *  Initialization
   ****************************************************************************/ /**
      Convert radar from polar to cartesian coordinates and initialize state.
      */ /**
      Initialize state.
      */ /*
    P_ << 1, 0, 0, 0,
        0, 1, 0, 0,
        0, 0, 1000, 0,
        0, 0, 0, 1000;
    */ // done initializing, no need to predict or update //dt - expressed in seconds /*****************************************************************************
     *  Update
     ****************************************************************************/ /*
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      UpdateRadar(meas_package);
    } else {
      UpdateLidar(meas_package);
    }*/