text: #include "ukf.h"
  #include "Eigen/Dense"
  #include <iostream>
  
  using Eigen::MatrixXd;
  using Eigen::VectorXd;
  
  using namespace std;
  
  /**
   * Initializes Unscented Kalman filter
   */
  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_ = 0.9;
  
    // Process noise standard deviation yaw acceleration in rad/s^2
    std_yawdd_ = 0.6;
    
    /**
     * 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;
    
    /**
     * End DO NOT MODIFY section for measurement noise values 
     */
    
    /**
     * TODO: Complete the initialization. See ukf.h for other member properties.
     * Hint: one or more values initialized above might be wildly off...
     */
      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;
      
  }
  
  UKF::~UKF() {}
  
  void UKF::ProcessMeasurement(MeasurementPackage meas_package) {
    /**
     * TODO: Complete this function! Make sure you switch between lidar and radar
     * measurements.
     */
      if (!is_initialized_) {
          /**
           * TODO: Initialize the state x with the first measurement.
           * TODO: Create the covariance matrix.
           *
           */
          
          //first measurement
          x_ << 1, 1, 0, 0, 0;
          
          if (meas_package.sensor_type_ == MeasurementPackage::RADAR) {
              //TODO: Convert radar from polar to catesian coordinates
              x_ << meas_package.raw_measurements_[0]*cos(meas_package.raw_measurements_[1]),
              meas_package.raw_measurements_[0]*sin(meas_package.raw_measurements_[1]),
                  0,
                  0,
                  0;
          }
          else if (meas_package.sensor_type_ == MeasurementPackage::LASER){
              //TODO: Initialize state.
              x_ << meas_package.raw_measurements_[0],
                  meas_package.raw_measurements_[1],
                  0,
                  0,
                  0;
          }
          time_us_ = meas_package.timestamp_;
          //done initializaing, no need to predict or update
          is_initialized_ = true;
          return;
      }
      
      /**
       * Prediction
       */
      double dt = (meas_package.timestamp_ - time_us_)/1000000.0;
      time_us_ = meas_package.timestamp_;
      
      Prediction(dt);
      
      /**
       * Update
       * - Use the sensor type to perform the update step
       * - update the state and covariance matrices
       */
      if (meas_package.sensor_type_ == MeasurementPackage::RADAR && use_radar_ == true) {
          // TODO: Radar updates
          UpdateRadar(meas_package);
          cout << "radar is good here!" << endl;
      }
      else if(meas_package.sensor_type_ == MeasurementPackage::LASER && use_laser_ == true){
          //TODO: Laser updates
          UpdateLidar(meas_package);
          cout << "lidar is good here!" << endl;
      }
      
  }
  
  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.
     */
      // set state dimension
      n_x_ = 5;
      
      // set augmented dimension
      n_aug_ = 7;
      
      // define spreading parameter
      lambda_ = 3 - n_aug_;
      
      // create augmented mean vector
      VectorXd x_aug = VectorXd(n_aug_);
      
      // create augmented state covariance
      MatrixXd P_aug = MatrixXd(n_aug_, n_aug_);
      
      // create sigma point matrix
      MatrixXd Xsig_aug = MatrixXd(n_aug_, 2 * n_aug_ + 1);
      
      // create augmented mean state
      x_aug.head(5) = x_;
      x_aug(5) = 0;
      x_aug(6) = 0;
      
      // create augmented covariance matrix
      P_aug.fill(0.0);
      P_aug.topLeftCorner(5,5) = P_;
      P_aug(5,5) = std_a_*std_a_;
      P_aug(6,6) = std_yawdd_*std_yawdd_;
      
      // create square root matrix
      MatrixXd L = P_aug.llt().matrixL();
  
      // create augmented sigma points
      Xsig_aug.col(0)  = x_aug;
      for (int i = 0; i< n_aug_; ++i) {
          Xsig_aug.col(i+1)       = x_aug + sqrt(lambda_+n_aug_) * L.col(i);
          Xsig_aug.col(i+1+n_aug_) = x_aug - sqrt(lambda_+n_aug_) * L.col(i);
      }
      
      Xsig_pred_ = MatrixXd(n_x_, 2*n_aug_ + 1);
  
      // 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;
          
      }
  
      // create vector for weights
      weights_ = VectorXd(2*n_aug_+1);
      
      // create vector for predicted state
      x_pri = VectorXd(n_x_);
      
      // create covariance matrix for prediction
      P_pri = MatrixXd(n_x_, n_x_);
      
      // set weights
      double weight_0 = lambda_/(lambda_+n_aug_);
      weights_(0) = weight_0;
      for (int i=1; i<2*n_aug_+1; ++i) {  // 2n+1 weights
          double weight = 0.5/(n_aug_+lambda_);
          weights_(i) = weight;
      }
      
      // predicted state mean
      x_pri.fill(0.0);
      for (int i = 0; i < 2 * n_aug_ + 1; ++i) {  // iterate over sigma points
          x_pri = x_pri + weights_(i) * Xsig_pred_.col(i);
      }
      
      // predicted state covariance matrix
      P_pri.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_pri;
          // 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_pri = P_pri + weights_(i) * x_diff * x_diff.transpose();
      }
      
  }
  
  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 can also calculate the lidar NIS, if desired.
     */
  
      // set measurement dimension, lidar measurement
      int n_z = 2;
  
      // create example vector for incoming lidar measurement
      VectorXd z = VectorXd(n_z);
      z <<    meas_package.raw_measurements_[0],   // x in m
              meas_package.raw_measurements_[1];   // y in m
  
      // create matrix for sigma points in measurement space
      MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);
  
      // mean predicted measurement
      VectorXd z_pred = VectorXd(n_z);
  
      // measurement covariance matrix S
      MatrixXd S = MatrixXd(n_z,n_z);
  
      // transform sigma points into measurement space
      for (int i = 0; i < 2 * n_aug_ + 1; ++i) {  // 2n+1 simga points
          // extract values for better readability
          double p_x = Xsig_pred_(0,i);
          double p_y = Xsig_pred_(1,i);
  
          // measurement model
          Zsig(0,i) = p_x;                     // x
          Zsig(1,i) = p_y;                     // y
  
      }
  
      // mean predicted measurement
      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
      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;
  
          S = S + weights_(i) * z_diff * z_diff.transpose();
      }
  
      // add measurement noise covariance matrix
      MatrixXd R = MatrixXd(n_z,n_z);
  
      R <<  std_laspx_*std_laspx_, 0,
      0, std_laspx_*std_laspx_;
      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;
  
          // state difference
          VectorXd x_diff = Xsig_pred_.col(i) - x_pri;
          // 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_diff = z - z_pred;
  
      // update state mean and covariance matrix
      x_ = x_pri + K * z_diff;
      P_ = P_pri - K*S*K.transpose();
  
  }
  
  
  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 can also calculate the radar NIS, if desired.
       */
      
      // set measurement dimension, radar can measure r, phi, and r_dot
      int n_z = 3;
      
      // create example vector for incoming radar measurement
      VectorXd z = VectorXd(n_z);
      z << meas_package.raw_measurements_[0],
          meas_package.raw_measurements_[1],
      meas_package.raw_measurements_[2];
      
      // create matrix for sigma points in measurement space
      MatrixXd Zsig = MatrixXd(n_z, 2 * n_aug_ + 1);
      
      // mean predicted measurement
      VectorXd z_pred = VectorXd(n_z);
      
      // measurement covariance matrix S
      MatrixXd S = MatrixXd(n_z,n_z);
      
      Zsig.fill(0.0);
      // transform sigma points into measurement space
      for (int i = 0; i < 2 * n_aug_ + 1; ++i) {  // 2n+1 simga points
          // extract values for better readability
          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
      }
      
      // mean predicted measurement
      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
      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
      MatrixXd R = MatrixXd(n_z,n_z);
      R <<  std_radr_*std_radr_, 0, 0,
      0, std_radphi_*std_radphi_, 0,
      0, 0,std_radrd_*std_radrd_;
      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_pri;
          // 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_diff = z - 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;
      
      // update state mean and covariance matrix
      x_ = x_pri + K * z_diff;
      P_ = P_pri - K*S*K.transpose();
      
  }
  
  