src/FusionEKF.cpp
/*
 *! Execute first step of tracking.
 @param[in] measuement_pack ; measurement dataset
 @return true: Initialization completion, false; the following tracking.
*/
Execute first step of tracking.
Execute first step of tracking .
bool FusionEKF::InitialTracking(const MeasurementPackage &measurement_pack) {
  if (!is_initialized_) {
    this->InitializeSelf();
    cout << "EKF: " << endl;
    if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
      float ro;
      float theta;
      ro = measurement_pack.raw_measurements_(0);
      theta = measurement_pack.raw_measurements_(1);
      x_state_(0) =  ro * cos(theta);
      x_state_(1) =  ro * sin(theta);
      ekf_.Init(x_state_, P_, ekf_.F_, Hj_, R_radar_, ekf_.Q_);
    } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
      x_state_(0) = measurement_pack.raw_measurements_(0);
      x_state_(1) = measurement_pack.raw_measurements_(1);
      ekf_.Init(x_state_, P_, ekf_.F_, H_laser_, R_laser_, ekf_.Q_);
    }
    previous_timestamp_ = measurement_pack.timestamp_;
    is_initialized_ = true;
    return true;
  }
  return false;
}
bool FusionEKF :: InitialTracking ( const MeasurementPackage & measurement_pack ) { if ( ! is_initialized_ ) { this -> InitializeSelf ( ) ; cout << " " << endl ; if ( measurement_pack . sensor_type_ == MeasurementPackage :: RADAR ) { float ro ; float theta ; ro = measurement_pack . raw_measurements_ ( 0 ) ; theta = measurement_pack . raw_measurements_ ( 1 ) ; x_state_ ( 0 ) = ro * cos ( theta ) ; x_state_ ( 1 ) = ro * sin ( theta ) ; ekf_ . Init ( x_state_ , P_ , ekf_ . F_ , Hj_ , R_radar_ , ekf_ . Q_ ) ; } else if ( measurement_pack . sensor_type_ == MeasurementPackage :: LASER ) { x_state_ ( 0 ) = measurement_pack . raw_measurements_ ( 0 ) ; x_state_ ( 1 ) = measurement_pack . raw_measurements_ ( 1 ) ; ekf_ . Init ( x_state_ , P_ , ekf_ . F_ , H_laser_ , R_laser_ , ekf_ . Q_ ) ; } previous_timestamp_ = measurement_pack . timestamp_ ; is_initialized_ = true ; return true ; } return false ; }
Execute first step of tracking.
Execute first step of tracking .
/*****************************************************************************
   *  Initialization
   ****************************************************************************/ /**
         Convert radar from polar to cartesian coordinates and initialize state.
      */ /**
         Initialize state.
      */ // done initializing, no need to predict or update