text: #include "ukf.h"
  #include "Eigen/Dense"
  #include <iostream>
  
  #define EPS 0.001
  
  using namespace std;
  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  using std::vector;
  
  /**
   * Initializes Unscented Kalman filter
   * This is scaffolding, do not modify
   */
  UKF::UKF() {
    // if this is false, laser measurements will be ignored (except during init)
    use_laser_ = true;
  
    // if this is false, radar measurements will be ignored (except during init)
    use_radar_ = true;
  
    // initial state vector
    x_ = VectorXd(5);
  
    // initial covariance matrix
    P_ = MatrixXd(5, 5);
  
    // Process noise standard deviation longitudinal acceleration in m/s^2
    std_a_ = 1.5;
  
    // Process noise standard deviation yaw acceleration in rad/s^2
    std_yawdd_ = 0.57;
    
    //DO NOT MODIFY measurement noise values below these are provided by the sensor manufacturer.
    // Laser measurement noise standard deviation position1 in m
    std_laspx_ = 0.15;
  
    // Laser measurement noise standard deviation position2 in m
    std_laspy_ = 0.15;
  
    // Radar measurement noise standard deviation radius in m
    std_radr_ = 0.3;
  
    // Radar measurement noise standard deviation angle in rad
    std_radphi_ = 0.03;
  
    // Radar measurement noise standard deviation radius change in m/s
    std_radrd_ = 0.3;
    //DO NOT MODIFY measurement noise values above these are provided by the sensor manufacturer.
    
    // State dimension
    n_x_ = x_.size();
  
    // Augmented state dimension
    n_aug_ = n_x_ + 2;
  
    // Number of state dimention
    n_sig_ = 2 * n_aug_ + 1;
  
    // Predicted sigma points matrix
    Xsig_pred_ = MatrixXd(n_x_, n_sig_);
  
    // Sigma point spreading parameter
    lambda_ = 3 - n_aug_;
  
    // Weights of sigma points
    weights_ = VectorXd(n_sig_);
  
    // Radar measurement noise covariance matrix
    R_radar_ = MatrixXd(3, 3);
    R_radar_ << std_radr_*std_radr_, 0, 0,
                0, std_radphi_*std_radphi_, 0,
                0, 0,std_radrd_*std_radrd_;
  
    // Lidar measurement noise covariance matrix
    R_lidar_ = MatrixXd(2, 2);
    R_lidar_ << std_laspx_*std_laspx_,0,
                0,std_laspy_*std_laspy_;
  
  }
  
  UKF::~UKF() {}
  
  /**
   * @param {MeasurementPackage} meas_package The latest measurement data of
   * either radar or laser.
   */
  void UKF::ProcessMeasurement(MeasurementPackage meas_package) {
    if (!is_initialized_) {
  
      // Initialize the weights
      weights_(0) = lambda_/(lambda_ + n_aug_);
      for (int i=1; i < n_sig_; i++) {  //2n+1 weights
        double weight = 0.5/(n_aug_+lambda_);
        weights_(i) = weight;
      }
  
      // Initialize the covariance matrix
      P_ << 1, 0, 0, 0, 0,
            0, 1, 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) {
        /**
        Convert radar from polar to cartesian coordinates and initialize state.
        */
        double rho = meas_package.raw_measurements_[0]; // range
    	  double phi = meas_package.raw_measurements_[1]; // bearing
    	  double rhoDot = meas_package.raw_measurements_[2]; // velocity of rho
  
        double x = rho * cos(phi) >= EPS ? rho * cos(phi): EPS;
    	  double y = rho * sin(phi) >= EPS ? rho * sin(phi) : EPS;
        double vx = rhoDot * cos(phi);
    	  double vy = rhoDot * sin(phi);
        double v  = sqrt(vx * vx + vy * vy);
  
        x_ << x, y, v, 0, 0;
      } else {
        /**
        Initialize state.
        */
        x_ << meas_package.raw_measurements_[0], meas_package.raw_measurements_[1], 0, 0, 0;
        x_(0) = fabs(x_(0)) < EPS ? EPS : x_(0);
        x_(1) = fabs(x_(1)) < EPS ? EPS : x_(1);
      }
  
      time_us_ = meas_package.timestamp_;
      is_initialized_ = true;
  
      return;
    }
  
    // Calculate dt from current and previous time stamp.
    double dt = (meas_package.timestamp_ - time_us_) / 1000000.0;
    time_us_ = meas_package.timestamp_;
  
    Prediction(dt);
    // Update pased on the measurment type.
    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);
    }
  }
  
  /**
   * Predicts sigma points, the state, and the state covariance matrix.
   * @param {double} delta_t the change in time (in seconds) between the last
   * measurement and this one.
   */
  void UKF::Prediction(double delta_t) {
    /**
    TODO:
  
    Complete this function! Estimate the object's location. Modify the state
    vector, x_. Predict sigma points, the state, and the state covariance matrix.
    */
    
    // Initialize augmented vectors/matix
    VectorXd x_aug = VectorXd(n_aug_);
    MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
    MatrixXd Xsig_aug = MatrixXd(n_aug_, n_sig_);
  
    // Set up augmented vectors/matix
    x_aug.fill(0.0);
    x_aug.head(n_x_) = x_;
  
    P_aug.fill(0.0);
    P_aug.topLeftCorner(n_x_, n_x_) = P_;
    P_aug(5, 5) = std_a_ * std_a_;
    P_aug(6, 6) = std_yawdd_ * std_yawdd_;
  
    // Calculate square root of P
    MatrixXd A = P_aug.llt().matrixL();
  
    // Create sigma points
    // Set first column of sigma point matrix
    Xsig_aug.col(0)  = x_aug;
  
    // Set remaining sigma points
    for (int i = 0; i < n_aug_; i++)
    {
      Xsig_aug.col(i+1)     = x_aug + sqrt(lambda_ + n_aug_) * A.col(i);
      Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt(lambda_ + n_aug_) * A.col(i);
    }
  
    // Predict sigma points
    for (int i = 0; i< 2*n_aug_+1; i++)
    {
      // Extract values for better readability
      double p_x = Xsig_aug(0,i);
      double p_y = Xsig_aug(1,i);
      double v = Xsig_aug(2,i);
      double yaw = Xsig_aug(3,i);
      double yawd = Xsig_aug(4,i);
      double nu_a = Xsig_aug(5,i);
      double nu_yawdd = Xsig_aug(6,i);
  
      // Predicted state values
      double px_p, py_p;
  
      // Avoid division by zero
      if (fabs(yawd) > 0.001) {
          px_p = p_x + v/yawd * ( sin(yaw + yawd*delta_t) - sin(yaw));
          py_p = p_y + v/yawd * ( cos(yaw) - cos(yaw+yawd*delta_t) );
      }
      else {
          px_p = p_x + v*delta_t*cos(yaw);
          py_p = p_y + v*delta_t*sin(yaw);
      }
  
      double v_p = v;
      double yaw_p = yaw + yawd*delta_t;
      double yawd_p = yawd;
  
      // Add noise
      px_p = px_p + 0.5*nu_a*delta_t*delta_t * cos(yaw);
      py_p = py_p + 0.5*nu_a*delta_t*delta_t * sin(yaw);
      v_p = v_p + nu_a*delta_t;
  
      yaw_p = yaw_p + 0.5*nu_yawdd*delta_t*delta_t;
      yawd_p = yawd_p + nu_yawdd*delta_t;
  
      // Write predicted sigma point into right column
      Xsig_pred_(0,i) = px_p;
      Xsig_pred_(1,i) = py_p;
      Xsig_pred_(2,i) = v_p;
      Xsig_pred_(3,i) = yaw_p;
      Xsig_pred_(4,i) = yawd_p;
    }
  
    // Predicted state mean
    x_.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //iterate over sigma points
      x_ = x_ + weights_(i) * Xsig_pred_.col(i);
    }
  
    // Predicted state covariance matrix
    P_.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //iterate over sigma points
  
      // State difference
      VectorXd x_diff = Xsig_pred_.col(i) - x_;
      // Angle normalization
      while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
      while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
  
      P_ = P_ + weights_(i) * x_diff * x_diff.transpose() ;
    }
  }
  
  /**
   * Updates the state and the state covariance matrix using a laser measurement.
   * @param {MeasurementPackage} meas_package
   */
  void UKF::UpdateLidar(MeasurementPackage meas_package) {
    /**
    TODO:
  
    Complete this function! Use lidar data to update the belief about the object's
    position. Modify the state vector, x_, and covariance, P_.
  
    You'll also need to calculate the lidar NIS.
    */
  
    // Set measurement dimension
    int n_z = 2;
  
    // Create matrix for sigma points in measurement space
    MatrixXd Zsig = Xsig_pred_.block(0, 0, n_z, n_sig_);
  
    Update(meas_package, Zsig, n_z);
  }
  
  /**
   * Updates the state and the state covariance matrix using a radar measurement.
   * @param {MeasurementPackage} meas_package
   */
  void UKF::UpdateRadar(MeasurementPackage meas_package) {
    /**
    TODO:
  
    Complete this function! Use radar data to update the belief about the object's
    position. Modify the state vector, x_, and covariance, P_.
  
    You'll also need to calculate the radar NIS.
    */
  
    // Set measurement dimension, radar can measure r, phi, and r_dot
    int n_z = 3;
  
    // Create matrix for sigma points in measurement space
    MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);
  
    // Transform sigma points into measurement space
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //2n+1 simga points
  
      // Extract values for better readibility
      double p_x = Xsig_pred_(0,i);
      double p_y = Xsig_pred_(1,i);
      double v  = Xsig_pred_(2,i);
      double yaw = Xsig_pred_(3,i);
  
      double v1 = cos(yaw)*v;
      double v2 = sin(yaw)*v;
  
      // Measurement model
      Zsig(0,i) = sqrt(p_x*p_x + p_y*p_y);                        //r
      Zsig(1,i) = atan2(p_y,p_x);                                 //phi
      Zsig(2,i) = (p_x*v1 + p_y*v2 ) / sqrt(p_x*p_x + p_y*p_y);   //r_dot
    }
  
    Update(meas_package, Zsig, n_z);
  }
  
  /**
   * Updates the state and the state covariance matrix
   * @param meas_package The measurement at k+1
   * @param Zsig Sigma points in measurement space
   * @param n_z Measurement dimension
   */
  void UKF::Update(MeasurementPackage meas_package, MatrixXd Zsig, int n_z) {
    // Mean predicted measurement
    VectorXd z_pred = VectorXd(n_z);
    z_pred.fill(0.0);
    for (int i=0; i < 2*n_aug_+1; i++) {
      z_pred = z_pred + weights_(i) * Zsig.col(i);
    }
  
    // Innovation covariance matrix S
    MatrixXd S = MatrixXd(n_z,n_z);
    S.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //2n+1 simga points
      // Residual
      VectorXd z_diff = Zsig.col(i) - z_pred;
  
      // Angle normalization
      while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
      while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
  
      S = S + weights_(i) * z_diff * z_diff.transpose();
    }
  
    // Add measurement noise covariance matrix based on measurment type
    MatrixXd R = MatrixXd(n_z,n_z);
    R = meas_package.sensor_type_ == MeasurementPackage::RADAR ? R_radar_ : R_lidar_;
    S = S + R;
  
    // Create matrix for cross correlation Tc
    MatrixXd Tc = MatrixXd(n_x_, n_z);
  
    // Calculate cross correlation matrix
    Tc.fill(0.0);
    for (int i = 0; i < 2 * n_aug_ + 1; i++) {  //2n+1 simga points
  
      //residual
      VectorXd z_diff = Zsig.col(i) - z_pred;
      //angle normalization
      while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
      while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
  
      // state difference
      VectorXd x_diff = Xsig_pred_.col(i) - x_;
      //angle normalization
      while (x_diff(3)> M_PI) x_diff(3)-=2.*M_PI;
      while (x_diff(3)<-M_PI) x_diff(3)+=2.*M_PI;
  
      Tc = Tc + weights_(i) * x_diff * z_diff.transpose();
    }
  
    // Kalman gain K;
    MatrixXd K = Tc * S.inverse();
  
    // Residual
    VectorXd z = meas_package.raw_measurements_;
    VectorXd z_diff = z - z_pred;
  
    // Angle normalization
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      while (z_diff(1)> M_PI) z_diff(1)-=2.*M_PI;
      while (z_diff(1)<-M_PI) z_diff(1)+=2.*M_PI;
    }
  
    // Update state mean and covariance matrix
    x_ = x_ + K * z_diff;
    P_ = P_ - K*S*K.transpose();
  
    // Calculate NIS
    if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
      NIS_radar_ = z.transpose() * S.inverse() * z;
    } else if (meas_package.sensor_type_ == MeasurementPackage::LASER) {
      NIS_laser_ = z.transpose() * S.inverse() * z;
    }
  }