text: #include "FusionEKF.h"
  #include "tools.h"
  #include "Eigen/Dense"
  #include <iostream>
  
  using namespace std;
  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  using std::vector;
  
  /*
   * Constructor.
   */
  FusionEKF::FusionEKF() {
    is_initialized_ = false;
  
    previous_timestamp_ = 0;
  
    // initializing matrices
    R_laser_ = MatrixXd(2, 2);
    R_radar_ = MatrixXd(3, 3);
    H_laser_ = MatrixXd(2, 4);
    Hj_ = MatrixXd(3, 4);
  
    //measurement covariance matrix - laser
    R_laser_ << 0.0225, 0,
                0, 0.0225;
  
    //measurement covariance matrix - radar
    R_radar_ << 0.09, 0, 0,
                0, 0.0009, 0,
                0, 0, 0.09;
  
    //keep px and py only for liday
    H_laser_ << 1, 0, 0, 0,
                0, 1, 0, 0;
  
    MatrixXd F_ = MatrixXd(4, 4);
    F_ << 1, 0, 1, 0,
          0, 1, 0, 1,
          0, 0, 1, 0,
          0, 0, 0, 1;
  
    MatrixXd Q_ = MatrixXd(4, 4);
  
    MatrixXd P_ = MatrixXd(4, 4);
    P_ << 1, 0, 0, 0,
          0, 1, 0, 0,
          0, 0, 1000, 0,
          0, 0, 0, 1000;
  
    VectorXd x_ = VectorXd(4);
    x_ << 1, 1, 1, 1;
  
    // initialize variables in Kalman Filter
    ekf_.Init(x_, P_, F_, H_laser_, R_laser_, Q_);
  
    tools = Tools();
  }
  
  /**
  * Destructor.
  */
  FusionEKF::~FusionEKF() {}
  
  void FusionEKF::ProcessMeasurement(const MeasurementPackage &measurement_pack) {
  
  
    /*****************************************************************************
     *  Initialization
     ****************************************************************************/
    if (!is_initialized_) {
  
      // first measurement
      cout << "EKF: " << endl;
      previous_timestamp_ = measurement_pack.timestamp_;
  
      if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
        //convert from polar to cartesian 
        double ro = measurement_pack.raw_measurements_[0];
        double phi = measurement_pack.raw_measurements_[1];
        ekf_.x_ << ro*std::cos(phi), ro*std::sin(phi), 0, 0;
      } else if (measurement_pack.sensor_type_ == MeasurementPackage::LASER) {
        ekf_.x_ << measurement_pack.raw_measurements_[0], measurement_pack.raw_measurements_[1], 0, 0;
      }
  
      // done initializing, no need to predict or update
      is_initialized_ = true;
      return;
    }
  
    /*****************************************************************************
     *  Prediction
     ****************************************************************************/
    //use time difference to calculate F state transtion and Q error from acceleration
    long long incoming = measurement_pack.timestamp_;
    float dt = (incoming - previous_timestamp_) / 1000000.0;
    previous_timestamp_ = incoming; 
  
    ekf_.F_(0, 2) = dt;
    ekf_.F_(1, 3) = dt;
  
    double dt_2 = dt * dt;
    double dt_3 = dt_2 * dt;
    double dt_4 = dt_3 * dt;
  
    ekf_.Q_ << dt_4 / 4 * noise_ax_, 0, dt_3 / 2 * noise_ax_, 0,
               0, dt_4 / 4 * noise_ay_, 0, dt_3 / 2 * noise_ay_,
               dt_3 / 2 * noise_ax_, 0, dt_2 * noise_ax_, 0,
               0, dt_3 /2 * noise_ay_, 0, dt_2 * noise_ay_;  
  
    ekf_.Predict();
  
    /*****************************************************************************
     *  Update
     ****************************************************************************/
  
    if (measurement_pack.sensor_type_ == MeasurementPackage::RADAR) {
      // calc jacobian to map to linear 
      ekf_.H_ = tools.CalculateJacobian(ekf_.x_);
      ekf_.R_ = R_radar_;
      ekf_.UpdateEKF(measurement_pack.raw_measurements_);
    } else {
      // laser updates
      ekf_.H_ = H_laser_;
      ekf_.R_ = R_laser_;
      ekf_.Update(measurement_pack.raw_measurements_);
    }
  
    cout << "x_ = " << ekf_.x_ << endl;
    cout << "P_ = " << ekf_.P_ << endl;
  }
  