ukf.h

#ifndef UKF_H
#define UKF_H

#include "Eigen/Dense"
#include "measurement_package.h"

using Eigen::MatrixXd;
using Eigen::VectorXd;

class UKF {
public:
  UKF();
  virtual ~UKF();

  /**
   * ProcessMeasurement
   * @param meas_package The latest measurement data of either radar or laser
   */
  void ProcessMeasurement(const MeasurementPackage& meas_package);

  /**
   * Prediction Predicts sigma points, the state, and the state covariance
   * matrix
   * @param delta_t Time between k and k+1 in s
   */
  void Prediction(double delta_t);
  MatrixXd AugmentationAndSigmaPointsGeneration(const VectorXd& x, const MatrixXd& P);
  MatrixXd SigmaPointsPrediction(const MatrixXd& Xsig_aug, double delta_t);
  void UKFPredict(const MatrixXd& Xsig_pred, VectorXd& x_out, MatrixXd& P_out);

  /**
   * Updates the state and the state covariance matrix using a laser measurement
   * @param meas_package The measurement at k+1
   */
  void UpdateLidar(const MeasurementPackage& meas_package);

  /**
   * Updates the state and the state covariance matrix using a radar measurement
   * @param meas_package The measurement at k+1
   */
  void UpdateRadar(const MeasurementPackage& meas_package);
  void PredictRadarMeasurements(MatrixXd& Zsig, VectorXd& z_pred, MatrixXd& S);

  // common functions
  void UKFUpdate(const MatrixXd& Zsig, const VectorXd& z_pred, const MatrixXd& S,
                 const VectorXd& z, VectorXd& x, MatrixXd& P);
  double NormalizeAngle(double angle_rad);

  // initially set to false, set to true in first call of ProcessMeasurement
  bool is_initialized_;

  // if this is false, laser measurements will be ignored (except for init)
  bool use_laser_;

  // if this is false, radar measurements will be ignored (except for init)
  bool use_radar_;

  // state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
  Eigen::VectorXd x_;

  // state covariance matrix
  Eigen::MatrixXd P_;

  // predicted sigma points matrix
  Eigen::MatrixXd Xsig_pred_;

  // time when the state is true, in us
  long long time_us_;

  // Process noise standard deviation longitudinal acceleration in m/s^2
  double std_a_;

  // Process noise standard deviation yaw acceleration in rad/s^2
  double std_yawdd_;

  // Laser measurement noise standard deviation position1 in m
  double std_laspx_;

  // Laser measurement noise standard deviation position2 in m
  double std_laspy_;

  // Radar measurement noise standard deviation radius in m
  double std_radr_;

  // Radar measurement noise standard deviation angle in rad
  double std_radphi_;

  // Radar measurement noise standard deviation radius change in m/s
  double std_radrd_ ;

  // Weights of sigma points
  Eigen::VectorXd weights_;

  // State dimension
  int n_x_;

  // Augmented state dimension
  int n_aug_;

  // Sigma point spreading parameter
  double lambda_;

  // radar measurement dimension
  int n_z_radar = 3;
  int n_z_lidar = 2;

  // radar measurement noise covariance matrix
  MatrixXd R_radar_;

  // lidar measurement model H_
  MatrixXd H_lidar_;

  // lidar measurement noise covariance matrix
  MatrixXd R_lidar_;
};

#endif  // UKF_H