add lidar_radar_fusion_cnn_ukf project

src/fusion/lidar_radar_fusion_cnn _ukf/ukf.cpp

@@ -0,0 +1,1347 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ukf.h"
+
+/**
+* Initializes Unscented Kalman filter
+ */
+UKF::UKF()
+  : num_state_(5)
+  , num_lidar_state_(2)
+  , num_lidar_direction_state_(3)
+  , num_motion_model_(3)
+  , is_direction_cv_available_(false)
+  , is_direction_ctrv_available_(false)
+  , is_direction_rm_available_(false)
+  , std_lane_direction_(0.15)
+{
+  // initial state vector
+  x_merge_ = Eigen::MatrixXd(5, 1);
+
+  // initial state vector
+  x_cv_ = Eigen::MatrixXd(5, 1);
+
+  // initial state vector
+  x_ctrv_ = Eigen::MatrixXd(5, 1);
+
+  // initial state vector
+  x_rm_ = Eigen::MatrixXd(5, 1);
+
+  // initial covariance matrix
+  p_merge_ = Eigen::MatrixXd(5, 5);
+
+  // initial covariance matrix
+  p_cv_ = Eigen::MatrixXd(5, 5);
+
+  // initial covariance matrix
+  p_ctrv_ = Eigen::MatrixXd(5, 5);
+
+  // initial covariance matrix
+  p_rm_ = Eigen::MatrixXd(5, 5);
+
+  // Process noise standard deviation longitudinal acceleration in m/s^2
+  std_a_cv_ = 1.5;
+  std_a_ctrv_ = 1.5;
+  std_a_rm_ = 3;
+  std_ctrv_yawdd_ = 1.5;
+  std_cv_yawdd_ = 1.5;
+  std_rm_yawdd_ = 3;
+
+  // Laser measurement noise standard deviation position1 in m
+  std_laspx_ = 0.15;
+  // Laser measurement noise standard deviation position2 in m
+  std_laspy_ = 0.15;
+
+  // time when the state is true, in us
+  time_ = 0.0;
+
+  // predicted sigma points matrix
+  x_sig_pred_cv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // predicted sigma points matrix
+  x_sig_pred_ctrv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // predicted sigma points matrix
+  x_sig_pred_rm_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // create vector for weights
+  weights_c_ = Eigen::VectorXd(2 * num_state_ + 1);
+  weights_s_ = Eigen::VectorXd(2 * num_state_ + 1);
+
+  // transition probability
+  p1_.push_back(0.9);
+  p1_.push_back(0.05);
+  p1_.push_back(0.05);
+
+  p2_.push_back(0.05);
+  p2_.push_back(0.9);
+  p2_.push_back(0.05);
+
+  p3_.push_back(0.05);
+  p3_.push_back(0.05);
+  p3_.push_back(0.9);
+
+  mode_match_prob_cv2cv_ = 0;
+  mode_match_prob_ctrv2cv_ = 0;
+  mode_match_prob_rm2cv_ = 0;
+
+  mode_match_prob_cv2ctrv_ = 0;
+  mode_match_prob_ctrv2ctrv_ = 0;
+  mode_match_prob_rm2ctrv_ = 0;
+
+  mode_match_prob_cv2rm_ = 0;
+  mode_match_prob_ctrv2rm_ = 0;
+  mode_match_prob_rm2rm_ = 0;
+
+  mode_prob_cv_ = 0.33;
+  mode_prob_ctrv_ = 0.33;
+  mode_prob_rm_ = 0.33;
+
+  z_pred_cv_ = Eigen::VectorXd(2);
+  z_pred_ctrv_ = Eigen::VectorXd(2);
+  z_pred_rm_ = Eigen::VectorXd(2);
+
+  s_cv_ = Eigen::MatrixXd(2, 2);
+  s_ctrv_ = Eigen::MatrixXd(2, 2);
+  s_rm_ = Eigen::MatrixXd(2, 2);
+
+  k_cv_ = Eigen::MatrixXd(5, 2);
+  k_ctrv_ = Eigen::MatrixXd(5, 2);
+  k_rm_ = Eigen::MatrixXd(5, 2);
+
+  pd_ = 0.9;
+  pg_ = 0.99;
+
+  // tracking parameter
+  lifetime_ = 0;
+  is_static_ = false;
+
+  // bounding box params
+  is_stable_ = false;
+  iv::fusion::Dimension * pdi = object_.mutable_dimensions();
+  pdi->set_x(1.0);
+  pdi->set_y(1.0);
+//  object_.di = 1.0;
+//  object_.dimensions.y = 1.0;
+
+  // for static classification
+  init_meas_ = Eigen::VectorXd(2);
+
+  x_merge_yaw_ = 0;
+
+  // for raukf
+  cv_meas_ = Eigen::VectorXd(2);
+  ctrv_meas_ = Eigen::VectorXd(2);
+  rm_meas_ = Eigen::VectorXd(2);
+
+  r_cv_ = Eigen::MatrixXd(2, 2);
+  r_ctrv_ = Eigen::MatrixXd(2, 2);
+  r_rm_ = Eigen::MatrixXd(2, 2);
+
+  q_cv_ = Eigen::MatrixXd(5, 5);
+  q_ctrv_ = Eigen::MatrixXd(5, 5);
+  q_rm_ = Eigen::MatrixXd(5, 5);
+
+  nis_cv_ = 0;
+  nis_ctrv_ = 0;
+  nis_rm_ = 0;
+
+  new_x_sig_cv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+  new_x_sig_ctrv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+  new_x_sig_rm_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  new_z_sig_cv_ = Eigen::MatrixXd(2, 2 * num_state_ + 1);
+  new_z_sig_ctrv_ = Eigen::MatrixXd(2, 2 * num_state_ + 1);
+  new_z_sig_rm_ = Eigen::MatrixXd(2, 2 * num_state_ + 1);
+
+  new_z_pred_cv_ = Eigen::VectorXd(2);
+  new_z_pred_ctrv_ = Eigen::VectorXd(2);
+  new_z_pred_rm_ = Eigen::VectorXd(2);
+
+  new_s_cv_ = Eigen::MatrixXd(2, 2);
+  new_s_ctrv_ = Eigen::MatrixXd(2, 2);
+  new_s_rm_ = Eigen::MatrixXd(2, 2);
+
+  // for lane direction combined filter
+  lidar_direction_r_cv_ = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+  lidar_direction_r_ctrv_ = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+  lidar_direction_r_rm_ = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+
+  k_lidar_direction_cv_ = Eigen::MatrixXd(num_state_, num_lidar_direction_state_);
+  k_lidar_direction_ctrv_ = Eigen::MatrixXd(num_state_, num_lidar_direction_state_);
+  k_lidar_direction_rm_ = Eigen::MatrixXd(num_state_, num_lidar_direction_state_);
+
+  lidar_direction_ctrv_meas_ = Eigen::VectorXd(num_lidar_direction_state_);
+}
+
+double UKF::normalizeAngle(const double angle)
+{
+  double normalized_angle = angle;
+  while (normalized_angle > M_PI)
+    normalized_angle -= 2. * M_PI;
+  while (normalized_angle < -M_PI)
+    normalized_angle += 2. * M_PI;
+  return normalized_angle;
+}
+
+void UKF::initialize(const Eigen::VectorXd& z, const double timestamp, const int target_id)
+{
+  ukf_id_ = target_id;
+
+  // first measurement
+  x_merge_ << 0, 0, 0, 0, 0.1;
+
+  // init covariance matrix by hardcoding since no clue about initial state covrariance
+  p_merge_ << 0.5, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 1;
+
+  // set weights
+  // reference from "The Unscented Kalman Filter for Nonlinear Estimation, Eric A. Wan and Rudolph van der Merwe, 2000"
+  // alpha = 0.0025, beta = 2, k = 0
+  double alpha = 0.0025;
+  double beta = 2;
+  double k = 0;
+  lambda_ = alpha * alpha * (num_state_ + k) - num_state_;
+  double weight_s_0 = lambda_ / (lambda_ + num_state_);
+  double weight_c_0 = lambda_ / (lambda_ + num_state_) + (1 - alpha * alpha + beta);
+  weights_s_(0) = weight_s_0;
+  weights_c_(0) = weight_c_0;
+  for (int i = 1; i < 2 * num_state_ + 1; i++)
+  {  // 2n+1 weights
+    double weight = 0.5 / (num_state_ + lambda_);
+    weights_s_(i) = weight;
+    weights_c_(i) = weight;
+  }
+
+  // init timestamp
+  time_ = timestamp;
+
+  x_merge_(0) = z(0);
+  x_merge_(1) = z(1);
+
+  z_pred_cv_(0) = z(0);
+  z_pred_cv_(1) = z(1);
+
+  z_pred_ctrv_(0) = z(0);
+  z_pred_ctrv_(1) = z(1);
+
+  z_pred_rm_(0) = z(0);
+  z_pred_rm_(1) = z(1);
+
+  x_cv_ = x_ctrv_ = x_rm_ = x_merge_;
+  p_cv_ = p_ctrv_ = p_rm_ = p_merge_;
+
+  s_cv_ << 1, 0, 0, 1;
+  s_ctrv_ << 1, 0, 0, 1;
+  s_rm_ << 1, 0, 0, 1;
+
+  // initialize R covariance
+  r_cv_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
+  r_ctrv_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
+  r_rm_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
+
+  // initialize lidar-lane R covariance
+  // clang-format off
+  lidar_direction_r_cv_ << std_laspx_ * std_laspx_,                       0,                                       0,
+                                            0, std_laspy_ * std_laspy_,                                       0,
+                                            0,                       0, std_lane_direction_*std_lane_direction_;
+  lidar_direction_r_ctrv_ << std_laspx_ * std_laspx_,                       0,                                       0,
+                                              0, std_laspy_ * std_laspy_,                                       0,
+                                              0,                       0, std_lane_direction_*std_lane_direction_;
+  lidar_direction_r_rm_ << std_laspx_ * std_laspx_,                       0,                                       0,
+                                            0, std_laspy_ * std_laspy_,                                       0,
+                                            0,                       0, std_lane_direction_*std_lane_direction_;
+  // clang-format on
+
+  // init tracking num
+  tracking_num_ = 1;
+}
+
+void UKF::updateModeProb(const std::vector<double>& lambda_vec)
+{
+  double cvGauss = lambda_vec[0];
+  double ctrvGauss = lambda_vec[1];
+  double rmGauss = lambda_vec[2];
+  double sumGauss = cvGauss * mode_prob_cv_ + ctrvGauss * mode_prob_ctrv_ + rmGauss * mode_prob_rm_;
+  mode_prob_cv_ = (cvGauss * mode_prob_cv_) / sumGauss;
+  mode_prob_ctrv_ = (ctrvGauss * mode_prob_ctrv_) / sumGauss;
+  mode_prob_rm_ = (rmGauss * mode_prob_rm_) / sumGauss;
+  // prevent each prob from becoming 0
+  if (fabs(mode_prob_cv_) < 0.0001)
+    mode_prob_cv_ = 0.0001;
+  if (fabs(mode_prob_ctrv_) < 0.0001)
+    mode_prob_ctrv_ = 0.0001;
+  if (fabs(mode_prob_rm_) < 0.0001)
+    mode_prob_rm_ = 0.0001;
+}
+
+void UKF::updateYawWithHighProb()
+{
+  if (mode_prob_cv_ > mode_prob_ctrv_)
+  {
+    if (mode_prob_cv_ > mode_prob_rm_)
+    {
+      x_merge_yaw_ = x_cv_(3);
+    }
+    else
+    {
+      x_merge_yaw_ = x_rm_(3);
+    }
+  }
+  else
+  {
+    if (mode_prob_ctrv_ > mode_prob_rm_)
+    {
+      x_merge_yaw_ = x_ctrv_(3);
+    }
+    else
+    {
+      x_merge_yaw_ = x_rm_(3);
+    }
+  }
+  x_merge_(3) = x_merge_yaw_;
+}
+
+void UKF::mergeEstimationAndCovariance()
+{
+  x_merge_ = mode_prob_cv_ * x_cv_ + mode_prob_ctrv_ * x_ctrv_ + mode_prob_rm_ * x_rm_;
+  while (x_merge_(3) > M_PI)
+    x_merge_(3) -= 2. * M_PI;
+  while (x_merge_(3) < -M_PI)
+    x_merge_(3) += 2. * M_PI;
+
+  // not interacting yaw(-pi ~ pi)
+  updateYawWithHighProb();
+
+  p_merge_ = mode_prob_cv_ * (p_cv_ + (x_cv_ - x_merge_) * (x_cv_ - x_merge_).transpose()) +
+             mode_prob_ctrv_ * (p_ctrv_ + (x_ctrv_ - x_merge_) * (x_ctrv_ - x_merge_).transpose()) +
+             mode_prob_rm_ * (p_rm_ + (x_rm_ - x_merge_) * (x_rm_ - x_merge_).transpose());
+}
+
+void UKF::mixingProbability()
+{
+  double sumProb1 = mode_prob_cv_ * p1_[0] + mode_prob_ctrv_ * p2_[0] + mode_prob_rm_ * p3_[0];
+  double sumProb2 = mode_prob_cv_ * p1_[1] + mode_prob_ctrv_ * p2_[1] + mode_prob_rm_ * p3_[1];
+  double sumProb3 = mode_prob_cv_ * p1_[2] + mode_prob_ctrv_ * p2_[2] + mode_prob_rm_ * p3_[2];
+  mode_match_prob_cv2cv_ = mode_prob_cv_ * p1_[0] / sumProb1;
+  mode_match_prob_ctrv2cv_ = mode_prob_ctrv_ * p2_[0] / sumProb1;
+  mode_match_prob_rm2cv_ = mode_prob_rm_ * p3_[0] / sumProb1;
+
+  mode_match_prob_cv2ctrv_ = mode_prob_cv_ * p1_[1] / sumProb2;
+  mode_match_prob_ctrv2ctrv_ = mode_prob_ctrv_ * p2_[1] / sumProb2;
+  mode_match_prob_rm2ctrv_ = mode_prob_rm_ * p3_[1] / sumProb2;
+
+  mode_match_prob_cv2rm_ = mode_prob_cv_ * p1_[2] / sumProb3;
+  mode_match_prob_ctrv2rm_ = mode_prob_ctrv_ * p2_[2] / sumProb3;
+  mode_match_prob_rm2rm_ = mode_prob_rm_ * p3_[2] / sumProb3;
+}
+
+void UKF::interaction()
+{
+  Eigen::MatrixXd x_pre_cv = x_cv_;
+  Eigen::MatrixXd x_pre_ctrv = x_ctrv_;
+  Eigen::MatrixXd x_pre_rm = x_rm_;
+  Eigen::MatrixXd p_pre_cv = p_cv_;
+  Eigen::MatrixXd p_pre_ctrv = p_ctrv_;
+  Eigen::MatrixXd p_pre_rm = p_rm_;
+  x_cv_ = mode_match_prob_cv2cv_ * x_pre_cv + mode_match_prob_ctrv2cv_ * x_pre_ctrv + mode_match_prob_rm2cv_ * x_pre_rm;
+  x_ctrv_ = mode_match_prob_cv2ctrv_ * x_pre_cv + mode_match_prob_ctrv2ctrv_ * x_pre_ctrv +
+            mode_match_prob_rm2ctrv_ * x_pre_rm;
+  x_rm_ = mode_match_prob_cv2rm_ * x_pre_cv + mode_match_prob_ctrv2rm_ * x_pre_ctrv + mode_match_prob_rm2rm_ * x_pre_rm;
+
+  // not interacting yaw(-pi ~ pi)
+  x_cv_(3) = x_pre_cv(3);
+  x_ctrv_(3) = x_pre_ctrv(3);
+  x_rm_(3) = x_pre_rm(3);
+
+  // normalizing angle
+  while (x_cv_(3) > M_PI)
+    x_cv_(3) -= 2. * M_PI;
+  while (x_cv_(3) < -M_PI)
+    x_cv_(3) += 2. * M_PI;
+  while (x_ctrv_(3) > M_PI)
+    x_ctrv_(3) -= 2. * M_PI;
+  while (x_ctrv_(3) < -M_PI)
+    x_ctrv_(3) += 2. * M_PI;
+  while (x_rm_(3) > M_PI)
+    x_rm_(3) -= 2. * M_PI;
+  while (x_rm_(3) < -M_PI)
+    x_rm_(3) += 2. * M_PI;
+
+  p_cv_ = mode_match_prob_cv2cv_ * (p_pre_cv + (x_pre_cv - x_cv_) * (x_pre_cv - x_cv_).transpose()) +
+          mode_match_prob_ctrv2cv_ * (p_pre_ctrv + (x_pre_ctrv - x_cv_) * (x_pre_ctrv - x_cv_).transpose()) +
+          mode_match_prob_rm2cv_ * (p_pre_rm + (x_pre_rm - x_cv_) * (x_pre_rm - x_cv_).transpose());
+  p_ctrv_ = mode_match_prob_cv2ctrv_ * (p_pre_cv + (x_pre_cv - x_ctrv_) * (x_pre_cv - x_ctrv_).transpose()) +
+            mode_match_prob_ctrv2ctrv_ * (p_pre_ctrv + (x_pre_ctrv - x_ctrv_) * (x_pre_ctrv - x_ctrv_).transpose()) +
+            mode_match_prob_rm2ctrv_ * (p_pre_rm + (x_pre_rm - x_ctrv_) * (x_pre_rm - x_ctrv_).transpose());
+  p_rm_ = mode_match_prob_cv2rm_ * (p_pre_cv + (x_pre_cv - x_rm_) * (x_pre_cv - x_rm_).transpose()) +
+          mode_match_prob_ctrv2rm_ * (p_pre_ctrv + (x_pre_ctrv - x_rm_) * (x_pre_ctrv - x_rm_).transpose()) +
+          mode_match_prob_rm2rm_ * (p_pre_rm + (x_pre_rm - x_rm_) * (x_pre_rm - x_rm_).transpose());
+}
+
+void UKF::predictionSUKF(const double dt, const bool has_subscribed_vectormap)
+{
+  /*****************************************************************************
+  *  Init covariance Q if it is necessary
+  ****************************************************************************/
+  initCovarQs(dt, x_merge_(3));
+  /*****************************************************************************
+  *  Prediction Motion Model
+  ****************************************************************************/
+  predictionMotion(dt, MotionModel::CTRV);
+  /*****************************************************************************
+  *  Prediction Measurement
+  ****************************************************************************/
+  predictionLidarMeasurement(MotionModel::CTRV, num_lidar_state_);
+  if (has_subscribed_vectormap)
+  {
+    predictionLidarMeasurement(MotionModel::CTRV, num_lidar_direction_state_);
+  }
+}
+
+void UKF::predictionIMMUKF(const double dt, const bool has_subscribed_vectormap)
+{
+  /*****************************************************************************
+  *  Init covariance Q if it is needed
+  ****************************************************************************/
+  initCovarQs(dt, x_merge_(3));
+  /*****************************************************************************
+  *  IMM Mixing and Interaction
+  ****************************************************************************/
+  mixingProbability();
+  interaction();
+  /*****************************************************************************
+  *  Prediction Motion Model
+  ****************************************************************************/
+  predictionMotion(dt, MotionModel::CV);
+  predictionMotion(dt, MotionModel::CTRV);
+  predictionMotion(dt, MotionModel::RM);
+  /*****************************************************************************
+  *  Prediction Measurement
+  ****************************************************************************/
+  predictionLidarMeasurement(MotionModel::CV, num_lidar_state_);
+  predictionLidarMeasurement(MotionModel::CTRV, num_lidar_state_);
+  predictionLidarMeasurement(MotionModel::RM, num_lidar_state_);
+
+  if (has_subscribed_vectormap)
+  {
+    predictionLidarMeasurement(MotionModel::CV, num_lidar_direction_state_);
+    predictionLidarMeasurement(MotionModel::CTRV, num_lidar_direction_state_);
+    predictionLidarMeasurement(MotionModel::RM, num_lidar_direction_state_);
+  }
+}
+
+void UKF::findMaxZandS(Eigen::VectorXd& max_det_z, Eigen::MatrixXd& max_det_s)
+{
+  double cv_det = s_cv_.determinant();
+  double ctrv_det = s_ctrv_.determinant();
+  double rm_det = s_rm_.determinant();
+
+  if (cv_det > ctrv_det)
+  {
+    if (cv_det > rm_det)
+    {
+      max_det_z = z_pred_cv_;
+      max_det_s = s_cv_;
+    }
+    else
+    {
+      max_det_z = z_pred_rm_;
+      max_det_s = s_rm_;
+    }
+  }
+  else
+  {
+    if (ctrv_det > rm_det)
+    {
+      max_det_z = z_pred_ctrv_;
+      max_det_s = s_ctrv_;
+    }
+    else
+    {
+      max_det_z = z_pred_rm_;
+      max_det_s = s_rm_;
+    }
+  }
+}
+
+void UKF::updateEachMotion(const double detection_probability, const double gate_probability, const double gating_thres,
+                           const iv::fusion::fusionobjectarray& object_vec,
+                           std::vector<double>& lambda_vec)
+{
+  // calculating association probability
+  double num_meas = object_vec.obj_size();
+  double b = 2 * num_meas * (1 - detection_probability * gate_probability) / (gating_thres * detection_probability);
+
+  Eigen::VectorXd max_det_z;
+  Eigen::MatrixXd max_det_s;
+  findMaxZandS(max_det_z, max_det_s);
+  double Vk = M_PI * sqrt(gating_thres * max_det_s.determinant());
+
+  for (int motion_ind = 0; motion_ind < num_motion_model_; motion_ind++)
+  {
+    Eigen::MatrixXd x(x_cv_.rows(), x_cv_.cols());
+    Eigen::MatrixXd p(p_cv_.rows(), p_cv_.cols());
+    bool is_direction_available = false;
+    int num_meas_state = 0;
+    Eigen::VectorXd z_pred;
+    Eigen::MatrixXd s_pred;
+    Eigen::MatrixXd kalman_gain;
+    Eigen::VectorXd likely_meas;
+    double e_sum = 0;
+    std::vector<double> e_vec;
+    std::vector<Eigen::VectorXd> diff_vec;
+    std::vector<Eigen::VectorXd> meas_vec;
+
+    if (motion_ind == MotionModel::CV)
+    {
+      x = x_cv_;
+      p = p_cv_;
+      if (is_direction_cv_available_)
+      {
+        is_direction_available = true;
+        num_meas_state = num_lidar_direction_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_lidar_direction_cv_;
+        s_pred = s_lidar_direction_cv_;
+        kalman_gain = k_lidar_direction_cv_;
+      }
+      else
+      {
+        num_meas_state = num_lidar_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_cv_;
+        s_pred = s_cv_;
+        kalman_gain = k_cv_;
+      }
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      x = x_ctrv_;
+      p = p_ctrv_;
+      if (is_direction_ctrv_available_)
+      {
+        is_direction_available = true;
+        num_meas_state = num_lidar_direction_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_lidar_direction_ctrv_;
+        s_pred = s_lidar_direction_ctrv_;
+        kalman_gain = k_lidar_direction_ctrv_;
+      }
+      else
+      {
+        num_meas_state = num_lidar_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_ctrv_;
+        s_pred = s_ctrv_;
+        kalman_gain = k_ctrv_;
+      }
+    }
+    else
+    {
+      x = x_rm_;
+      p = p_rm_;
+      if (is_direction_rm_available_)
+      {
+        is_direction_available = true;
+        num_meas_state = num_lidar_direction_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_lidar_direction_rm_;
+        s_pred = s_lidar_direction_rm_;
+        kalman_gain = k_lidar_direction_rm_;
+      }
+      else
+      {
+        num_meas_state = num_lidar_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_rm_;
+        s_pred = s_rm_;
+        kalman_gain = k_rm_;
+      }
+    }
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      Eigen::VectorXd meas = Eigen::VectorXd(num_meas_state);
+      meas(0) = object_vec.obj(i).centroid().x();
+      meas(1) = object_vec.obj(i).centroid().y();
+      if (is_direction_available)
+        meas(2) = object_vec.obj(i).yaw();
+      meas_vec.push_back(meas);
+      Eigen::VectorXd diff = meas - z_pred;
+      diff_vec.push_back(diff);
+      double e = exp(-0.5 * diff.transpose() * s_pred.inverse() * diff);
+      e_vec.push_back(e);
+      e_sum += e;
+    }
+    double beta_zero = b / (b + e_sum);
+
+    std::vector<double> beta_vec;
+
+    if (num_meas != 0)
+    {
+      std::vector<double>::iterator max_iter = std::max_element(e_vec.begin(), e_vec.end());
+      int max_ind = std::distance(e_vec.begin(), max_iter);
+      likely_meas = meas_vec[max_ind];
+    }
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      double temp = e_vec[i] / (b + e_sum);
+      beta_vec.push_back(temp);
+    }
+    Eigen::VectorXd sigma_x;
+    sigma_x.setZero(num_meas_state);
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      sigma_x += beta_vec[i] * diff_vec[i];
+    }
+
+    Eigen::MatrixXd sigma_p;
+    sigma_p.setZero(num_meas_state, num_meas_state);
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      sigma_p += (beta_vec[i] * diff_vec[i] * diff_vec[i].transpose() - sigma_x * sigma_x.transpose());
+    }
+
+    // update x and P
+    Eigen::MatrixXd updated_x(x_cv_.rows(), x_cv_.cols());
+    updated_x = x + kalman_gain * sigma_x;
+
+    updated_x(3) = normalizeAngle(updated_x(3));
+
+    Eigen::MatrixXd updated_p(p_cv_.rows(), p_cv_.cols());
+    if (num_meas != 0)
+    {
+      updated_p = beta_zero * p + (1 - beta_zero) * (p - kalman_gain * s_pred * kalman_gain.transpose()) +
+                  kalman_gain * sigma_p * kalman_gain.transpose();
+    }
+    else
+    {
+      updated_p = p - kalman_gain * s_pred * kalman_gain.transpose();
+    }
+
+    double lambda;
+    if (num_meas != 0)
+    {
+      lambda =
+          (1 - gate_probability * detection_probability) / pow(Vk, num_meas) +
+          detection_probability * pow(Vk, 1 - num_meas) * e_sum / (num_meas * sqrt(2 * M_PI * s_pred.determinant()));
+    }
+    else
+    {
+      lambda = (1 - gate_probability * detection_probability);
+    }
+
+    lambda_vec.push_back(lambda);
+
+    if (motion_ind == MotionModel::CV)
+    {
+      x_cv_ = updated_x;
+      p_cv_ = updated_p;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      x_ctrv_ = updated_x;
+      p_ctrv_ = updated_p;
+    }
+    else
+    {
+      x_rm_ = updated_x;
+      p_rm_ = updated_p;
+    }
+  }
+}
+
+void UKF::updateMeasurementForCTRV(const iv::fusion::fusionobjectarray& object_vec)
+{
+  std::vector<double> e_ctrv_vec;
+  std::vector<Eigen::VectorXd> meas_vec;
+  int i;
+  for(i=0;i<object_vec.obj_size();i++)
+//  for (auto const& object : object_vec)
+  {
+      iv::fusion::fusionobject object = object_vec.obj(i);
+    Eigen::VectorXd meas;
+    if (is_direction_ctrv_available_)
+    {
+      meas = Eigen::VectorXd(num_lidar_direction_state_);
+      meas << object.centroid().x(), object.centroid().y(), object.yaw();
+      meas_vec.push_back(meas);
+      Eigen::VectorXd diff_ctrv = meas - z_pred_lidar_direction_ctrv_;
+      double e_ctrv = exp(-0.5 * diff_ctrv.transpose() * s_lidar_direction_ctrv_.inverse() * diff_ctrv);
+      e_ctrv_vec.push_back(e_ctrv);
+    }
+    else
+    {
+      meas = Eigen::VectorXd(num_lidar_state_);
+      meas << object.centroid().x(), object.centroid().y();
+      meas_vec.push_back(meas);
+      Eigen::VectorXd diff_ctrv = meas - z_pred_ctrv_;
+      double e_ctrv = exp(-0.5 * diff_ctrv.transpose() * s_ctrv_.inverse() * diff_ctrv);
+      e_ctrv_vec.push_back(e_ctrv);
+    }
+  }
+  std::vector<double>::iterator max_ctrv_iter = std::max_element(e_ctrv_vec.begin(), e_ctrv_vec.end());
+  int max_ctrv_ind = std::distance(e_ctrv_vec.begin(), max_ctrv_iter);
+  if (is_direction_ctrv_available_)
+  {
+    lidar_direction_ctrv_meas_ = meas_vec[max_ctrv_ind];
+  }
+  else
+  {
+    ctrv_meas_ = meas_vec[max_ctrv_ind];
+  }
+}
+
+void UKF::uppateForCTRV()
+{
+  Eigen::VectorXd x = x_ctrv_.col(0);
+
+  if (is_direction_ctrv_available_)
+  {
+    x_ctrv_.col(0) = x + k_lidar_direction_ctrv_ * (lidar_direction_ctrv_meas_ - z_pred_lidar_direction_ctrv_);
+    p_ctrv_ = p_ctrv_ - k_lidar_direction_ctrv_ * s_lidar_direction_ctrv_ * k_lidar_direction_ctrv_.transpose();
+    x_merge_.col(0) = x_ctrv_.col(0);
+  }
+  else
+  {
+    x_ctrv_.col(0) = x + k_ctrv_ * (ctrv_meas_ - z_pred_ctrv_);
+    p_ctrv_ = p_ctrv_ - k_ctrv_ * s_ctrv_ * k_ctrv_.transpose();
+    x_merge_.col(0) = x_ctrv_.col(0);
+  }
+}
+
+void UKF::updateSUKF(const iv::fusion::fusionobjectarray& object_vec)
+{
+  // Applying this skip process only to updateSUKF
+  // since updateIMMUKF update covariance even if there is no measurement.
+  if (object_vec.obj_size() == 0)
+  {
+    return;
+  }
+  updateKalmanGain(MotionModel::CTRV);
+  updateMeasurementForCTRV(object_vec);
+  uppateForCTRV();
+}
+
+void UKF::updateIMMUKF(const double detection_probability, const double gate_probability, const double gating_thres,
+                       const iv::fusion::fusionobjectarray& object_vec)
+{
+  /*****************************************************************************
+  *  IMM Update
+  ****************************************************************************/
+  // update kalman gain
+  updateKalmanGain(MotionModel::CV);
+  updateKalmanGain(MotionModel::CTRV);
+  updateKalmanGain(MotionModel::RM);
+
+  // update state varibale x and state covariance p
+  std::vector<double> lambda_vec;
+  updateEachMotion(detection_probability, gate_probability, gating_thres, object_vec, lambda_vec);
+  /*****************************************************************************
+  *  IMM Merge Step
+  ****************************************************************************/
+  updateModeProb(lambda_vec);
+  mergeEstimationAndCovariance();
+}
+
+void UKF::ctrv(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+               const double delta_t, std::vector<double>& state)
+{
+  // 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;
+
+  while (yaw_p > M_PI)
+    yaw_p -= 2. * M_PI;
+  while (yaw_p < -M_PI)
+    yaw_p += 2. * M_PI;
+
+  state[0] = px_p;
+  state[1] = py_p;
+  state[2] = v_p;
+  state[3] = yaw_p;
+  state[4] = yawd_p;
+}
+
+void UKF::cv(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+             const double delta_t, std::vector<double>& state)
+{
+  // Reference: Bayesian Environment Representation, Prediction, and Criticality Assessment for Driver Assistance
+  // Systems, 2016
+  double px_p = p_x + v * cos(yaw) * delta_t;
+  double py_p = p_y + v * sin(yaw) * delta_t;
+  double v_p = v;
+  double yaw_p = yaw;
+  double yawd_p = 0;
+
+  state[0] = px_p;
+  state[1] = py_p;
+  state[2] = v_p;
+  state[3] = yaw_p;
+  state[4] = yawd_p;
+}
+
+void UKF::randomMotion(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+                       const double delta_t, std::vector<double>& state)
+{
+  // Reference: Bayesian Environment Representation, Prediction, and Criticality Assessment for Driver Assistance
+  // Systems, 2016
+  double px_p = p_x;
+  double py_p = p_y;
+  double v_p = 0.0;
+  double yaw_p = yaw;
+  double yawd_p = 0;
+
+  state[0] = px_p;
+  state[1] = py_p;
+  state[2] = v_p;
+  state[3] = yaw_p;
+  state[4] = yawd_p;
+}
+
+void UKF::initCovarQs(const double dt, const double yaw)
+{
+  if (tracking_num_ != TrackingState::Init)
+  {
+    return;
+  }
+  double dt_2 = dt * dt;
+  double dt_3 = dt_2 * dt;
+  double dt_4 = dt_3 * dt;
+  double cos_yaw = cos(yaw);
+  double sin_yaw = sin(yaw);
+  double cos_2_yaw = cos(yaw) * cos(yaw);
+  double sin_2_yaw = sin(yaw) * sin(yaw);
+  double cos_sin = cos_yaw * sin_yaw;
+
+  double cv_var_a = std_a_cv_ * std_a_cv_;
+  double cv_var_yawdd = std_cv_yawdd_ * std_cv_yawdd_;
+
+  double ctrv_var_a = std_a_ctrv_ * std_a_ctrv_;
+  double ctrv_var_yawdd = std_ctrv_yawdd_ * std_ctrv_yawdd_;
+
+  double rm_var_a = std_a_rm_ * std_a_rm_;
+  double rm_var_yawdd = std_rm_yawdd_ * std_rm_yawdd_;
+
+  q_cv_ << 0.5 * 0.5 * dt_4 * cos_2_yaw * cv_var_a, 0.5 * 0.5 * dt_4 * cos_sin * cv_var_a,
+      0.5 * dt_3 * cos_yaw * cv_var_a, 0, 0, 0.5 * 0.5 * dt_4 * cos_sin * cv_var_a,
+      0.5 * 0.5 * dt_4 * sin_2_yaw * cv_var_a, 0.5 * dt_3 * sin_yaw * cv_var_a, 0, 0, 0.5 * dt_3 * cos_yaw * cv_var_a,
+      0.5 * dt_3 * sin_yaw * cv_var_a, dt_2 * cv_var_a, 0, 0, 0, 0, 0, 0.5 * 0.5 * dt_4 * cv_var_yawdd,
+      0.5 * dt_3 * cv_var_yawdd, 0, 0, 0, 0.5 * dt_3 * cv_var_yawdd, dt_2 * cv_var_yawdd;
+  q_ctrv_ << 0.5 * 0.5 * dt_4 * cos_2_yaw * ctrv_var_a, 0.5 * 0.5 * dt_4 * cos_sin * ctrv_var_a,
+      0.5 * dt_3 * cos_yaw * ctrv_var_a, 0, 0, 0.5 * 0.5 * dt_4 * cos_sin * ctrv_var_a,
+      0.5 * 0.5 * dt_4 * sin_2_yaw * ctrv_var_a, 0.5 * dt_3 * sin_yaw * ctrv_var_a, 0, 0,
+      0.5 * dt_3 * cos_yaw * ctrv_var_a, 0.5 * dt_3 * sin_yaw * ctrv_var_a, dt_2 * ctrv_var_a, 0, 0, 0, 0, 0,
+      0.5 * 0.5 * dt_4 * ctrv_var_yawdd, 0.5 * dt_3 * ctrv_var_yawdd, 0, 0, 0, 0.5 * dt_3 * ctrv_var_yawdd,
+      dt_2 * ctrv_var_yawdd;
+  q_rm_ << 0.5 * 0.5 * dt_4 * cos_2_yaw * rm_var_a, 0.5 * 0.5 * dt_4 * cos_sin * rm_var_a,
+      0.5 * dt_3 * cos_yaw * rm_var_a, 0, 0, 0.5 * 0.5 * dt_4 * cos_sin * rm_var_a,
+      0.5 * 0.5 * dt_4 * sin_2_yaw * rm_var_a, 0.5 * dt_3 * sin_yaw * rm_var_a, 0, 0, 0.5 * dt_3 * cos_yaw * rm_var_a,
+      0.5 * dt_3 * sin_yaw * rm_var_a, dt_2 * rm_var_a, 0, 0, 0, 0, 0, 0.5 * 0.5 * dt_4 * rm_var_yawdd,
+      0.5 * dt_3 * rm_var_yawdd, 0, 0, 0, 0.5 * dt_3 * rm_var_yawdd, dt_2 * rm_var_yawdd;
+}
+
+void UKF::predictionMotion(const double delta_t, const int model_ind)
+{
+  /*****************************************************************************
+ *  Initialize model parameters
+ ****************************************************************************/
+  Eigen::MatrixXd x(x_cv_.rows(), 1);
+  Eigen::MatrixXd p(p_cv_.rows(), p_cv_.cols());
+  Eigen::MatrixXd q(p_cv_.rows(), p_cv_.cols());
+  Eigen::MatrixXd x_sig_pred(x_sig_pred_cv_.rows(), x_sig_pred_cv_.cols());
+  if (model_ind == MotionModel::CV)
+  {
+    x = x_cv_.col(0);
+    p = p_cv_;
+    q = q_cv_;
+    x_sig_pred = x_sig_pred_cv_;
+  }
+  else if (model_ind == MotionModel::CTRV)
+  {
+    x = x_ctrv_.col(0);
+    p = p_ctrv_;
+    q = q_ctrv_;
+    x_sig_pred = x_sig_pred_ctrv_;
+  }
+  else
+  {
+    x = x_rm_.col(0);
+    p = p_rm_;
+    q = q_rm_;
+    x_sig_pred = x_sig_pred_rm_;
+  }
+
+  /*****************************************************************************
+  *  Create Sigma Points
+  ****************************************************************************/
+
+  Eigen::MatrixXd x_sig = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // create square root matrix
+  Eigen::MatrixXd L = p.llt().matrixL();
+
+  // create augmented sigma points
+  x_sig.col(0) = x;
+  for (int i = 0; i < num_state_; i++)
+  {
+    Eigen::VectorXd pred1 = x + sqrt(lambda_ + num_state_) * L.col(i);
+    Eigen::VectorXd pred2 = x - sqrt(lambda_ + num_state_) * L.col(i);
+
+    while (pred1(3) > M_PI)
+      pred1(3) -= 2. * M_PI;
+    while (pred1(3) < -M_PI)
+      pred1(3) += 2. * M_PI;
+
+    while (pred2(3) > M_PI)
+      pred2(3) -= 2. * M_PI;
+    while (pred2(3) < -M_PI)
+      pred2(3) += 2. * M_PI;
+
+    x_sig.col(i + 1) = pred1;
+    x_sig.col(i + 1 + num_state_) = pred2;
+  }
+
+  /*****************************************************************************
+  *  Predict Sigma Points
+  ****************************************************************************/
+  // predict sigma points
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    // extract values for better readability
+    double p_x = x_sig(0, i);
+    double p_y = x_sig(1, i);
+    double v = x_sig(2, i);
+    double yaw = x_sig(3, i);
+    double yawd = x_sig(4, i);
+
+    std::vector<double> state(5);
+    if (model_ind == MotionModel::CV)
+      cv(p_x, p_y, v, yaw, yawd, delta_t, state);
+    else if (model_ind == MotionModel::CTRV)
+      ctrv(p_x, p_y, v, yaw, yawd, delta_t, state);
+    else
+      randomMotion(p_x, p_y, v, yaw, yawd, delta_t, state);
+
+    // write predicted sigma point into right column
+    x_sig_pred(0, i) = state[0];
+    x_sig_pred(1, i) = state[1];
+    x_sig_pred(2, i) = state[2];
+    x_sig_pred(3, i) = state[3];
+    x_sig_pred(4, i) = state[4];
+  }
+
+  /*****************************************************************************
+  *  Convert Predicted Sigma Points to Mean/Covariance
+  ****************************************************************************/
+  // predicted state mean
+  x.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {  // iterate over sigma points
+    x = x + weights_s_(i) * x_sig_pred.col(i);
+  }
+
+  while (x(3) > M_PI)
+    x(3) -= 2. * M_PI;
+  while (x(3) < -M_PI)
+    x(3) += 2. * M_PI;
+  // predicted state covariance matrix
+  p.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {  // iterate over sigma points
+    // state difference
+    Eigen::VectorXd x_diff = x_sig_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_c_(i) * x_diff * x_diff.transpose();
+  }
+
+  p = p + q;
+
+  /*****************************************************************************
+  *  Update model parameters
+  ****************************************************************************/
+  if (model_ind == MotionModel::CV)
+  {
+    x_cv_.col(0) = x;
+    p_cv_ = p;
+    x_sig_pred_cv_ = x_sig_pred;
+  }
+  else if (model_ind == MotionModel::CTRV)
+  {
+    x_ctrv_.col(0) = x;
+    p_ctrv_ = p;
+    x_sig_pred_ctrv_ = x_sig_pred;
+  }
+  else
+  {
+    x_rm_.col(0) = x;
+    p_rm_ = p;
+    x_sig_pred_rm_ = x_sig_pred;
+  }
+}
+
+void UKF::updateKalmanGain(const int motion_ind)
+{
+  Eigen::VectorXd x(x_cv_.rows());
+  Eigen::MatrixXd x_sig_pred(x_sig_pred_cv_.rows(), x_sig_pred_cv_.cols());
+  Eigen::VectorXd z_pred;
+  Eigen::MatrixXd s_pred;
+  int num_meas_state = 0;
+  if (motion_ind == MotionModel::CV)
+  {
+    x = x_cv_.col(0);
+    x_sig_pred = x_sig_pred_cv_;
+    if (is_direction_cv_available_)
+    {
+      num_meas_state = num_lidar_direction_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_lidar_direction_cv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_lidar_direction_cv_;
+    }
+    else
+    {
+      num_meas_state = num_lidar_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_cv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_cv_;
+    }
+  }
+  else if (motion_ind == MotionModel::CTRV)
+  {
+    x = x_ctrv_.col(0);
+    x_sig_pred = x_sig_pred_ctrv_;
+    if (is_direction_ctrv_available_)
+    {
+      num_meas_state = num_lidar_direction_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_lidar_direction_ctrv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_lidar_direction_ctrv_;
+    }
+    else
+    {
+      num_meas_state = num_lidar_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_ctrv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_ctrv_;
+    }
+  }
+  else
+  {
+    x = x_rm_.col(0);
+    x_sig_pred = x_sig_pred_rm_;
+    if (is_direction_rm_available_)
+    {
+      num_meas_state = num_lidar_direction_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_lidar_direction_rm_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_lidar_direction_rm_;
+    }
+    else
+    {
+      num_meas_state = num_lidar_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_rm_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_rm_;
+    }
+  }
+
+  Eigen::MatrixXd cross_covariance = Eigen::MatrixXd(num_state_, num_meas_state);
+  cross_covariance.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    Eigen::VectorXd z_sig_point(num_meas_state);
+    if (num_meas_state == num_lidar_direction_state_)
+    {
+      z_sig_point << x_sig_pred(0, i), x_sig_pred(1, i), x_sig_pred(3, i);
+    }
+    else
+    {
+      z_sig_point << x_sig_pred(0, i), x_sig_pred(1, i);
+    }
+    Eigen::VectorXd z_diff = z_sig_point - z_pred;
+    Eigen::VectorXd x_diff = x_sig_pred.col(i) - x;
+
+    x_diff(3) = normalizeAngle(x_diff(3));
+
+    if (num_meas_state == num_lidar_direction_state_)
+    {
+      z_diff(2) = normalizeAngle(z_diff(2));
+    }
+
+    cross_covariance = cross_covariance + weights_c_(i) * x_diff * z_diff.transpose();
+  }
+
+  Eigen::MatrixXd kalman_gain = cross_covariance * s_pred.inverse();
+
+  if (num_meas_state == num_lidar_direction_state_)
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      k_lidar_direction_cv_ = kalman_gain;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      k_lidar_direction_ctrv_ = kalman_gain;
+    }
+    else
+    {
+      k_lidar_direction_rm_ = kalman_gain;
+    }
+  }
+  else
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      k_cv_ = kalman_gain;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      k_ctrv_ = kalman_gain;
+    }
+    else
+    {
+      k_rm_ = kalman_gain;
+    }
+  }
+}
+
+void UKF::predictionLidarMeasurement(const int motion_ind, const int num_meas_state)
+{
+  Eigen::MatrixXd x_sig_pred(x_sig_pred_cv_.rows(), x_sig_pred_cv_.cols());
+  Eigen::MatrixXd covariance_r(num_meas_state, num_meas_state);
+  if (motion_ind == MotionModel::CV)
+  {
+    x_sig_pred = x_sig_pred_cv_;
+    if (num_meas_state == num_lidar_direction_state_)
+      covariance_r = lidar_direction_r_cv_;
+    else
+      covariance_r = r_cv_;
+  }
+  else if (motion_ind == MotionModel::CTRV)
+  {
+    x_sig_pred = x_sig_pred_ctrv_;
+    if (num_meas_state == num_lidar_direction_state_)
+      covariance_r = lidar_direction_r_ctrv_;
+    else
+      covariance_r = r_ctrv_;
+  }
+  else
+  {
+    x_sig_pred = x_sig_pred_rm_;
+    if (num_meas_state == num_lidar_direction_state_)
+      covariance_r = lidar_direction_r_rm_;
+    else
+      covariance_r = r_rm_;
+  }
+
+  Eigen::MatrixXd z_sig = Eigen::MatrixXd(num_meas_state, 2 * num_state_ + 1);
+
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    double p_x = x_sig_pred(0, i);
+    double p_y = x_sig_pred(1, i);
+
+    z_sig(0, i) = p_x;
+    z_sig(1, i) = p_y;
+
+    if (num_meas_state == num_lidar_direction_state_)
+    {
+      double p_yaw = x_sig_pred(3, i);
+      z_sig(2, i) = p_yaw;
+    }
+  }
+
+  Eigen::VectorXd z_pred = Eigen::VectorXd(num_meas_state);
+  z_pred.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    z_pred = z_pred + weights_s_(i) * z_sig.col(i);
+  }
+
+  if (num_meas_state == num_lidar_direction_state_)
+    z_pred(2) = normalizeAngle(z_pred(2));
+
+  Eigen::MatrixXd s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+  s_pred.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    Eigen::VectorXd z_diff = z_sig.col(i) - z_pred;
+    if (num_meas_state == num_lidar_direction_state_)
+      z_diff(2) = normalizeAngle(z_diff(2));
+    s_pred = s_pred + weights_c_(i) * z_diff * z_diff.transpose();
+  }
+
+  // add measurement noise covariance matrix
+  s_pred += covariance_r;
+
+  if (num_meas_state == num_lidar_direction_state_)
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      z_pred_lidar_direction_cv_ = z_pred;
+      s_lidar_direction_cv_ = s_pred;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      z_pred_lidar_direction_ctrv_ = z_pred;
+      s_lidar_direction_ctrv_ = s_pred;
+    }
+    else
+    {
+      z_pred_lidar_direction_rm_ = z_pred;
+      s_lidar_direction_rm_ = s_pred;
+    }
+  }
+  else
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      z_pred_cv_ = z_pred;
+      s_cv_ = s_pred;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      z_pred_ctrv_ = z_pred;
+      s_ctrv_ = s_pred;
+    }
+    else
+    {
+      z_pred_rm_ = z_pred;
+      s_rm_ = s_pred;
+    }
+  }
+}
+
+double UKF::calculateNIS(const iv::fusion::fusionobject& in_object, const int motion_ind)
+{
+  Eigen::VectorXd z_pred = Eigen::VectorXd(num_lidar_direction_state_);
+  Eigen::MatrixXd s_pred = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+  if (motion_ind == MotionModel::CV)
+  {
+    z_pred = z_pred_lidar_direction_cv_;
+    s_pred = s_lidar_direction_cv_;
+  }
+  else if (motion_ind == MotionModel::CTRV)
+  {
+    z_pred = z_pred_lidar_direction_ctrv_;
+    s_pred = s_lidar_direction_ctrv_;
+  }
+  else
+  {
+    z_pred = z_pred_lidar_direction_rm_;
+    s_pred = s_lidar_direction_rm_;
+  }
+
+  // Pick up yaw estimation and yaw variance
+  double diff = in_object.yaw() - z_pred(2);
+  double nis = diff * s_pred(2, 2) * diff;
+
+  return nis;
+}
+
+bool UKF::isLaneDirectionAvailable(const iv::fusion::fusionobject& in_object, const int motion_ind,
+                                   const double lane_direction_chi_thres)
+{
+  predictionLidarMeasurement(motion_ind, num_lidar_direction_state_);
+
+  double lidar_direction_nis = calculateNIS(in_object, motion_ind);
+
+  bool is_direction_available = false;
+  if (lidar_direction_nis < lane_direction_chi_thres)
+  {
+    is_direction_available = true;
+  }
+  return is_direction_available;
+}
+
+void UKF::checkLaneDirectionAvailability(const iv::fusion::fusionobject& in_object,
+                                         const double lane_direction_chi_thres, const bool use_sukf)
+{
+  if (use_sukf)
+  {
+    is_direction_ctrv_available_ = isLaneDirectionAvailable(in_object, MotionModel::CTRV, lane_direction_chi_thres);
+  }
+  else
+  {
+    is_direction_cv_available_ = isLaneDirectionAvailable(in_object, MotionModel::CV, lane_direction_chi_thres);
+    is_direction_ctrv_available_ = isLaneDirectionAvailable(in_object, MotionModel::CTRV, lane_direction_chi_thres);
+  }
+}
+
+void UKF::prediction(const bool use_sukf, const bool has_subscribed_vectormap, const double dt)
+{
+  if (use_sukf)
+  {
+    predictionSUKF(dt, has_subscribed_vectormap);
+  }
+  else
+  {
+    predictionIMMUKF(dt, has_subscribed_vectormap);
+  }
+}
+
+void UKF::update(const bool use_sukf, const double detection_probability, const double gate_probability,
+                 const double gating_thres, const iv::fusion::fusionobjectarray & object_vec)
+{
+  if (use_sukf)
+  {
+    updateSUKF(object_vec);
+  }
+  else
+  {
+    updateIMMUKF(detection_probability, gate_probability, gating_thres, object_vec);
+  }
+}

src/fusion/lidar_radar_fusion_cnn_ukf/fusion.hpp

@@ -0,0 +1,317 @@
+#ifndef FUSION_HPP
+#define FUSION_HPP
+#include "fusion_probabilities.h"
+#include "fusionobjectarray.pb.h"
+#include <iostream>
+#include "opencv2/opencv.hpp"
+#include "objectarray.pb.h"
+#include "mobileye.pb.h"
+
+#include "Eigen/Eigen"
+
+namespace iv {
+namespace fusion {
+
+//std::vector<Match_index> match_idxs;
+
+static float time_step = 0.3;
+static std::vector<std::string> labels = {"unknown","ped","bike","car","bus_or_truck"};
+
+float ComputeDis(cv::Point2f po1, cv::Point2f po2)
+{
+    return (sqrt(pow((po1.x-po2.x),2) + pow((po1.y-po2.y),2)));
+}
+
+void Get_AssociationMat(iv::lidar::objectarray& lidar_object_arry,iv::radar::radarobjectarray& radar_object_array,
+                        std::vector<match_index>& match_idx,std::vector<int>&radar_idx)
+{
+    std::cout<<" get mat  begin  "<<std::endl;
+    int nlidar = lidar_object_arry.obj_size();
+    int nradar = radar_object_array.obj_size();
+    match_idx.clear();
+    radar_idx.clear();
+    for(int i = 0; i<nlidar; i++)
+    {
+        match_index match;
+        match.nlidar = i;
+        std::vector<int> fuindex;
+        for(int j =0; j<nradar; j++)
+        {
+            if(radar_object_array.obj(j).bvalid() == false) continue;
+            if(iv::fusion::FusionProbabilities::ComputRadarLidarmatch(radar_object_array.obj(j),lidar_object_arry.obj(i)))
+            {
+                fuindex.push_back(j);
+            }
+        }
+        if(fuindex.size() >0)
+        {
+            std::vector<float> dis;
+            cv::Point2f po1;
+            po1.x = lidar_object_arry.obj(i).centroid().x();
+            po1.y = lidar_object_arry.obj(i).centroid().y();
+            for(int index =0; index< dis.size(); index++)
+            {
+                cv::Point2f po2;
+                po2.x = radar_object_array.obj(dis[index]).x();
+                po2.y = radar_object_array.obj(dis[index]).y();
+                dis.push_back(ComputeDis(po1,po2));
+            }
+//            std::cout<<"  x    y     :   "<<po1.x<<"   "<<po1.y<<std::endl;
+            auto smallest = std::min_element(std::begin(dis), std::end(dis));
+            int index_ = std::distance(std::begin(dis), smallest);
+            dis.clear();
+            match.nradar = index_;
+        }else {
+            match.nradar = -1000;
+        }
+        match_idx.push_back(match);
+
+    }
+//std::cout<<"  match_size   :   "<<match_idx.size()<<"    "<<match_idx[0].nradar<<std::endl;
+
+    for(int k = 0; k<radar_object_array.obj_size(); k++)
+    {
+        std::vector<int> indexs;
+        for(int radar_idx =0; radar_idx<match_idx.size(); radar_idx++)
+        {
+            indexs.push_back(match_idx[radar_idx].nradar);
+        }
+        if(radar_object_array.obj(k).bvalid() == false) continue;
+        if(abs(radar_object_array.obj(k).x())< 2 && radar_object_array.obj(k).y()< 100 ){
+            if(!(std::find(indexs.begin(),indexs.end(),k) !=indexs.end()))
+            {
+                radar_idx.push_back(k);
+                //            std::cout<<"    x    y   "<<radar_object_array.obj(k).x()<<"   "<<radar_object_array.obj(k).y()<<std::endl;
+            }
+        }}
+//            std::cout<<"   radar_size   :   "<<"    "<<radar_idx.size()<<std::endl;
+    std::cout<<"   get  mat end  "<<std::endl;
+}
+
+void RLfusion(iv::lidar::objectarray& lidar_object_arr,iv::radar::radarobjectarray& radar_object_array, iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array)
+{
+    std::cout<< "    get   fusion  begin   "<<std::endl;
+    lidar_radar_fusion_object_array.Clear();
+    std::vector<match_index> match_idx;
+    std::vector<int> radar_idx;
+    Get_AssociationMat(lidar_object_arr,radar_object_array,match_idx,radar_idx);
+    for(int i =0; i< match_idx.size(); i++)
+    {
+        iv::fusion::fusionobject fusion_object;
+        fusion_object.set_id(lidar_object_arr.obj(match_idx[i].nlidar).id());
+        fusion_object.set_type_name(lidar_object_arr.obj(match_idx[i].nlidar).type_name());
+        fusion_object.set_prob(lidar_object_arr.obj(match_idx[i].nlidar).score());
+        if(match_idx.at(i).nradar == -1000)
+        {
+//            std::cout<<"   fusion    is    ok  "<<std::endl;
+            fusion_object.set_yaw(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x());
+            vel_relative.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y());
+            vel_relative_ = fusion_object.mutable_vel_relative();
+            vel_relative_->CopyFrom(vel_relative);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x());
+            centroid.set_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y());
+            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
+            centroid_ = fusion_object.mutable_centroid();
+            centroid_->CopyFrom(centroid);
+        }else {
+//             std::cout<<"   fusion    is    ok  "<<std::endl;
+            fusion_object.set_yaw(radar_object_array.obj(match_idx.at(i).nradar).angle());
+            fusion_object.set_lifetime(radar_object_array.obj(match_idx.at(i).nradar).has_bridge_object());
+
+            iv::fusion::VelXY vel_relative;
+            iv::fusion::VelXY *vel_relative_;
+            vel_relative.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_relative.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            vel_relative_ = fusion_object.mutable_vel_relative();
+            vel_relative_->CopyFrom(vel_relative);
+
+            iv::fusion::VelXY vel_abs;
+            iv::fusion::VelXY *vel_abs_;
+            vel_abs.set_x(radar_object_array.obj(match_idx.at(i).nradar).vx());
+            vel_abs.set_y(radar_object_array.obj(match_idx.at(i).nradar).vy());
+            vel_abs_ = fusion_object.mutable_vel_abs();
+            vel_abs_->CopyFrom(vel_abs);
+
+            iv::fusion::PointXYZ centroid;
+            iv::fusion::PointXYZ *centroid_;
+            centroid.set_x(radar_object_array.obj(match_idx[i].nradar).x());
+            centroid.set_y(radar_object_array.obj(match_idx[i].nradar).y());
+            centroid.set_z(lidar_object_arr.obj(match_idx[i].nlidar).centroid().z());
+            centroid_ = fusion_object.mutable_centroid();
+            centroid_->CopyFrom(centroid);
+        }
+
+        iv::fusion::PointXYZ min_point;
+        iv::fusion::PointXYZ *min_point_;
+        min_point.set_x(lidar_object_arr.obj(match_idx[i].nlidar).position().x()
+                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/2.0);
+        min_point.set_y(lidar_object_arr.obj(match_idx[i].nlidar).position().y()
+                        - lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/2.0);
+        min_point_ = fusion_object.mutable_min_point();
+        min_point_->CopyFrom(min_point);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x());
+        dimension.set_y(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y());
+        dimension.set_z(lidar_object_arr.obj(match_idx[i].nlidar).dimensions().z());
+        dimension_ = fusion_object.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        if((lidar_object_arr.obj(match_idx[i].nlidar).centroid().x()>0)&&
+                (lidar_object_arr.obj(match_idx[i].nlidar).centroid().y()>0))
+        {
+            int xp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().x()/0.2)/2.0);
+            if(xp == 0)xp=1;
+            int yp = (int)((lidar_object_arr.obj(match_idx[i].nlidar).dimensions().y()/0.2)/2.0);
+            if(yp == 0)yp=1;
+            int ix,iy;
+            for(ix = 0; ix<(xp*2); ix++)
+            {
+                for(iy = 0; iy<(yp*2); iy++)
+                {
+                    iv::fusion::NomalXYZ nomal_centroid;
+                    iv::fusion::NomalXYZ *nomal_centroid_;
+                    float nomal_x = ix*0.2 - xp*0.2;
+                    float nomal_y = iy*0.2 - yp*0.2;
+                    float nomal_z = lidar_object_arr.obj(match_idx[i].nlidar).centroid().z();
+                    float s = nomal_x*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) -
+                            nomal_y*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                    float t = nomal_x*sin(lidar_object_arr.obj(match_idx[i].nlidar).tyaw()) +
+                            nomal_y*cos(lidar_object_arr.obj(match_idx[i].nlidar).tyaw());
+                    nomal_centroid.set_nomal_x(lidar_object_arr.obj(match_idx[i].nlidar).centroid().x() + s);
+                    nomal_centroid.set_nomal_y(lidar_object_arr.obj(match_idx[i].nlidar).centroid().y() + t);
+                    nomal_centroid_ = fusion_object.add_nomal_centroid();
+                    nomal_centroid_->CopyFrom(nomal_centroid);
+                }
+            }
+        }
+        iv::fusion::fusionobject *pobj = lidar_radar_fusion_object_array.add_obj();
+        pobj->CopyFrom(fusion_object);
+    }
+    for(int j = 0; j< radar_idx.size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_yaw(radar_object_array.obj(radar_idx[j]).angle());
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(radar_object_array.obj(radar_idx[j]).vx());
+        vel_relative.set_y(radar_object_array.obj(radar_idx[j]).vy());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(radar_object_array.obj(radar_idx[j]).x());
+        centroid.set_y(radar_object_array.obj(radar_idx[j]).y());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(0.5);
+        dimension.set_y(0.5);
+        dimension.set_z(1.0);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        int xp = (int)((0.5/0.2)/2.0);
+        if(xp == 0)xp=1;
+        int yp = (int)((0.5/0.2)/2.0);
+        if(yp == 0)yp=1;
+        int ix,iy;
+        for(ix = 0; ix<(xp*2); ix++)
+        {
+            for(iy = 0; iy<(yp*2); iy++)
+            {
+                iv::fusion::NomalXYZ nomal_centroid;
+                iv::fusion::NomalXYZ *nomal_centroid_;
+                float nomal_x = ix*0.2 - xp*0.2;
+                float nomal_y = iy*0.2 - yp*0.2;
+                float nomal_z = 1.0;
+                float s = nomal_x*cos(radar_object_array.obj(radar_idx[j]).angle())
+                        - nomal_y*sin(radar_object_array.obj(radar_idx[j]).angle());
+                float t = nomal_x*sin(radar_object_array.obj(radar_idx[j]).angle())
+                        + nomal_y*cos(radar_object_array.obj(radar_idx[j]).angle());
+                nomal_centroid.set_nomal_x(radar_object_array.obj(radar_idx[j]).x() + s);
+                nomal_centroid.set_nomal_y(radar_object_array.obj(radar_idx[j]).y() + t);
+                nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                nomal_centroid_->CopyFrom(nomal_centroid);
+            }
+        }
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+    }
+    std::cout<<"   fusion   end    "<<std::endl;
+}
+}
+}
+
+void AddMobileye(iv::fusion::fusionobjectarray& lidar_radar_fusion_object_array, iv::mobileye::mobileye& xobs_info)
+{
+    int time_step = 0.3;
+    for(int j = 0; j< xobs_info.xobj_size() ; j++){
+        iv::fusion::fusionobject fusion_obj;
+        fusion_obj.set_yaw(xobs_info.xobj(j).obsang());
+        iv::fusion::VelXY vel_relative;
+        iv::fusion::VelXY *vel_relative_;
+        vel_relative.set_x(xobs_info.xobj(j).obs_rel_vel_x() * tan(xobs_info.xobj(j).obsang()));
+        vel_relative.set_y(xobs_info.xobj(j).obs_rel_vel_x());
+        vel_relative_ = fusion_obj.mutable_vel_relative();
+        vel_relative_->CopyFrom(vel_relative);
+
+        iv::fusion::PointXYZ centroid;
+        iv::fusion::PointXYZ *centroid_;
+        centroid.set_x(-xobs_info.xobj(j).pos_y());
+        centroid.set_y(xobs_info.xobj(j).pos_x());
+        centroid.set_z(1.0);
+        centroid_ = fusion_obj.mutable_centroid();
+        centroid_->CopyFrom(centroid);
+
+        iv::fusion::Dimension dimension;
+        iv::fusion::Dimension *dimension_;
+        dimension.set_x(xobs_info.xobj(j).obswidth());
+        dimension.set_y(2.0);
+        dimension.set_z(1.0);
+        dimension_ = fusion_obj.mutable_dimensions();
+        dimension_->CopyFrom(dimension);
+
+        int xp = (int)((xobs_info.xobj(j).obswidth()/0.2)/2.0);
+        if(xp == 0)xp=1;
+        int yp = (int)((2.0/0.2)/2.0);
+        if(yp == 0)yp=1;
+        int ix,iy;
+        for(ix = 0; ix<(xp*2); ix++)
+        {
+            for(iy = 0; iy<(yp*2); iy++)
+            {
+                iv::fusion::NomalXYZ nomal_centroid;
+                iv::fusion::NomalXYZ *nomal_centroid_;
+                float nomal_x = ix*0.2 - xp*0.2;
+                float nomal_y = iy*0.2 - yp*0.2;
+                float nomal_z = 1.0;
+                float s = nomal_x*cos(0)
+                        - nomal_y*sin(0);
+                float t = nomal_x*sin(0)
+                        + nomal_y*cos(0);
+                nomal_centroid.set_nomal_x(-xobs_info.xobj(j).pos_y() + s);
+                nomal_centroid.set_nomal_y(xobs_info.xobj(j).pos_x() + t);
+                nomal_centroid_ = fusion_obj.add_nomal_centroid();
+                nomal_centroid_->CopyFrom(nomal_centroid);
+            }
+        }
+
+        iv::fusion::fusionobject *obj = lidar_radar_fusion_object_array.add_obj();
+        obj->CopyFrom(fusion_obj);
+    }
+
+}
+
+
+#endif // FUSION_HPP

src/fusion/lidar_radar_fusion_cnn_ukf/fusion_probabilities.cpp

@@ -0,0 +1,25 @@
+#include "fusion_probabilities.h"
+
+//毫米波雷达object点是否和激光雷达object的俯视box匹配
+int iv::fusion::FusionProbabilities::ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::lidarobject& lidarobject)
+{
+    Eigen::Matrix<float,3,1> radar_in_radar, radar_in_lidar;
+    radar_in_radar << radarPoint.x(), radarPoint.y(),-0.95;
+    radar_in_lidar = iv::fusion::Transformation::RadarToLidar(radar_in_radar);
+//    std::cout<<" x    y   "<<lidarobject.centroid().x()<<"    "<<lidarobject.centroid().y()<<std::endl;
+    if(!((radarPoint.x()>=(lidarobject.centroid().x() - lidarobject.dimensions().x()/2.0 -2))&&
+         (radarPoint.y()>= (lidarobject.centroid().y()-lidarobject.dimensions().y()/2.0 -2 ))
+         &&(radarPoint.x()<=(lidarobject.centroid().x() + lidarobject.dimensions().x()/2.0 +2))&&
+         (radarPoint.y()<=(lidarobject.centroid().y() + lidarobject.dimensions().y()/2.0 +2))))
+    {
+        return 0;
+    } else {
+        return 1;
+    }
+}
+
+
+
+
+
+

src/fusion/lidar_radar_fusion_cnn_ukf/fusion_probabilities.h

@@ -0,0 +1,35 @@
+#ifndef FUSION_PROBABILITIES_H
+#define FUSION_PROBABILITIES_H
+#include "transformation.h"
+#include "objectarray.pb.h"
+
+//struct correct_box
+//{
+//    Point max_point;
+//    Point min_point;
+//};
+
+
+namespace iv {
+namespace fusion {
+
+
+class FusionProbabilities{
+public:
+    FusionProbabilities(){};
+    ~FusionProbabilities(){};
+
+
+    static int ComputeRadarInBox(Point RadarInCamera, Point top_left, Point bot_right);
+    
+
+    static int ComputRadarLidarmatch(const  iv::radar::radarobject& radarPoint, const  iv::lidar::lidarobject& lidarobject);
+    };
+
+
+   }
+}
+
+
+#endif // FUSION_PROBABILITIES_H
+

src/fusion/lidar_radar_fusion_cnn_ukf/imageBuffer.h

@@ -0,0 +1,84 @@
+#ifndef IMAGEBUFFER_H
+#define IMAGEBUFFER_H
+
+#include <opencv2/opencv.hpp>
+#include <mutex>
+#include <condition_variable>
+#include <queue>
+
+
+template<typename T>
+class ConsumerProducerQueue
+{
+
+public:
+    ConsumerProducerQueue(int mxsz,bool dropFrame) :
+            maxSize(mxsz),dropFrame(dropFrame)
+    { }
+
+    bool add(T request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        if(dropFrame && isFull())
+        {
+            lock.unlock();
+			return false;
+        }
+        else {
+            cond.wait(lock, [this]() { return !isFull(); });
+            cpq.push(request);
+            //lock.unlock();
+            cond.notify_all();
+            return true;
+        }
+    }
+
+    void consume(T &request)
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        cond.wait(lock, [this]()
+        { return !isEmpty(); });
+        request = cpq.front();
+        cpq.pop();
+        //lock.unlock();
+        cond.notify_all();
+
+    }
+
+    bool isFull() const
+    {
+        return cpq.size() >= maxSize;
+    }
+
+    bool isEmpty() const
+    {
+        return cpq.size() == 0;
+    }
+
+    int length() const
+    {
+        return cpq.size();
+    }
+
+    void clear()
+    {
+        std::unique_lock<std::mutex> lock(mutex);
+        while (!isEmpty())
+        {
+            cpq.pop();
+        }
+        lock.unlock();
+        cond.notify_all();
+    }
+
+private:
+    std::condition_variable cond;
+    std::mutex mutex;
+    std::queue<T> cpq;
+    int maxSize;
+    bool dropFrame;
+};
+
+
+
+#endif

src/fusion/lidar_radar_fusion_cnn_ukf/imm_ukf_pda.cpp

@@ -0,0 +1,918 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "imm_ukf_pda.h"
+#include "modulecomm.h"
+#include "ivlog.h"
+extern iv::Ivlog *givlog;
+
+extern void * gpatrack;
+ImmUkfPda::ImmUkfPda()
+  : target_id_(0)
+  ,  // assign unique ukf_id_ to each tracking targets
+  init_(false),
+  frame_count_(0),
+  has_subscribed_vectormap_(false)
+{
+    tracking_frame_ = "world";
+    life_time_thres_ = 8;
+    gating_thres_ = 9.22;
+    gate_probability_ = 0.99;
+    detection_probability_ = 0.9;
+    static_velocity_thres_ = 0.5;
+    static_num_history_thres_ = 3;
+    prevent_explosion_thres_ = 1000;
+    merge_distance_threshold_ = 0.5;
+    use_sukf_ = false;
+    use_vectormap_ = false;
+    is_benchmark_ = false;
+
+
+//  private_nh_.param<std::string>("tracking_frame", tracking_frame_, "world");
+//  private_nh_.param<int>("life_time_thres", life_time_thres_, 8);
+//  private_nh_.param<double>("gating_thres", gating_thres_, 9.22);
+//  private_nh_.param<double>("gate_probability", gate_probability_, 0.99);
+//  private_nh_.param<double>("detection_probability", detection_probability_, 0.9);
+//  private_nh_.param<double>("static_velocity_thres", static_velocity_thres_, 0.5);
+//  private_nh_.param<int>("static_velocity_history_thres", static_num_history_thres_, 3);
+//  private_nh_.param<double>("prevent_explosion_thres", prevent_explosion_thres_, 1000);
+//  private_nh_.param<double>("merge_distance_threshold", merge_distance_threshold_, 0.5);
+//  private_nh_.param<bool>("use_sukf", use_sukf_, false);
+
+//  // for vectormap assisted tracking
+//  private_nh_.param<bool>("use_vectormap", use_vectormap_, false);
+//  private_nh_.param<double>("lane_direction_chi_thres", lane_direction_chi_thres_, 2.71);
+//  private_nh_.param<double>("nearest_lane_distance_thres", nearest_lane_distance_thres_, 1.0);
+//  private_nh_.param<std::string>("vectormap_frame", vectormap_frame_, "map");
+
+//  // rosparam for benchmark
+//  private_nh_.param<bool>("is_benchmark", is_benchmark_, false);
+//  private_nh_.param<std::string>("kitti_data_dir", kitti_data_dir_, "");
+//  if (is_benchmark_)
+//  {
+//    result_file_path_ = kitti_data_dir_ + "benchmark_results.txt";
+//    std::remove(result_file_path_.c_str());
+//  }
+}
+
+void ImmUkfPda::run()
+{
+//  pub_object_array_ = node_handle_.advertise<iv::lidar::objectarray >("/detection/objects", 1);
+//  sub_detected_array_ = node_handle_.subscribe("/detection/fusion_tools/objects", 1, &ImmUkfPda::callback, this);
+
+//  if (use_vectormap_)
+//  {
+//    vmap_.subscribe(private_nh_, vector_map::Category::POINT |
+//                                 vector_map::Category::NODE  |
+//                                 vector_map::Category::LANE, 1);
+//  }
+}
+
+void ImmUkfPda::callback(const iv::fusion::fusionobjectarray & input)
+{
+//  input_header_ = input.header;
+
+//  if(use_vectormap_)
+//  {
+//    checkVectormapSubscription();
+//  }
+
+//  bool success = updateNecessaryTransform();
+//  if (!success)
+//  {
+//    ROS_INFO("Could not find coordiante transformation");
+//    return;
+//  }
+
+//  iv::lidar::objectarray  transformed_input;
+   iv::fusion::fusionobjectarray  fused_objects_output;
+//  transformPoseToGlobal(input, transformed_input);
+    tracker(input, fused_objects_output);
+//  transformPoseToLocal(detected_objects_output);
+
+//  pub_object_array_.publish(detected_objects_output);
+
+//  if (is_benchmark_)
+//  {
+//    dumpResultText(detected_objects_output);
+//  }
+}
+
+void ImmUkfPda::checkVectormapSubscription()
+{
+//  if (use_vectormap_ && !has_subscribed_vectormap_)
+//  {
+//    lanes_ = vmap_.findByFilter([](const vector_map_msgs::Lane& lane) { return true; });
+//    if (lanes_.empty())
+//    {
+//      ROS_INFO("Has not subscribed vectormap");
+//    }
+//    else
+//    {
+//      has_subscribed_vectormap_ = true;
+//    }
+//  }
+}
+
+//bool ImmUkfPda::updateNecessaryTransform()
+//{
+//  bool success = true;
+//  try
+//  {
+//    tf_listener_.waitForTransform(input_header_.frame_id, tracking_frame_, ros::Time(0), ros::Duration(1.0));
+//    tf_listener_.lookupTransform(tracking_frame_, input_header_.frame_id, ros::Time(0), local2global_);
+//  }
+//  catch (tf::TransformException ex)
+//  {
+//    ROS_ERROR("%s", ex.what());
+//    success = false;
+//  }
+//  if (use_vectormap_ && has_subscribed_vectormap_)
+//  {
+//    try
+//    {
+//      tf_listener_.waitForTransform(vectormap_frame_, tracking_frame_, ros::Time(0), ros::Duration(1.0));
+//      tf_listener_.lookupTransform(vectormap_frame_, tracking_frame_, ros::Time(0), tracking_frame2lane_frame_);
+//      tf_listener_.lookupTransform(tracking_frame_, vectormap_frame_, ros::Time(0), lane_frame2tracking_frame_);
+//    }
+//    catch (tf::TransformException ex)
+//    {
+//      ROS_ERROR("%s", ex.what());
+//    }
+//  }
+//  return success;
+//}
+
+//void ImmUkfPda::transformPoseToGlobal(const iv::lidar::objectarray & input,
+//                                      iv::lidar::objectarray & transformed_input)
+//{
+//  transformed_input.header = input_header_;
+//  for (auto const &object: input.objects)
+//  {
+//    geometry_msgs::Pose out_pose = getTransformedPose(object.pose, local2global_);
+
+//    iv::lidar::object   dd;
+//    dd.header = input.header;
+//    dd = object;
+//    dd.pose = out_pose;
+
+//    transformed_input.objects.push_back(dd);
+//  }
+//}
+
+//void ImmUkfPda::transformPoseToLocal(iv::lidar::objectarray & detected_objects_output)
+//{
+//  detected_objects_output.header = input_header_;
+
+//  tf::Transform inv_local2global = local2global_.inverse();
+//  tf::StampedTransform global2local;
+//  global2local.setData(inv_local2global);
+//  for (auto& object : detected_objects_output.objects)
+//  {
+//    geometry_msgs::Pose out_pose = getTransformedPose(object.pose, global2local);
+//    object.header = input_header_;
+//    object.pose = out_pose;
+//  }
+//}
+
+//geometry_msgs::Pose ImmUkfPda::getTransformedPose(const geometry_msgs::Pose& in_pose,
+//                                                  const tf::StampedTransform& tf_stamp)
+//{
+//  tf::Transform transform;
+//  geometry_msgs::PoseStamped out_pose;
+//  transform.setOrigin(tf::Vector3(in_pose.position.x, in_pose.position().y(), in_pose.position.z));
+//  transform.setRotation(
+//      tf::Quaternion(in_pose.orientation.x, in_pose.orientation.y, in_pose.orientation.z, in_pose.orientation.w));
+//  geometry_msgs::PoseStamped pose_out;
+//  tf::poseTFToMsg(tf_stamp * transform, out_pose.pose);
+//  return out_pose.pose;
+//}
+
+void ImmUkfPda::measurementValidation(const iv::fusion::fusionobjectarray & input, UKF& target,
+                                      const bool second_init, const Eigen::VectorXd& max_det_z,
+                                      const Eigen::MatrixXd& max_det_s,
+                                      iv::fusion::fusionobjectarray & object_vec,
+                                      std::vector<bool>& matching_vec)
+{
+  // alert: different from original imm-pda filter, here picking up most likely measurement
+  // if making it allows to have more than one measurement, you will see non semipositive definite covariance
+  bool exists_smallest_nis_object = false;
+  double smallest_nis = std::numeric_limits<double>::max();
+  int smallest_nis_ind = 0;
+  for (size_t i = 0; i < input.obj_size(); i++)
+  {
+    double x = input.obj(i).centroid().x();
+    double y = input.obj(i).centroid().y();
+
+    Eigen::VectorXd meas = Eigen::VectorXd(2);
+    meas << x, y;
+
+    Eigen::VectorXd diff = meas - max_det_z;
+    double nis = diff.transpose() * max_det_s.inverse() * diff;
+
+    if (nis < gating_thres_)
+    {
+      if (nis < smallest_nis)
+      {
+        smallest_nis = nis;
+        target.object_ = input.obj(i);
+        smallest_nis_ind = i;
+        exists_smallest_nis_object = true;
+      }
+    }
+  }
+  if (exists_smallest_nis_object)
+  {
+    matching_vec[smallest_nis_ind] = true;
+    if (use_vectormap_ && has_subscribed_vectormap_)
+    {
+//      iv::lidar::object   direction_updated_object;
+//      bool use_direction_meas =
+//          updateDirection(smallest_nis, target.object_, direction_updated_object, target);
+//      if (use_direction_meas)
+//      {
+//        object_vec.push_back(direction_updated_object);
+//      }
+//      else
+//      {
+//        object_vec.push_back(target.object_);
+//      }
+    }
+    else
+    {
+        iv::fusion::fusionobject * px = object_vec.add_obj();
+        px->CopyFrom(target.object_);
+ //     object_vec.push_back(target.object_);
+    }
+  }
+}
+
+//bool ImmUkfPda::updateDirection(const double smallest_nis, const iv::lidar::object  & in_object,
+//                                    iv::lidar::object  & out_object, UKF& target)
+//{
+//  bool use_lane_direction = false;
+//  target.is_direction_cv_available_ = false;
+//  target.is_direction_ctrv_available_ = false;
+//  bool get_lane_success = storeObjectWithNearestLaneDirection(in_object, out_object);
+//  if (!get_lane_success)
+//  {
+//    return use_lane_direction;
+//  }
+//  target.checkLaneDirectionAvailability(out_object, lane_direction_chi_thres_, use_sukf_);
+//  if (target.is_direction_cv_available_ || target.is_direction_ctrv_available_)
+//  {
+//    use_lane_direction = true;
+//  }
+//  return use_lane_direction;
+//}
+
+//bool ImmUkfPda::storeObjectWithNearestLaneDirection(const iv::lidar::object  & in_object,
+//                                                 iv::lidar::object  & out_object)
+//{
+//  geometry_msgs::Pose lane_frame_pose = getTransformedPose(in_object.pose, tracking_frame2lane_frame_);
+//  double min_dist = std::numeric_limits<double>::max();
+
+//  double min_yaw = 0;
+//  for (auto const& lane : lanes_)
+//  {
+//    vector_map_msgs::Node node = vmap_.findByKey(vector_map::Key<vector_map_msgs::Node>(lane.bnid));
+//    vector_map_msgs::Point point = vmap_.findByKey(vector_map::Key<vector_map_msgs::Point>(node.pid));
+//    double distance = std::sqrt(std::pow(point.bx - lane_frame_pose.position().y(), 2) +
+//                                std::pow(point.ly - lane_frame_pose.position.x, 2));
+//    if (distance < min_dist)
+//    {
+//      min_dist = distance;
+//      vector_map_msgs::Node front_node = vmap_.findByKey(vector_map::Key<vector_map_msgs::Node>(lane.fnid));
+//      vector_map_msgs::Point front_point = vmap_.findByKey(vector_map::Key<vector_map_msgs::Point>(front_node.pid));
+//      min_yaw = std::atan2((front_point.bx - point.bx), (front_point.ly - point.ly));
+//    }
+//  }
+
+//  bool success = false;
+//  if (min_dist < nearest_lane_distance_thres_)
+//  {
+//    success = true;
+//  }
+//  else
+//  {
+//    return success;
+//  }
+
+//  // map yaw in rotation matrix representation
+//  tf::Quaternion map_quat = tf::createQuaternionFromYaw(min_yaw);
+//  tf::Matrix3x3 map_matrix(map_quat);
+
+//  // vectormap_frame to tracking_frame rotation matrix
+//  tf::Quaternion rotation_quat = lane_frame2tracking_frame_.getRotation();
+//  tf::Matrix3x3 rotation_matrix(rotation_quat);
+
+//  // rotated yaw in matrix representation
+//  tf::Matrix3x3 rotated_matrix = rotation_matrix * map_matrix;
+//  double roll, pitch, yaw;
+//  rotated_matrix.getRPY(roll, pitch, yaw);
+
+//  out_object = in_object;
+//  out_object.angle = yaw;
+//  return success;
+//}
+
+void ImmUkfPda::updateTargetWithAssociatedObject(const iv::fusion::fusionobjectarray & object_vec,
+                                                 UKF& target)
+{
+  target.lifetime_++;
+//  if (!target.object_.label.empty() && target.object_.label !="unknown")
+//  {
+//    target.label_ = target.object_.label;
+//  }
+  updateTrackingNum(object_vec, target);
+  if (target.tracking_num_ == TrackingState::Stable || target.tracking_num_ == TrackingState::Occlusion)
+  {
+    target.is_stable_ = true;
+  }
+}
+
+void ImmUkfPda::updateBehaviorState(const UKF& target, iv::fusion::fusionobject  & object)
+{
+  if (target.mode_prob_cv_ > target.mode_prob_ctrv_ && target.mode_prob_cv_ > target.mode_prob_rm_)
+  {
+      object.set_behavior_state(MotionModel::CV);
+//    object.behavior_state = MotionModel::CV;
+  }
+  else if (target.mode_prob_ctrv_ > target.mode_prob_cv_ && target.mode_prob_ctrv_ > target.mode_prob_rm_)
+  {
+      object.set_behavior_state(MotionModel::CTRV);
+//    object.behavior_state = MotionModel::CTRV;
+  }
+  else
+  {
+      object.set_behavior_state(MotionModel::RM);
+ //   object.behavior_state = MotionModel::RM;
+  }
+}
+
+void ImmUkfPda::initTracker(const iv::fusion::fusionobjectarray & input, double timestamp)
+{
+  for (size_t i = 0; i < input.obj_size(); i++)
+  {
+    double px = input.obj(i).centroid().x();
+    double py = input.obj(i).centroid().y();
+    Eigen::VectorXd init_meas = Eigen::VectorXd(2);
+    init_meas << px, py;
+
+    UKF ukf;
+    ukf.initialize(init_meas, timestamp, target_id_);
+    targets_.push_back(ukf);
+    target_id_++;
+  }
+  timestamp_ = timestamp;
+  init_ = true;
+}
+
+void ImmUkfPda::secondInit(UKF& target, const iv::fusion::fusionobjectarray & object_vec, double dt)
+{
+  if (object_vec.obj_size() == 0)
+  {
+    target.tracking_num_ = TrackingState::Die;
+    return;
+  }
+  // record init measurement for env classification
+  target.init_meas_ << target.x_merge_(0), target.x_merge_(1);
+
+  // state update
+  double target_x = object_vec.obj(0).centroid().x();
+  double target_y = object_vec.obj(0).centroid().y();
+  double target_diff_x = target_x - target.x_merge_(0);
+  double target_diff_y = target_y - target.x_merge_(1);
+  double target_yaw = atan2(target_diff_y, target_diff_x);
+  double dist = sqrt(target_diff_x * target_diff_x + target_diff_y * target_diff_y);
+  double target_v = dist / dt;
+
+  while (target_yaw > M_PI)
+    target_yaw -= 2. * M_PI;
+  while (target_yaw < -M_PI)
+    target_yaw += 2. * M_PI;
+
+  target.x_merge_(0) = target.x_cv_(0) = target.x_ctrv_(0) = target.x_rm_(0) = target_x;
+  target.x_merge_(1) = target.x_cv_(1) = target.x_ctrv_(1) = target.x_rm_(1) = target_y;
+  target.x_merge_(2) = target.x_cv_(2) = target.x_ctrv_(2) = target.x_rm_(2) = target_v;
+  target.x_merge_(3) = target.x_cv_(3) = target.x_ctrv_(3) = target.x_rm_(3) = target_yaw;
+
+  target.tracking_num_++;
+  return;
+}
+
+void ImmUkfPda::updateTrackingNum(const iv::fusion::fusionobjectarray & object_vec, UKF& target)
+{
+  if (object_vec.obj_size() > 0)
+  {
+    if (target.tracking_num_ < TrackingState::Stable)
+    {
+      target.tracking_num_++;
+    }
+    else if (target.tracking_num_ == TrackingState::Stable)
+    {
+      target.tracking_num_ = TrackingState::Stable;
+    }
+    else if (target.tracking_num_ >= TrackingState::Stable && target.tracking_num_ < TrackingState::Lost)
+    {
+      target.tracking_num_ = TrackingState::Stable;
+    }
+    else if (target.tracking_num_ == TrackingState::Lost)
+    {
+      target.tracking_num_ = TrackingState::Die;
+    }
+  }
+  else
+  {
+    if (target.tracking_num_ < TrackingState::Stable)
+    {
+      target.tracking_num_ = TrackingState::Die;
+    }
+    else if (target.tracking_num_ >= TrackingState::Stable && target.tracking_num_ < TrackingState::Lost)
+    {
+      target.tracking_num_++;
+    }
+    else if (target.tracking_num_ == TrackingState::Lost)
+    {
+      target.tracking_num_ = TrackingState::Die;
+    }
+  }
+
+  return;
+}
+
+bool ImmUkfPda::probabilisticDataAssociation(const iv::fusion::fusionobjectarray & input, const double dt,
+                                             std::vector<bool>& matching_vec,
+                                             iv::fusion::fusionobjectarray & object_vec, UKF& target)
+{
+  double det_s = 0;
+  Eigen::VectorXd max_det_z;
+  Eigen::MatrixXd max_det_s;
+  bool success = true;
+
+  if (use_sukf_)
+  {
+    max_det_z = target.z_pred_ctrv_;
+    max_det_s = target.s_ctrv_;
+    det_s = max_det_s.determinant();
+  }
+  else
+  {
+    // find maxDetS associated with predZ
+    target.findMaxZandS(max_det_z, max_det_s);
+    det_s = max_det_s.determinant();
+  }
+
+  // prevent ukf not to explode
+  if (std::isnan(det_s) || det_s > prevent_explosion_thres_)
+  {
+    target.tracking_num_ = TrackingState::Die;
+    success = false;
+    return success;
+  }
+
+  bool is_second_init;
+  if (target.tracking_num_ == TrackingState::Init)
+  {
+    is_second_init = true;
+  }
+  else
+  {
+    is_second_init = false;
+  }
+
+  // measurement gating
+  measurementValidation(input, target, is_second_init, max_det_z, max_det_s, object_vec, matching_vec);
+
+  // second detection for a target: update v and yaw
+  if (is_second_init)
+  {
+    secondInit(target, object_vec, dt);
+    success = false;
+    return success;
+  }
+
+  updateTargetWithAssociatedObject(object_vec, target);
+
+  if (target.tracking_num_ == TrackingState::Die)
+  {
+    success = false;
+    return success;
+  }
+  return success;
+}
+
+void ImmUkfPda::makeNewTargets(const double timestamp, const iv::fusion::fusionobjectarray & input,
+                               const std::vector<bool>& matching_vec)
+{
+  for (size_t i = 0; i < input.obj_size(); i++)
+  {
+    if (matching_vec[i] == false)
+    {
+      double px = input.obj(i).centroid().x();
+      double py = input.obj(i).centroid().y();
+      Eigen::VectorXd init_meas = Eigen::VectorXd(2);
+      init_meas << px, py;
+
+      UKF ukf;
+      ukf.initialize(init_meas, timestamp, target_id_);
+      ukf.object_ = input.obj(i);
+      targets_.push_back(ukf);
+      target_id_++;
+    }
+  }
+}
+
+void ImmUkfPda::staticClassification()
+{
+  for (size_t i = 0; i < targets_.size(); i++)
+  {
+    // targets_[i].x_merge_(2) is referred for estimated velocity
+    double current_velocity = std::abs(targets_[i].x_merge_(2));
+    targets_[i].vel_history_.push_back(current_velocity);
+    if (targets_[i].tracking_num_ == TrackingState::Stable && targets_[i].lifetime_ > life_time_thres_)
+    {
+      int index = 0;
+      double sum_vel = 0;
+      double avg_vel = 0;
+      for (auto rit = targets_[i].vel_history_.rbegin(); index < static_num_history_thres_; ++rit)
+      {
+        index++;
+        sum_vel += *rit;
+      }
+      avg_vel = double(sum_vel / static_num_history_thres_);
+
+      if(avg_vel < static_velocity_thres_ && current_velocity < static_velocity_thres_)
+      {
+        targets_[i].is_static_ = true;
+      }
+    }
+  }
+}
+
+bool
+ImmUkfPda::arePointsClose(const iv::fusion::PointXYZ & in_point_a,
+                                const iv::fusion::PointXYZ & in_point_b,
+                                float in_radius)
+{
+  return (fabs(in_point_a.x() - in_point_b.x()) <= in_radius) && (fabs(in_point_a.y() - in_point_b.y()) <= in_radius);
+}
+
+bool
+ImmUkfPda::arePointsEqual(const iv::fusion::PointXYZ & in_point_a,
+                               const iv::fusion::PointXYZ & in_point_b)
+{
+  return arePointsClose(in_point_a, in_point_b, CENTROID_DISTANCE);
+}
+
+bool
+ImmUkfPda::isPointInPool(const std::vector<iv::fusion::PointXYZ>& in_pool,
+                          const iv::fusion::PointXYZ& in_point)
+{
+  for(size_t j=0; j<in_pool.size(); j++)
+  {
+    if (arePointsEqual(in_pool[j], in_point))
+    {
+      return true;
+    }
+  }
+  return false;
+}
+
+iv::fusion::fusionobjectarray
+ImmUkfPda::removeRedundantObjects(const iv::fusion::fusionobjectarray & in_detected_objects,
+                            const std::vector<size_t> in_tracker_indices)
+{
+  if (in_detected_objects.obj_size() != in_tracker_indices.size())
+    return in_detected_objects;
+
+  iv::fusion::fusionobjectarray  resulting_objects;
+//  resulting_objects.header = in_detected_objects.header;
+
+  std::vector<iv::fusion::PointXYZ> centroids;
+  //create unique points
+  for(size_t i=0; i<in_detected_objects.obj_size(); i++)
+  {
+    if(!isPointInPool(centroids, in_detected_objects.obj(i).centroid()))
+    {
+      centroids.push_back(in_detected_objects.obj(i).centroid());
+    }
+  }
+  //assign objects to the points
+  std::vector<std::vector<size_t>> matching_objects(centroids.size());
+  for(size_t k=0; k<in_detected_objects.obj_size(); k++)
+  {
+    const auto& object=in_detected_objects.obj(k);
+    for(size_t i=0; i< centroids.size(); i++)
+    {
+      if (arePointsClose(object.centroid(), centroids[i], merge_distance_threshold_))
+      {
+        matching_objects[i].push_back(k);//store index of matched object to this point
+      }
+    }
+  }
+  //get oldest object on each point
+  for(size_t i=0; i< matching_objects.size(); i++)
+  {
+    size_t oldest_object_index = 0;
+    int oldest_lifespan = -1;
+    std::string best_label;
+    for(size_t j=0; j<matching_objects[i].size(); j++)
+    {
+      size_t current_index = matching_objects[i][j];
+      int current_lifespan = targets_[in_tracker_indices[current_index]].lifetime_;
+      if (current_lifespan > oldest_lifespan)
+      {
+        oldest_lifespan = current_lifespan;
+        oldest_object_index = current_index;
+      }
+      if (!targets_[in_tracker_indices[current_index]].label_.empty() &&
+        targets_[in_tracker_indices[current_index]].label_ != "unknown")
+      {
+        best_label = targets_[in_tracker_indices[current_index]].label_;
+      }
+    }
+    // delete nearby targets except for the oldest target
+    for(size_t j=0; j<matching_objects[i].size(); j++)
+    {
+      size_t current_index = matching_objects[i][j];
+      if(current_index != oldest_object_index)
+      {
+        targets_[in_tracker_indices[current_index]].tracking_num_= TrackingState::Die;
+      }
+    }
+    iv::fusion::fusionobject   best_object;
+    best_object = in_detected_objects.obj(oldest_object_index);
+    if (best_label != "unknown"
+        && !best_label.empty())
+    {
+//      best_object.label = best_label;
+    }
+
+    iv::fusion::fusionobject * px = resulting_objects.add_obj();
+    px->CopyFrom(best_object);
+ //   resulting_objects.push_back(best_object);
+  }
+
+  return resulting_objects;
+
+}
+
+void ImmUkfPda::makeOutput(const iv::fusion::fusionobjectarray & input,
+                           const std::vector<bool> &matching_vec,
+                           iv::fusion::fusionobjectarray & fused_objects_output)
+{
+  iv::fusion::fusionobjectarray  tmp_objects;
+//  tmp_objects.header = input.header;
+  std::vector<size_t> used_targets_indices;
+  for (size_t i = 0; i < targets_.size(); i++)
+  {
+
+    double tx = targets_[i].x_merge_(0);
+    double ty = targets_[i].x_merge_(1);
+
+    double tv = targets_[i].x_merge_(2);
+    double tyaw = targets_[i].x_merge_(3);
+    double tyaw_rate = targets_[i].x_merge_(4);
+
+    while (tyaw > M_PI)
+      tyaw -= 2. * M_PI;
+    while (tyaw < -M_PI)
+      tyaw += 2. * M_PI;
+
+ //   tf::Quaternion q = tf::createQuaternionFromYaw(tyaw);
+
+    iv::fusion::fusionobject   dd;
+    dd = targets_[i].object_;
+    dd.set_id(targets_[i].ukf_id_);
+//    dd.id = targets_[i].ukf_id_;
+    dd.set_velocity_linear_x(tv);
+   // dd.velocity_linear_x = tv;
+    dd.set_acceleration_linear_y(tyaw_rate);
+   // dd.acceleration_linear_y = tyaw_rate;
+    dd.set_velocity_reliable(targets_[i].is_stable_);
+ //   dd.velocity_reliable = targets_[i].is_stable_;
+    dd.set_pose_reliable(targets_[i].is_stable_);
+ //   dd.pose_reliable = targets_[i].is_stable_;
+
+
+    if (!targets_[i].is_static_ && targets_[i].is_stable_)
+    {
+      // Aligh the longest side of dimentions with the estimated orientation
+      if(targets_[i].object_.dimensions().x() < targets_[i].object_.dimensions().y())
+      {
+          iv::fusion::Dimension * pd = dd.mutable_dimensions();
+          pd->set_x(targets_[i].object_.dimensions().y());
+          pd->set_y(targets_[i].object_.dimensions().x());
+//        dd.dimensions.x = targets_[i].object_.dimensions.y;
+//        dd.dimensions.y = targets_[i].object_.dimensions.x;
+      }
+
+      iv::fusion::PointXYZ * pp = dd.mutable_centroid();
+      pp->set_x(tx);
+      pp->set_y((ty));
+//      dd.position.x = tx;
+//      dd.position().y() = ty;
+
+//      if (!std::isnan(q[0]))
+//        dd.pose.orientation.x = q[0];
+//      if (!std::isnan(q[1]))
+//        dd.pose.orientation.y = q[1];
+//      if (!std::isnan(q[2]))
+//        dd.pose.orientation.z = q[2];
+//      if (!std::isnan(q[3]))
+//        dd.pose.orientation.w = q[3];
+      dd.set_yaw(tyaw);
+  //    dd.tyaw = tyaw;
+    }
+
+
+    updateBehaviorState(targets_[i], dd);
+
+    if (targets_[i].is_stable_ || (targets_[i].tracking_num_ >= TrackingState::Init &&
+                                   targets_[i].tracking_num_ < TrackingState::Stable))
+    {
+        iv::fusion::fusionobject * px = tmp_objects.add_obj();
+        px->CopyFrom(dd);
+//      tmp_objects.push_back(dd);
+      used_targets_indices.push_back(i);
+    }
+  }
+ // std::cout<<"run hear."<<std::endl;
+
+  fused_objects_output = removeRedundantObjects(tmp_objects, used_targets_indices);
+
+
+}
+
+void ImmUkfPda::removeUnnecessaryTarget()
+{
+  std::vector<UKF> temp_targets;
+  for (size_t i = 0; i < targets_.size(); i++)
+  {
+    if (targets_[i].tracking_num_ != TrackingState::Die)
+    {
+      temp_targets.push_back(targets_[i]);
+    }
+  }
+  std::vector<UKF>().swap(targets_);
+  targets_ = temp_targets;
+}
+
+void ImmUkfPda::dumpResultText(iv::fusion::fusionobjectarray & fused_objects)
+{
+  std::ofstream outputfile(result_file_path_, std::ofstream::out | std::ofstream::app);
+  for (size_t i = 0; i < fused_objects.obj_size(); i++)
+  {
+ //   double yaw = tf::getYaw(detected_objects.objects[i].pose.orientation);
+    double yaw = fused_objects.obj(i).yaw();
+    // KITTI tracking benchmark data format:
+    // (frame_number,tracked_id, object type, truncation, occlusion, observation angle, x1,y1,x2,y2, h, w, l, cx, cy,
+    // cz, yaw)
+    // x1, y1, x2, y2 are for 2D bounding box.
+    // h, w, l, are for height, width, length respectively
+    // cx, cy, cz are for object centroid
+
+    // Tracking benchmark is based on frame_number, tracked_id,
+    // bounding box dimentions and object pose(centroid and orientation) from bird-eye view
+    outputfile << std::to_string(frame_count_) << " " << std::to_string(fused_objects.obj(i).id()) << " "
+               << "Unknown"
+               << " "
+               << "-1"
+               << " "
+               << "-1"
+               << " "
+               << "-1"
+               << " "
+               << "-1 -1 -1 -1"
+               << " " << std::to_string(fused_objects.obj(i).dimensions().x()) << " "
+               << std::to_string(fused_objects.obj(i).dimensions().y()) << " "
+               << "-1"
+               << " " << std::to_string(fused_objects.obj(i).centroid().x()) << " "
+               << std::to_string(fused_objects.obj(i).centroid().y()) << " "
+               << "-1"
+               << " " << std::to_string(yaw) << "\n";
+  }
+  frame_count_++;
+}
+
+void ImmUkfPda::tracker(const iv::fusion::fusionobjectarray & input,
+                        iv::fusion::fusionobjectarray & fused_objects_output)
+{
+//  double timestamp = input.header.stamp.toSec();
+
+  if(input.obj_size() < 1)
+  {
+      fused_objects_output.set_timestamp(input.timestamp());
+      std::string out = fused_objects_output.SerializeAsString();
+    //   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+      iv::modulecomm::ModuleSendMsg(gpatrack,out.data(),out.length());
+              return;
+  }
+  double timestamp =  input.timestamp();
+//  qDebug("time stamp is %f",timestamp);
+  timestamp = timestamp/1000.0;
+  std::vector<bool> matching_vec(input.obj_size(), false);
+
+
+  if (!init_)
+  {
+    initTracker(input, timestamp);
+
+    makeOutput(input, matching_vec, fused_objects_output);
+    return;
+  }
+
+
+  double dt = (timestamp - timestamp_);
+  timestamp_ = timestamp;
+
+
+  // start UKF process
+  for (size_t i = 0; i < targets_.size(); i++)
+  {
+    targets_[i].is_stable_ = false;
+    targets_[i].is_static_ = false;
+
+    if (targets_[i].tracking_num_ == TrackingState::Die)
+    {
+      continue;
+    }
+    // prevent ukf not to explode
+    if (targets_[i].p_merge_.determinant() > prevent_explosion_thres_ ||
+        targets_[i].p_merge_(4, 4) > prevent_explosion_thres_)
+    {
+      targets_[i].tracking_num_ = TrackingState::Die;
+      continue;
+    }
+
+    targets_[i].prediction(use_sukf_, has_subscribed_vectormap_, dt);
+
+    iv::fusion::fusionobjectarray object_vec;
+    bool success = probabilisticDataAssociation(input, dt, matching_vec, object_vec, targets_[i]);
+    if (!success)
+    {
+      continue;
+    }
+
+    targets_[i].update(use_sukf_, detection_probability_, gate_probability_, gating_thres_, object_vec);
+  }
+  // end UKF process
+
+  // making new ukf target for no data association objects
+  makeNewTargets(timestamp, input, matching_vec);
+
+  // static dynamic classification
+  staticClassification();
+
+  // making output for visualization
+  makeOutput(input, matching_vec, fused_objects_output);
+
+  fused_objects_output.set_timestamp(input.timestamp());
+  if(fused_objects_output.obj_size()>0)
+  {
+      int j;
+      for(j=0;j<fused_objects_output.obj_size();j++)
+      {
+      iv::fusion::fusionobject obj = fused_objects_output.obj(j);
+      std::cout<<"obj "<<j<< " vel is "<<obj.velocity_linear_x()<<" id is "<<obj.id()<<std::endl;
+
+      }
+  }
+  else
+  {
+      std::cout<<"no obj "<<std::endl;
+  }
+
+  std::string out = fused_objects_output.SerializeAsString();
+//   char * strout = lidarobjtostr(lidarobjvec,ntlen);
+  iv::modulecomm::ModuleSendMsg(gpatrack,out.data(),out.length());
+
+  int i;
+  for(i=0;i<fused_objects_output.obj_size();i++)
+  {
+      iv::fusion::fusionobject * pobj = fused_objects_output.mutable_obj(i);
+  }
+
+  // remove unnecessary ukf object
+  removeUnnecessaryTarget();
+}
+
+void ImmUkfPda::call(const iv::fusion::fusionobjectarray &input)
+{
+    callback(input);
+}

src/fusion/lidar_radar_fusion_cnn_ukf/imm_ukf_pda.h

@@ -0,0 +1,177 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef OBJECT_TRACKING_IMM_UKF_JPDAF_H
+#define OBJECT_TRACKING_IMM_UKF_JPDAF_H
+
+
+#include <vector>
+#include <chrono>
+#include <stdio.h>
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+#include "pcl/io/pcd_io.h"
+
+#include "ukf.h"
+
+class ImmUkfPda
+{
+public:
+    void call(const iv::fusion::fusionobjectarray & input);
+private:
+  int target_id_;
+  bool init_;
+  double timestamp_;
+
+  std::vector<UKF> targets_;
+
+  // probabilistic data association params
+  double gating_thres_;
+  double gate_probability_;
+  double detection_probability_;
+
+  // object association param
+  int life_time_thres_;
+
+  // static classification param
+  double static_velocity_thres_;
+  int static_num_history_thres_;
+
+  // switch sukf and ImmUkfPda
+  bool use_sukf_;
+
+  // whether if benchmarking tracking result
+  bool is_benchmark_;
+  int frame_count_;
+  std::string kitti_data_dir_;
+
+  // for benchmark
+  std::string result_file_path_;
+
+  // prevent explode param for ukf
+  double prevent_explosion_thres_;
+
+  // for vectormap assisted tarcking
+  bool use_vectormap_;
+  bool has_subscribed_vectormap_;
+  double lane_direction_chi_thres_;
+  double nearest_lane_distance_thres_;
+  std::string vectormap_frame_;
+//  vector_map::VectorMap vmap_;
+//  std::vector<vector_map_msgs::Lane> lanes_;
+
+  double merge_distance_threshold_;
+  const double CENTROID_DISTANCE = 0.2;//distance to consider centroids the same
+
+  std::string input_topic_;
+  std::string output_topic_;
+
+  std::string tracking_frame_;
+
+//  tf::TransformListener tf_listener_;
+//  tf::StampedTransform local2global_;
+//  tf::StampedTransform tracking_frame2lane_frame_;
+//  tf::StampedTransform lane_frame2tracking_frame_;
+
+//  ros::NodeHandle node_handle_;
+//  ros::NodeHandle private_nh_;
+//  ros::Subscriber sub_detected_array_;
+//  ros::Publisher pub_object_array_;
+
+//  std_msgs::Header input_header_;
+
+  void callback(const iv::fusion::fusionobjectarray & input);
+
+
+//  void transformPoseToGlobal(const iv::lidar::objectarray & input,
+//                             iv::lidar::objectarray & transformed_input);
+//  void transformPoseToLocal(iv::lidar::objectarray & detected_objects_output);
+
+//  geometry_msgs::Pose getTransformedPose(const geometry_msgs::Pose& in_pose,
+//                                                const tf::StampedTransform& tf_stamp);
+
+//  bool updateNecessaryTransform();
+
+  void measurementValidation(const iv::fusion::fusionobjectarray & input, UKF& target, const bool second_init,
+                             const Eigen::VectorXd& max_det_z, const Eigen::MatrixXd& max_det_s,
+                             iv::fusion::fusionobjectarray  & object_vec, std::vector<bool>& matching_vec);
+  iv::fusion::fusionobject getNearestObject(UKF& target,
+                                                 const iv::fusion::fusionobjectarray  & object_vec);
+  void updateBehaviorState(const UKF& target, iv::fusion::fusionobject  & object);
+
+  void initTracker(const iv::fusion::fusionobjectarray & input, double timestamp);
+  void secondInit(UKF& target, const iv::fusion::fusionobjectarray & object_vec, double dt);
+
+  void updateTrackingNum(const iv::fusion::fusionobjectarray & object_vec, UKF& target);
+
+  bool probabilisticDataAssociation(const iv::fusion::fusionobjectarray & input, const double dt,
+                                    std::vector<bool>& matching_vec,
+                                    iv::fusion::fusionobjectarray & object_vec, UKF& target);
+  void makeNewTargets(const double timestamp, const iv::fusion::fusionobjectarray & input,
+                      const std::vector<bool>& matching_vec);
+
+  void staticClassification();
+
+  void makeOutput(const iv::fusion::fusionobjectarray & input,
+                  const std::vector<bool>& matching_vec,
+                  iv::fusion::fusionobjectarray & fused_objects_output);
+
+  void removeUnnecessaryTarget();
+
+  void dumpResultText(iv::fusion::fusionobjectarray & fused_objects);
+
+  void tracker(const iv::fusion::fusionobjectarray & transformed_input,
+               iv::fusion::fusionobjectarray & fused_objects_output);
+
+//  bool updateDirection(const double smallest_nis, const iv::lidar::object  & in_object,
+//                           iv::lidar::object  & out_object, UKF& target);
+
+//  bool storeObjectWithNearestLaneDirection(const iv::lidar::object  & in_object,
+ //                                     iv::lidar::object  & out_object);
+
+  void checkVectormapSubscription();
+
+  iv::fusion::fusionobjectarray
+  removeRedundantObjects(const iv::fusion::fusionobjectarray & in_fused_objects,
+                         const std::vector<size_t> in_tracker_indices);
+
+  iv::fusion::fusionobjectarray
+  forwardNonMatchedObject(const iv::fusion::fusionobjectarray & tmp_objects,
+                          const iv::fusion::fusionobjectarray &  input,
+                          const std::vector<bool>& matching_vec);
+
+  bool
+  arePointsClose(const iv::fusion::PointXYZ& in_point_a,
+                 const iv::fusion::PointXYZ& in_point_b,
+                 float in_radius);
+
+  bool
+  arePointsEqual(const iv::fusion::PointXYZ& in_point_a,
+                 const iv::fusion::PointXYZ& in_point_b);
+
+  bool
+  isPointInPool(const std::vector<iv::fusion::PointXYZ>& in_pool,
+                const iv::fusion::PointXYZ& in_point);
+
+  void updateTargetWithAssociatedObject(const iv::fusion::fusionobjectarray& object_vec,
+                                        UKF& target);
+
+public:
+  ImmUkfPda();
+  void run();
+};
+
+#endif /* OBJECT_TRACKING_IMM_UKF_JPDAF_H */

src/fusion/lidar_radar_fusion_cnn_ukf/lidar_radar_fusion_cnn _ukf.pro

@@ -0,0 +1,66 @@
+QT -= gui
+
+CONFIG += c++14 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any feature of Qt which as been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+INCLUDEPATH +=Tracker
+
+HEADERS += \
+    ../../include/msgtype/fusionobject.pb.h \
+    ../../include/msgtype/fusionobjectarray.pb.h \
+    ../../include/msgtype/mobileye.pb.h \
+    ../../include/msgtype/mobileye_lane.pb.h \
+    ../../include/msgtype/mobileye_obs.pb.h \
+    ../../include/msgtype/mobileye_tsr.pb.h \
+    ../../include/msgtype/object.pb.h \
+    ../../include/msgtype/objectarray.pb.h \
+    ../../include/msgtype/radarobject.pb.h \
+    ../../include/msgtype/radarobjectarray.pb.h \
+    fusion.hpp \
+    fusion_probabilities.h \
+    imm_ukf_pda.h \
+    transformation.h \
+    ukf.h
+
+
+SOURCES += \
+        ../../include/msgtype/fusionobject.pb.cc \
+        ../../include/msgtype/fusionobjectarray.pb.cc \
+    ../../include/msgtype/mobileye.pb.cc \
+    ../../include/msgtype/mobileye_lane.pb.cc \
+    ../../include/msgtype/mobileye_obs.pb.cc \
+    ../../include/msgtype/mobileye_tsr.pb.cc \
+    ../../include/msgtype/object.pb.cc \
+    ../../include/msgtype/objectarray.pb.cc \
+        ../../include/msgtype/radarobject.pb.cc \
+        ../../include/msgtype/radarobjectarray.pb.cc \
+        fusion_probabilities.cpp \
+    imm_ukf_pda.cpp \
+        main.cpp \
+        transformation.cpp \
+    ukf.cpp
+
+!include(../../../include/ivboost.pri ) {
+    error( "Couldn't find the ivboost.pri file!" )
+}
+
+INCLUDEPATH += /usr/include/eigen3
+INCLUDEPATH += /usr/include/pcl-1.8
+INCLUDEPATH += $$PWD/../../include/msgtype
+
+LIBS += -L/usr/local/lib -lprotobuf
+
+INCLUDEPATH += $$PWD/../../../include/
+
+LIBS += -L$$PWD/../../../bin -lxmlparam -lmodulecomm -livlog -livfault
+
+
+INCLUDEPATH += /usr/include/opencv4/
+LIBS += /usr/lib/aarch64-linux-gnu/libopencv*.so
+

src/fusion/lidar_radar_fusion_cnn_ukf/main.cpp

@@ -0,0 +1,141 @@
+#include <QCoreApplication>
+#include <QDateTime>
+#include <iostream>
+#include "modulecomm.h"
+#include "radarobjectarray.pb.h"
+#include "objectarray.pb.h"
+#include "fusionobjectarray.pb.h"
+#include "fusionobject.pb.h"
+#include <QThread>
+#include <QString>
+#include <QMutex>
+#include "eigen3/Eigen/Dense"
+//#include "data_manager.h"
+#include "fusion.hpp"
+#include "transformation.h"
+#include "mobileye.pb.h"
+#include "imm_ukf_pda.h"
+using namespace iv;
+using namespace fusion;
+
+
+ImmUkfPda giup;
+
+void *gpatrack = iv::modulecomm::RegisterSend("li_ra_fusion",10000000,1);
+static QMutex gMutex;
+
+typedef  iv::radar::radarobjectarray RadarDataType;
+typedef  iv::lidar::objectarray LidarDataType;
+typedef  std::chrono::system_clock::time_point TimeType;
+
+iv::radar::radarobjectarray radarobjvec;
+iv::lidar::objectarray lidar_obj;
+iv::mobileye::mobileye mobileye_info;
+iv::mobileye::obs obs_info;
+iv::mobileye::lane lane_info;
+iv::mobileye::tsr tsr_info;
+
+
+QTime gTime;
+using namespace std;
+int gntemp = 0;
+iv::fusion::fusionobjectarray li_ra_fusion;
+void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion);
+bool m_isTrackerInitialized = false;
+
+
+void Listenesrfront(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+
+    iv::radar::radarobjectarray radarobj;
+    if(nSize<1)return;
+    if(false == radarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<" Listenesrfront fail."<<std::endl;
+        return;
+    }
+    else{
+        //std::<<"srfront byte size:  "<<radarobjvec.ByteSize()<<std::endl;
+    }
+//        std::cout<<"radar is ok :  "<<radarobj.obj_size()<<std::endl;
+    gMutex.lock();
+    radarobjvec.CopyFrom(radarobj);
+    gMutex.unlock();
+}
+
+void Listenlidarcnndetect(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::lidar::objectarray lidarobj;
+    if(nSize<1)return;
+    if(false == lidarobj.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
+        return;
+    }
+//    std::cout<<"  lidar  is  ok   "<<lidarobj.obj_size()<<std::endl;
+    gMutex.lock();
+    lidar_obj.CopyFrom(lidarobj);
+//    datafusion(lidarobj,radarobjvec,li_ra_fusion);
+    gMutex.unlock();
+}
+
+void Listenmobileye(const char * strdata,const unsigned int nSize,const unsigned int index,const QDateTime * dt,const char * strmemname)
+{
+    iv::mobileye::mobileye mobileye;
+    if(nSize<1)return;
+    if(false == mobileye.ParseFromArray(strdata,nSize))
+    {
+        std::cout<<"PecerptionShow Listenesrfront fail."<<std::endl;
+        return;
+    }
+    gMutex.lock();
+    mobileye_info.CopyFrom(mobileye);
+    datafusion(lidar_obj,radarobjvec,li_ra_fusion);
+    gMutex.unlock();
+}
+
+int ccccc =0;
+void datafusion(iv::lidar::objectarray& lidar_obj,iv::radar::radarobjectarray& radarobjvec, iv::fusion::fusionobjectarray& li_ra_fusion)
+{
+//    gMutex.lock();
+//    Transformation::RadarPross(radarobjvec);
+    RLfusion(lidar_obj,radarobjvec,li_ra_fusion);
+    AddMobileye(li_ra_fusion,mobileye_info);
+    giup.call(li_ra_fusion);
+
+
+//    string out;
+
+//    if(li_ra_fusion.obj_size() == 0)
+//    {
+//        iv::fusion::fusionobject fake_obj;
+//        iv::fusion::fusionobject *fake_obj_;
+//        iv::fusion::PointXYZ fake_cen;
+//        iv::fusion::PointXYZ *fake_cen_;
+//        fake_cen.set_x(10000);
+//        fake_cen.set_y(10000);
+//        fake_cen.set_z(10000);
+//        fake_cen_ = fake_obj.mutable_centroid();
+//        fake_cen_ ->CopyFrom(fake_cen);
+
+//        fake_obj_ = li_ra_fusion.add_obj();
+//        fake_obj_->CopyFrom(fake_obj);
+//        out = li_ra_fusion.SerializeAsString();
+//    }else
+//    {
+//        out = li_ra_fusion.SerializeAsString();
+//    }
+//    iv::modulecomm::ModuleSendMsg(gfu,out.data(),out.length());
+////    gMutex.unlock();
+}
+
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+    void *gpa;
+    gpa = iv::modulecomm::RegisterRecv("radar",Listenesrfront);
+    gpa = iv::modulecomm::RegisterRecv("lidar_track",Listenlidarcnndetect);
+    gpa = iv::modulecomm::RegisterRecv("mobileye",Listenmobileye);
+    return a.exec();
+}

src/fusion/lidar_radar_fusion_cnn_ukf/perceptionoutput.cpp

@@ -0,0 +1,8 @@
+#include "perceptionoutput.h"
+
+iv::Perception::PerceptionOutput::PerceptionOutput()
+{
+
+}
+
+

src/fusion/lidar_radar_fusion_cnn_ukf/transformation.cpp

@@ -0,0 +1,72 @@
+#include "transformation.h"
+
+using namespace Eigen;
+using namespace std;
+
+//计算毫米波目标距离
+float iv::fusion::Transformation::ComputelonDistance(double x, double y)
+{
+    return (sqrt(pow(x,2) + pow(y,2)));
+}
+
+//计算毫米波目标速度
+float iv::fusion::Transformation::ComputeRadarSpeed(double vx, double vy)
+{
+    return (sqrt(pow(vx,2) + pow(vy,2)));
+}
+
+//radar to lidar
+Eigen::Matrix<float,3,1> iv::fusion::Transformation::RadarToLidar(Eigen::Matrix<float,3,1> radar_int_radar)
+{
+    Eigen::Matrix<float,3,3> radar_to_lidar_R;
+    radar_to_lidar_R << -0.07164891, -0.99734797, -0.01278487,
+                        0.99733994,  -0.07180872, 0.01251175,
+                        -0.01339663, -0.0118544,  0.99983999;
+    Eigen::Matrix<float,3,1> radar_in_lidar;
+    Eigen::Matrix<float,3,1> radar_to_lidar_T;
+    radar_to_lidar_T << -0.35455075, -0.84757324, 0.26293475;
+    radar_in_lidar = radar_to_lidar_R*radar_int_radar + radar_to_lidar_T;
+    return radar_in_lidar;
+}
+
+//lidar to radar
+Eigen::Matrix<float,3,1> iv::fusion::Transformation::LidarToRadar(Eigen::Matrix<float,3,1> lidar_in_lidar)
+{
+    Eigen::Matrix<float,3,3> lidar_to_radar_R;
+    Eigen::Matrix<float,3,1> lidar_in_radar;
+    Eigen::Matrix<float,3,1> lidar_to_radar_T;
+    lidar_in_lidar = lidar_to_radar_R*lidar_in_lidar + lidar_to_radar_T;
+    return lidar_in_radar;
+}
+
+
+
+//毫米波数据去重
+//将速度相近、目标距离相近、纵向坐标相近的目标状态置为false
+void iv::fusion::Transformation::RadarPross(iv::radar::radarobjectarray& radar_object)
+{
+    for(int r = 0; r<(radar_object.obj_size()-1);r++)
+    {
+        if(radar_object.obj(r).bvalid() == false) continue;
+        for (int s=(r+1); s<(radar_object.obj_size());s++)
+        {
+            if(radar_object.obj(s).bvalid() == false) continue;
+            float lonDistance1 = ComputelonDistance(radar_object.obj(r).x(),radar_object.obj(r).y());
+            float lonDistance2 = ComputelonDistance(radar_object.obj(s).x(),radar_object.obj(s).y());
+            float RadarSpeed1 = ComputeRadarSpeed(radar_object.obj(r).vx(),radar_object.obj(r).vy());
+            float RadarSpeeds = ComputeRadarSpeed(radar_object.obj(s).vx(), radar_object.obj(s).vy());
+
+            if(
+                   (fabs(lonDistance1- lonDistance2) <= 1.00) &&
+                   (fabs(radar_object.obj(r).x() - radar_object.obj(s).x()) <=1.00)&&
+                   (fabs(RadarSpeed1-RadarSpeeds)<=1.00)
+             )
+            {
+                iv::radar::radarobject & radar_objct_new = (iv::radar::radarobject&)radar_object.obj(r);
+                radar_objct_new.set_bvalid(false);
+            }
+
+        }
+
+    }
+}

src/fusion/lidar_radar_fusion_cnn_ukf/transformation.h

@@ -0,0 +1,58 @@
+#ifndef TRANSFORMATION_H
+#define TRANSFORMATION_H
+#include "eigen3/Eigen/Dense"
+//#include "objectarray.pb.h"
+#include "radarobjectarray.pb.h"
+#include "fusionobjectarray.pb.h"
+#include <opencv2/opencv.hpp>
+struct Point{
+    double x;
+    double y;
+    double z;
+    double i;   //强度信息
+    double r;
+    double g;
+    double b;
+};
+
+struct Vel_Struct{
+    double vx;
+    double vy;
+};
+
+struct match_index
+{
+    int nlidar;
+    int nradar;
+};
+
+namespace iv {
+namespace fusion {
+
+
+class Transformation{
+public:
+    Transformation(){};
+    ~Transformation(){};
+
+    static Point RadarToCamera(Point radarInfo);
+
+//    static Point CameraToWorld(Point CameraInfo);
+
+    static void RadarPross(iv::radar::radarobjectarray& radar_object);
+
+    static float ComputelonDistance(double x, double y);
+
+    static float ComputeRadarSpeed(double vx, double vy);
+
+    static Vel_Struct lidarVelTra(float yaw, float LineVel);
+
+    static Eigen::Matrix<float,3,1> RadarToLidar(Eigen::Matrix<float,3,1> radar_in_radar);
+    static Eigen::Matrix<float,3,1> LidarToRadar(Eigen::Matrix<float,3,1> lidar_in_lidar);
+
+
+
+    };
+}
+}
+#endif // TRANSFORMATION_H

src/fusion/lidar_radar_fusion_cnn_ukf/ukf.cpp

@@ -0,0 +1,1347 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "ukf.h"
+
+/**
+* Initializes Unscented Kalman filter
+ */
+UKF::UKF()
+  : num_state_(5)
+  , num_lidar_state_(2)
+  , num_lidar_direction_state_(3)
+  , num_motion_model_(3)
+  , is_direction_cv_available_(false)
+  , is_direction_ctrv_available_(false)
+  , is_direction_rm_available_(false)
+  , std_lane_direction_(0.15)
+{
+  // initial state vector
+  x_merge_ = Eigen::MatrixXd(5, 1);
+
+  // initial state vector
+  x_cv_ = Eigen::MatrixXd(5, 1);
+
+  // initial state vector
+  x_ctrv_ = Eigen::MatrixXd(5, 1);
+
+  // initial state vector
+  x_rm_ = Eigen::MatrixXd(5, 1);
+
+  // initial covariance matrix
+  p_merge_ = Eigen::MatrixXd(5, 5);
+
+  // initial covariance matrix
+  p_cv_ = Eigen::MatrixXd(5, 5);
+
+  // initial covariance matrix
+  p_ctrv_ = Eigen::MatrixXd(5, 5);
+
+  // initial covariance matrix
+  p_rm_ = Eigen::MatrixXd(5, 5);
+
+  // Process noise standard deviation longitudinal acceleration in m/s^2
+  std_a_cv_ = 1.5;
+  std_a_ctrv_ = 1.5;
+  std_a_rm_ = 3;
+  std_ctrv_yawdd_ = 1.5;
+  std_cv_yawdd_ = 1.5;
+  std_rm_yawdd_ = 3;
+
+  // Laser measurement noise standard deviation position1 in m
+  std_laspx_ = 0.15;
+  // Laser measurement noise standard deviation position2 in m
+  std_laspy_ = 0.15;
+
+  // time when the state is true, in us
+  time_ = 0.0;
+
+  // predicted sigma points matrix
+  x_sig_pred_cv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // predicted sigma points matrix
+  x_sig_pred_ctrv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // predicted sigma points matrix
+  x_sig_pred_rm_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // create vector for weights
+  weights_c_ = Eigen::VectorXd(2 * num_state_ + 1);
+  weights_s_ = Eigen::VectorXd(2 * num_state_ + 1);
+
+  // transition probability
+  p1_.push_back(0.9);
+  p1_.push_back(0.05);
+  p1_.push_back(0.05);
+
+  p2_.push_back(0.05);
+  p2_.push_back(0.9);
+  p2_.push_back(0.05);
+
+  p3_.push_back(0.05);
+  p3_.push_back(0.05);
+  p3_.push_back(0.9);
+
+  mode_match_prob_cv2cv_ = 0;
+  mode_match_prob_ctrv2cv_ = 0;
+  mode_match_prob_rm2cv_ = 0;
+
+  mode_match_prob_cv2ctrv_ = 0;
+  mode_match_prob_ctrv2ctrv_ = 0;
+  mode_match_prob_rm2ctrv_ = 0;
+
+  mode_match_prob_cv2rm_ = 0;
+  mode_match_prob_ctrv2rm_ = 0;
+  mode_match_prob_rm2rm_ = 0;
+
+  mode_prob_cv_ = 0.33;
+  mode_prob_ctrv_ = 0.33;
+  mode_prob_rm_ = 0.33;
+
+  z_pred_cv_ = Eigen::VectorXd(2);
+  z_pred_ctrv_ = Eigen::VectorXd(2);
+  z_pred_rm_ = Eigen::VectorXd(2);
+
+  s_cv_ = Eigen::MatrixXd(2, 2);
+  s_ctrv_ = Eigen::MatrixXd(2, 2);
+  s_rm_ = Eigen::MatrixXd(2, 2);
+
+  k_cv_ = Eigen::MatrixXd(5, 2);
+  k_ctrv_ = Eigen::MatrixXd(5, 2);
+  k_rm_ = Eigen::MatrixXd(5, 2);
+
+  pd_ = 0.9;
+  pg_ = 0.99;
+
+  // tracking parameter
+  lifetime_ = 0;
+  is_static_ = false;
+
+  // bounding box params
+  is_stable_ = false;
+  iv::fusion::Dimension * pdi = object_.mutable_dimensions();
+  pdi->set_x(1.0);
+  pdi->set_y(1.0);
+//  object_.di = 1.0;
+//  object_.dimensions.y = 1.0;
+
+  // for static classification
+  init_meas_ = Eigen::VectorXd(2);
+
+  x_merge_yaw_ = 0;
+
+  // for raukf
+  cv_meas_ = Eigen::VectorXd(2);
+  ctrv_meas_ = Eigen::VectorXd(2);
+  rm_meas_ = Eigen::VectorXd(2);
+
+  r_cv_ = Eigen::MatrixXd(2, 2);
+  r_ctrv_ = Eigen::MatrixXd(2, 2);
+  r_rm_ = Eigen::MatrixXd(2, 2);
+
+  q_cv_ = Eigen::MatrixXd(5, 5);
+  q_ctrv_ = Eigen::MatrixXd(5, 5);
+  q_rm_ = Eigen::MatrixXd(5, 5);
+
+  nis_cv_ = 0;
+  nis_ctrv_ = 0;
+  nis_rm_ = 0;
+
+  new_x_sig_cv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+  new_x_sig_ctrv_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+  new_x_sig_rm_ = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  new_z_sig_cv_ = Eigen::MatrixXd(2, 2 * num_state_ + 1);
+  new_z_sig_ctrv_ = Eigen::MatrixXd(2, 2 * num_state_ + 1);
+  new_z_sig_rm_ = Eigen::MatrixXd(2, 2 * num_state_ + 1);
+
+  new_z_pred_cv_ = Eigen::VectorXd(2);
+  new_z_pred_ctrv_ = Eigen::VectorXd(2);
+  new_z_pred_rm_ = Eigen::VectorXd(2);
+
+  new_s_cv_ = Eigen::MatrixXd(2, 2);
+  new_s_ctrv_ = Eigen::MatrixXd(2, 2);
+  new_s_rm_ = Eigen::MatrixXd(2, 2);
+
+  // for lane direction combined filter
+  lidar_direction_r_cv_ = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+  lidar_direction_r_ctrv_ = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+  lidar_direction_r_rm_ = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+
+  k_lidar_direction_cv_ = Eigen::MatrixXd(num_state_, num_lidar_direction_state_);
+  k_lidar_direction_ctrv_ = Eigen::MatrixXd(num_state_, num_lidar_direction_state_);
+  k_lidar_direction_rm_ = Eigen::MatrixXd(num_state_, num_lidar_direction_state_);
+
+  lidar_direction_ctrv_meas_ = Eigen::VectorXd(num_lidar_direction_state_);
+}
+
+double UKF::normalizeAngle(const double angle)
+{
+  double normalized_angle = angle;
+  while (normalized_angle > M_PI)
+    normalized_angle -= 2. * M_PI;
+  while (normalized_angle < -M_PI)
+    normalized_angle += 2. * M_PI;
+  return normalized_angle;
+}
+
+void UKF::initialize(const Eigen::VectorXd& z, const double timestamp, const int target_id)
+{
+  ukf_id_ = target_id;
+
+  // first measurement
+  x_merge_ << 0, 0, 0, 0, 0.1;
+
+  // init covariance matrix by hardcoding since no clue about initial state covrariance
+  p_merge_ << 0.5, 0, 0, 0, 0, 0, 0.5, 0, 0, 0, 0, 0, 3, 0, 0, 0, 0, 0, 10, 0, 0, 0, 0, 0, 1;
+
+  // set weights
+  // reference from "The Unscented Kalman Filter for Nonlinear Estimation, Eric A. Wan and Rudolph van der Merwe, 2000"
+  // alpha = 0.0025, beta = 2, k = 0
+  double alpha = 0.0025;
+  double beta = 2;
+  double k = 0;
+  lambda_ = alpha * alpha * (num_state_ + k) - num_state_;
+  double weight_s_0 = lambda_ / (lambda_ + num_state_);
+  double weight_c_0 = lambda_ / (lambda_ + num_state_) + (1 - alpha * alpha + beta);
+  weights_s_(0) = weight_s_0;
+  weights_c_(0) = weight_c_0;
+  for (int i = 1; i < 2 * num_state_ + 1; i++)
+  {  // 2n+1 weights
+    double weight = 0.5 / (num_state_ + lambda_);
+    weights_s_(i) = weight;
+    weights_c_(i) = weight;
+  }
+
+  // init timestamp
+  time_ = timestamp;
+
+  x_merge_(0) = z(0);
+  x_merge_(1) = z(1);
+
+  z_pred_cv_(0) = z(0);
+  z_pred_cv_(1) = z(1);
+
+  z_pred_ctrv_(0) = z(0);
+  z_pred_ctrv_(1) = z(1);
+
+  z_pred_rm_(0) = z(0);
+  z_pred_rm_(1) = z(1);
+
+  x_cv_ = x_ctrv_ = x_rm_ = x_merge_;
+  p_cv_ = p_ctrv_ = p_rm_ = p_merge_;
+
+  s_cv_ << 1, 0, 0, 1;
+  s_ctrv_ << 1, 0, 0, 1;
+  s_rm_ << 1, 0, 0, 1;
+
+  // initialize R covariance
+  r_cv_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
+  r_ctrv_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
+  r_rm_ << std_laspx_ * std_laspx_, 0, 0, std_laspy_ * std_laspy_;
+
+  // initialize lidar-lane R covariance
+  // clang-format off
+  lidar_direction_r_cv_ << std_laspx_ * std_laspx_,                       0,                                       0,
+                                            0, std_laspy_ * std_laspy_,                                       0,
+                                            0,                       0, std_lane_direction_*std_lane_direction_;
+  lidar_direction_r_ctrv_ << std_laspx_ * std_laspx_,                       0,                                       0,
+                                              0, std_laspy_ * std_laspy_,                                       0,
+                                              0,                       0, std_lane_direction_*std_lane_direction_;
+  lidar_direction_r_rm_ << std_laspx_ * std_laspx_,                       0,                                       0,
+                                            0, std_laspy_ * std_laspy_,                                       0,
+                                            0,                       0, std_lane_direction_*std_lane_direction_;
+  // clang-format on
+
+  // init tracking num
+  tracking_num_ = 1;
+}
+
+void UKF::updateModeProb(const std::vector<double>& lambda_vec)
+{
+  double cvGauss = lambda_vec[0];
+  double ctrvGauss = lambda_vec[1];
+  double rmGauss = lambda_vec[2];
+  double sumGauss = cvGauss * mode_prob_cv_ + ctrvGauss * mode_prob_ctrv_ + rmGauss * mode_prob_rm_;
+  mode_prob_cv_ = (cvGauss * mode_prob_cv_) / sumGauss;
+  mode_prob_ctrv_ = (ctrvGauss * mode_prob_ctrv_) / sumGauss;
+  mode_prob_rm_ = (rmGauss * mode_prob_rm_) / sumGauss;
+  // prevent each prob from becoming 0
+  if (fabs(mode_prob_cv_) < 0.0001)
+    mode_prob_cv_ = 0.0001;
+  if (fabs(mode_prob_ctrv_) < 0.0001)
+    mode_prob_ctrv_ = 0.0001;
+  if (fabs(mode_prob_rm_) < 0.0001)
+    mode_prob_rm_ = 0.0001;
+}
+
+void UKF::updateYawWithHighProb()
+{
+  if (mode_prob_cv_ > mode_prob_ctrv_)
+  {
+    if (mode_prob_cv_ > mode_prob_rm_)
+    {
+      x_merge_yaw_ = x_cv_(3);
+    }
+    else
+    {
+      x_merge_yaw_ = x_rm_(3);
+    }
+  }
+  else
+  {
+    if (mode_prob_ctrv_ > mode_prob_rm_)
+    {
+      x_merge_yaw_ = x_ctrv_(3);
+    }
+    else
+    {
+      x_merge_yaw_ = x_rm_(3);
+    }
+  }
+  x_merge_(3) = x_merge_yaw_;
+}
+
+void UKF::mergeEstimationAndCovariance()
+{
+  x_merge_ = mode_prob_cv_ * x_cv_ + mode_prob_ctrv_ * x_ctrv_ + mode_prob_rm_ * x_rm_;
+  while (x_merge_(3) > M_PI)
+    x_merge_(3) -= 2. * M_PI;
+  while (x_merge_(3) < -M_PI)
+    x_merge_(3) += 2. * M_PI;
+
+  // not interacting yaw(-pi ~ pi)
+  updateYawWithHighProb();
+
+  p_merge_ = mode_prob_cv_ * (p_cv_ + (x_cv_ - x_merge_) * (x_cv_ - x_merge_).transpose()) +
+             mode_prob_ctrv_ * (p_ctrv_ + (x_ctrv_ - x_merge_) * (x_ctrv_ - x_merge_).transpose()) +
+             mode_prob_rm_ * (p_rm_ + (x_rm_ - x_merge_) * (x_rm_ - x_merge_).transpose());
+}
+
+void UKF::mixingProbability()
+{
+  double sumProb1 = mode_prob_cv_ * p1_[0] + mode_prob_ctrv_ * p2_[0] + mode_prob_rm_ * p3_[0];
+  double sumProb2 = mode_prob_cv_ * p1_[1] + mode_prob_ctrv_ * p2_[1] + mode_prob_rm_ * p3_[1];
+  double sumProb3 = mode_prob_cv_ * p1_[2] + mode_prob_ctrv_ * p2_[2] + mode_prob_rm_ * p3_[2];
+  mode_match_prob_cv2cv_ = mode_prob_cv_ * p1_[0] / sumProb1;
+  mode_match_prob_ctrv2cv_ = mode_prob_ctrv_ * p2_[0] / sumProb1;
+  mode_match_prob_rm2cv_ = mode_prob_rm_ * p3_[0] / sumProb1;
+
+  mode_match_prob_cv2ctrv_ = mode_prob_cv_ * p1_[1] / sumProb2;
+  mode_match_prob_ctrv2ctrv_ = mode_prob_ctrv_ * p2_[1] / sumProb2;
+  mode_match_prob_rm2ctrv_ = mode_prob_rm_ * p3_[1] / sumProb2;
+
+  mode_match_prob_cv2rm_ = mode_prob_cv_ * p1_[2] / sumProb3;
+  mode_match_prob_ctrv2rm_ = mode_prob_ctrv_ * p2_[2] / sumProb3;
+  mode_match_prob_rm2rm_ = mode_prob_rm_ * p3_[2] / sumProb3;
+}
+
+void UKF::interaction()
+{
+  Eigen::MatrixXd x_pre_cv = x_cv_;
+  Eigen::MatrixXd x_pre_ctrv = x_ctrv_;
+  Eigen::MatrixXd x_pre_rm = x_rm_;
+  Eigen::MatrixXd p_pre_cv = p_cv_;
+  Eigen::MatrixXd p_pre_ctrv = p_ctrv_;
+  Eigen::MatrixXd p_pre_rm = p_rm_;
+  x_cv_ = mode_match_prob_cv2cv_ * x_pre_cv + mode_match_prob_ctrv2cv_ * x_pre_ctrv + mode_match_prob_rm2cv_ * x_pre_rm;
+  x_ctrv_ = mode_match_prob_cv2ctrv_ * x_pre_cv + mode_match_prob_ctrv2ctrv_ * x_pre_ctrv +
+            mode_match_prob_rm2ctrv_ * x_pre_rm;
+  x_rm_ = mode_match_prob_cv2rm_ * x_pre_cv + mode_match_prob_ctrv2rm_ * x_pre_ctrv + mode_match_prob_rm2rm_ * x_pre_rm;
+
+  // not interacting yaw(-pi ~ pi)
+  x_cv_(3) = x_pre_cv(3);
+  x_ctrv_(3) = x_pre_ctrv(3);
+  x_rm_(3) = x_pre_rm(3);
+
+  // normalizing angle
+  while (x_cv_(3) > M_PI)
+    x_cv_(3) -= 2. * M_PI;
+  while (x_cv_(3) < -M_PI)
+    x_cv_(3) += 2. * M_PI;
+  while (x_ctrv_(3) > M_PI)
+    x_ctrv_(3) -= 2. * M_PI;
+  while (x_ctrv_(3) < -M_PI)
+    x_ctrv_(3) += 2. * M_PI;
+  while (x_rm_(3) > M_PI)
+    x_rm_(3) -= 2. * M_PI;
+  while (x_rm_(3) < -M_PI)
+    x_rm_(3) += 2. * M_PI;
+
+  p_cv_ = mode_match_prob_cv2cv_ * (p_pre_cv + (x_pre_cv - x_cv_) * (x_pre_cv - x_cv_).transpose()) +
+          mode_match_prob_ctrv2cv_ * (p_pre_ctrv + (x_pre_ctrv - x_cv_) * (x_pre_ctrv - x_cv_).transpose()) +
+          mode_match_prob_rm2cv_ * (p_pre_rm + (x_pre_rm - x_cv_) * (x_pre_rm - x_cv_).transpose());
+  p_ctrv_ = mode_match_prob_cv2ctrv_ * (p_pre_cv + (x_pre_cv - x_ctrv_) * (x_pre_cv - x_ctrv_).transpose()) +
+            mode_match_prob_ctrv2ctrv_ * (p_pre_ctrv + (x_pre_ctrv - x_ctrv_) * (x_pre_ctrv - x_ctrv_).transpose()) +
+            mode_match_prob_rm2ctrv_ * (p_pre_rm + (x_pre_rm - x_ctrv_) * (x_pre_rm - x_ctrv_).transpose());
+  p_rm_ = mode_match_prob_cv2rm_ * (p_pre_cv + (x_pre_cv - x_rm_) * (x_pre_cv - x_rm_).transpose()) +
+          mode_match_prob_ctrv2rm_ * (p_pre_ctrv + (x_pre_ctrv - x_rm_) * (x_pre_ctrv - x_rm_).transpose()) +
+          mode_match_prob_rm2rm_ * (p_pre_rm + (x_pre_rm - x_rm_) * (x_pre_rm - x_rm_).transpose());
+}
+
+void UKF::predictionSUKF(const double dt, const bool has_subscribed_vectormap)
+{
+  /*****************************************************************************
+  *  Init covariance Q if it is necessary
+  ****************************************************************************/
+  initCovarQs(dt, x_merge_(3));
+  /*****************************************************************************
+  *  Prediction Motion Model
+  ****************************************************************************/
+  predictionMotion(dt, MotionModel::CTRV);
+  /*****************************************************************************
+  *  Prediction Measurement
+  ****************************************************************************/
+  predictionLidarMeasurement(MotionModel::CTRV, num_lidar_state_);
+  if (has_subscribed_vectormap)
+  {
+    predictionLidarMeasurement(MotionModel::CTRV, num_lidar_direction_state_);
+  }
+}
+
+void UKF::predictionIMMUKF(const double dt, const bool has_subscribed_vectormap)
+{
+  /*****************************************************************************
+  *  Init covariance Q if it is needed
+  ****************************************************************************/
+  initCovarQs(dt, x_merge_(3));
+  /*****************************************************************************
+  *  IMM Mixing and Interaction
+  ****************************************************************************/
+  mixingProbability();
+  interaction();
+  /*****************************************************************************
+  *  Prediction Motion Model
+  ****************************************************************************/
+  predictionMotion(dt, MotionModel::CV);
+  predictionMotion(dt, MotionModel::CTRV);
+  predictionMotion(dt, MotionModel::RM);
+  /*****************************************************************************
+  *  Prediction Measurement
+  ****************************************************************************/
+  predictionLidarMeasurement(MotionModel::CV, num_lidar_state_);
+  predictionLidarMeasurement(MotionModel::CTRV, num_lidar_state_);
+  predictionLidarMeasurement(MotionModel::RM, num_lidar_state_);
+
+  if (has_subscribed_vectormap)
+  {
+    predictionLidarMeasurement(MotionModel::CV, num_lidar_direction_state_);
+    predictionLidarMeasurement(MotionModel::CTRV, num_lidar_direction_state_);
+    predictionLidarMeasurement(MotionModel::RM, num_lidar_direction_state_);
+  }
+}
+
+void UKF::findMaxZandS(Eigen::VectorXd& max_det_z, Eigen::MatrixXd& max_det_s)
+{
+  double cv_det = s_cv_.determinant();
+  double ctrv_det = s_ctrv_.determinant();
+  double rm_det = s_rm_.determinant();
+
+  if (cv_det > ctrv_det)
+  {
+    if (cv_det > rm_det)
+    {
+      max_det_z = z_pred_cv_;
+      max_det_s = s_cv_;
+    }
+    else
+    {
+      max_det_z = z_pred_rm_;
+      max_det_s = s_rm_;
+    }
+  }
+  else
+  {
+    if (ctrv_det > rm_det)
+    {
+      max_det_z = z_pred_ctrv_;
+      max_det_s = s_ctrv_;
+    }
+    else
+    {
+      max_det_z = z_pred_rm_;
+      max_det_s = s_rm_;
+    }
+  }
+}
+
+void UKF::updateEachMotion(const double detection_probability, const double gate_probability, const double gating_thres,
+                           const iv::fusion::fusionobjectarray& object_vec,
+                           std::vector<double>& lambda_vec)
+{
+  // calculating association probability
+  double num_meas = object_vec.obj_size();
+  double b = 2 * num_meas * (1 - detection_probability * gate_probability) / (gating_thres * detection_probability);
+
+  Eigen::VectorXd max_det_z;
+  Eigen::MatrixXd max_det_s;
+  findMaxZandS(max_det_z, max_det_s);
+  double Vk = M_PI * sqrt(gating_thres * max_det_s.determinant());
+
+  for (int motion_ind = 0; motion_ind < num_motion_model_; motion_ind++)
+  {
+    Eigen::MatrixXd x(x_cv_.rows(), x_cv_.cols());
+    Eigen::MatrixXd p(p_cv_.rows(), p_cv_.cols());
+    bool is_direction_available = false;
+    int num_meas_state = 0;
+    Eigen::VectorXd z_pred;
+    Eigen::MatrixXd s_pred;
+    Eigen::MatrixXd kalman_gain;
+    Eigen::VectorXd likely_meas;
+    double e_sum = 0;
+    std::vector<double> e_vec;
+    std::vector<Eigen::VectorXd> diff_vec;
+    std::vector<Eigen::VectorXd> meas_vec;
+
+    if (motion_ind == MotionModel::CV)
+    {
+      x = x_cv_;
+      p = p_cv_;
+      if (is_direction_cv_available_)
+      {
+        is_direction_available = true;
+        num_meas_state = num_lidar_direction_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_lidar_direction_cv_;
+        s_pred = s_lidar_direction_cv_;
+        kalman_gain = k_lidar_direction_cv_;
+      }
+      else
+      {
+        num_meas_state = num_lidar_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_cv_;
+        s_pred = s_cv_;
+        kalman_gain = k_cv_;
+      }
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      x = x_ctrv_;
+      p = p_ctrv_;
+      if (is_direction_ctrv_available_)
+      {
+        is_direction_available = true;
+        num_meas_state = num_lidar_direction_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_lidar_direction_ctrv_;
+        s_pred = s_lidar_direction_ctrv_;
+        kalman_gain = k_lidar_direction_ctrv_;
+      }
+      else
+      {
+        num_meas_state = num_lidar_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_ctrv_;
+        s_pred = s_ctrv_;
+        kalman_gain = k_ctrv_;
+      }
+    }
+    else
+    {
+      x = x_rm_;
+      p = p_rm_;
+      if (is_direction_rm_available_)
+      {
+        is_direction_available = true;
+        num_meas_state = num_lidar_direction_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_lidar_direction_rm_;
+        s_pred = s_lidar_direction_rm_;
+        kalman_gain = k_lidar_direction_rm_;
+      }
+      else
+      {
+        num_meas_state = num_lidar_state_;
+        z_pred = Eigen::VectorXd(num_meas_state);
+        s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+        kalman_gain = Eigen::MatrixXd(num_state_, num_meas_state);
+        z_pred = z_pred_rm_;
+        s_pred = s_rm_;
+        kalman_gain = k_rm_;
+      }
+    }
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      Eigen::VectorXd meas = Eigen::VectorXd(num_meas_state);
+      meas(0) = object_vec.obj(i).centroid().x();
+      meas(1) = object_vec.obj(i).centroid().y();
+      if (is_direction_available)
+        meas(2) = object_vec.obj(i).yaw();
+      meas_vec.push_back(meas);
+      Eigen::VectorXd diff = meas - z_pred;
+      diff_vec.push_back(diff);
+      double e = exp(-0.5 * diff.transpose() * s_pred.inverse() * diff);
+      e_vec.push_back(e);
+      e_sum += e;
+    }
+    double beta_zero = b / (b + e_sum);
+
+    std::vector<double> beta_vec;
+
+    if (num_meas != 0)
+    {
+      std::vector<double>::iterator max_iter = std::max_element(e_vec.begin(), e_vec.end());
+      int max_ind = std::distance(e_vec.begin(), max_iter);
+      likely_meas = meas_vec[max_ind];
+    }
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      double temp = e_vec[i] / (b + e_sum);
+      beta_vec.push_back(temp);
+    }
+    Eigen::VectorXd sigma_x;
+    sigma_x.setZero(num_meas_state);
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      sigma_x += beta_vec[i] * diff_vec[i];
+    }
+
+    Eigen::MatrixXd sigma_p;
+    sigma_p.setZero(num_meas_state, num_meas_state);
+
+    for (size_t i = 0; i < num_meas; i++)
+    {
+      sigma_p += (beta_vec[i] * diff_vec[i] * diff_vec[i].transpose() - sigma_x * sigma_x.transpose());
+    }
+
+    // update x and P
+    Eigen::MatrixXd updated_x(x_cv_.rows(), x_cv_.cols());
+    updated_x = x + kalman_gain * sigma_x;
+
+    updated_x(3) = normalizeAngle(updated_x(3));
+
+    Eigen::MatrixXd updated_p(p_cv_.rows(), p_cv_.cols());
+    if (num_meas != 0)
+    {
+      updated_p = beta_zero * p + (1 - beta_zero) * (p - kalman_gain * s_pred * kalman_gain.transpose()) +
+                  kalman_gain * sigma_p * kalman_gain.transpose();
+    }
+    else
+    {
+      updated_p = p - kalman_gain * s_pred * kalman_gain.transpose();
+    }
+
+    double lambda;
+    if (num_meas != 0)
+    {
+      lambda =
+          (1 - gate_probability * detection_probability) / pow(Vk, num_meas) +
+          detection_probability * pow(Vk, 1 - num_meas) * e_sum / (num_meas * sqrt(2 * M_PI * s_pred.determinant()));
+    }
+    else
+    {
+      lambda = (1 - gate_probability * detection_probability);
+    }
+
+    lambda_vec.push_back(lambda);
+
+    if (motion_ind == MotionModel::CV)
+    {
+      x_cv_ = updated_x;
+      p_cv_ = updated_p;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      x_ctrv_ = updated_x;
+      p_ctrv_ = updated_p;
+    }
+    else
+    {
+      x_rm_ = updated_x;
+      p_rm_ = updated_p;
+    }
+  }
+}
+
+void UKF::updateMeasurementForCTRV(const iv::fusion::fusionobjectarray& object_vec)
+{
+  std::vector<double> e_ctrv_vec;
+  std::vector<Eigen::VectorXd> meas_vec;
+  int i;
+  for(i=0;i<object_vec.obj_size();i++)
+//  for (auto const& object : object_vec)
+  {
+      iv::fusion::fusionobject object = object_vec.obj(i);
+    Eigen::VectorXd meas;
+    if (is_direction_ctrv_available_)
+    {
+      meas = Eigen::VectorXd(num_lidar_direction_state_);
+      meas << object.centroid().x(), object.centroid().y(), object.yaw();
+      meas_vec.push_back(meas);
+      Eigen::VectorXd diff_ctrv = meas - z_pred_lidar_direction_ctrv_;
+      double e_ctrv = exp(-0.5 * diff_ctrv.transpose() * s_lidar_direction_ctrv_.inverse() * diff_ctrv);
+      e_ctrv_vec.push_back(e_ctrv);
+    }
+    else
+    {
+      meas = Eigen::VectorXd(num_lidar_state_);
+      meas << object.centroid().x(), object.centroid().y();
+      meas_vec.push_back(meas);
+      Eigen::VectorXd diff_ctrv = meas - z_pred_ctrv_;
+      double e_ctrv = exp(-0.5 * diff_ctrv.transpose() * s_ctrv_.inverse() * diff_ctrv);
+      e_ctrv_vec.push_back(e_ctrv);
+    }
+  }
+  std::vector<double>::iterator max_ctrv_iter = std::max_element(e_ctrv_vec.begin(), e_ctrv_vec.end());
+  int max_ctrv_ind = std::distance(e_ctrv_vec.begin(), max_ctrv_iter);
+  if (is_direction_ctrv_available_)
+  {
+    lidar_direction_ctrv_meas_ = meas_vec[max_ctrv_ind];
+  }
+  else
+  {
+    ctrv_meas_ = meas_vec[max_ctrv_ind];
+  }
+}
+
+void UKF::uppateForCTRV()
+{
+  Eigen::VectorXd x = x_ctrv_.col(0);
+
+  if (is_direction_ctrv_available_)
+  {
+    x_ctrv_.col(0) = x + k_lidar_direction_ctrv_ * (lidar_direction_ctrv_meas_ - z_pred_lidar_direction_ctrv_);
+    p_ctrv_ = p_ctrv_ - k_lidar_direction_ctrv_ * s_lidar_direction_ctrv_ * k_lidar_direction_ctrv_.transpose();
+    x_merge_.col(0) = x_ctrv_.col(0);
+  }
+  else
+  {
+    x_ctrv_.col(0) = x + k_ctrv_ * (ctrv_meas_ - z_pred_ctrv_);
+    p_ctrv_ = p_ctrv_ - k_ctrv_ * s_ctrv_ * k_ctrv_.transpose();
+    x_merge_.col(0) = x_ctrv_.col(0);
+  }
+}
+
+void UKF::updateSUKF(const iv::fusion::fusionobjectarray& object_vec)
+{
+  // Applying this skip process only to updateSUKF
+  // since updateIMMUKF update covariance even if there is no measurement.
+  if (object_vec.obj_size() == 0)
+  {
+    return;
+  }
+  updateKalmanGain(MotionModel::CTRV);
+  updateMeasurementForCTRV(object_vec);
+  uppateForCTRV();
+}
+
+void UKF::updateIMMUKF(const double detection_probability, const double gate_probability, const double gating_thres,
+                       const iv::fusion::fusionobjectarray& object_vec)
+{
+  /*****************************************************************************
+  *  IMM Update
+  ****************************************************************************/
+  // update kalman gain
+  updateKalmanGain(MotionModel::CV);
+  updateKalmanGain(MotionModel::CTRV);
+  updateKalmanGain(MotionModel::RM);
+
+  // update state varibale x and state covariance p
+  std::vector<double> lambda_vec;
+  updateEachMotion(detection_probability, gate_probability, gating_thres, object_vec, lambda_vec);
+  /*****************************************************************************
+  *  IMM Merge Step
+  ****************************************************************************/
+  updateModeProb(lambda_vec);
+  mergeEstimationAndCovariance();
+}
+
+void UKF::ctrv(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+               const double delta_t, std::vector<double>& state)
+{
+  // 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;
+
+  while (yaw_p > M_PI)
+    yaw_p -= 2. * M_PI;
+  while (yaw_p < -M_PI)
+    yaw_p += 2. * M_PI;
+
+  state[0] = px_p;
+  state[1] = py_p;
+  state[2] = v_p;
+  state[3] = yaw_p;
+  state[4] = yawd_p;
+}
+
+void UKF::cv(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+             const double delta_t, std::vector<double>& state)
+{
+  // Reference: Bayesian Environment Representation, Prediction, and Criticality Assessment for Driver Assistance
+  // Systems, 2016
+  double px_p = p_x + v * cos(yaw) * delta_t;
+  double py_p = p_y + v * sin(yaw) * delta_t;
+  double v_p = v;
+  double yaw_p = yaw;
+  double yawd_p = 0;
+
+  state[0] = px_p;
+  state[1] = py_p;
+  state[2] = v_p;
+  state[3] = yaw_p;
+  state[4] = yawd_p;
+}
+
+void UKF::randomMotion(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+                       const double delta_t, std::vector<double>& state)
+{
+  // Reference: Bayesian Environment Representation, Prediction, and Criticality Assessment for Driver Assistance
+  // Systems, 2016
+  double px_p = p_x;
+  double py_p = p_y;
+  double v_p = 0.0;
+  double yaw_p = yaw;
+  double yawd_p = 0;
+
+  state[0] = px_p;
+  state[1] = py_p;
+  state[2] = v_p;
+  state[3] = yaw_p;
+  state[4] = yawd_p;
+}
+
+void UKF::initCovarQs(const double dt, const double yaw)
+{
+  if (tracking_num_ != TrackingState::Init)
+  {
+    return;
+  }
+  double dt_2 = dt * dt;
+  double dt_3 = dt_2 * dt;
+  double dt_4 = dt_3 * dt;
+  double cos_yaw = cos(yaw);
+  double sin_yaw = sin(yaw);
+  double cos_2_yaw = cos(yaw) * cos(yaw);
+  double sin_2_yaw = sin(yaw) * sin(yaw);
+  double cos_sin = cos_yaw * sin_yaw;
+
+  double cv_var_a = std_a_cv_ * std_a_cv_;
+  double cv_var_yawdd = std_cv_yawdd_ * std_cv_yawdd_;
+
+  double ctrv_var_a = std_a_ctrv_ * std_a_ctrv_;
+  double ctrv_var_yawdd = std_ctrv_yawdd_ * std_ctrv_yawdd_;
+
+  double rm_var_a = std_a_rm_ * std_a_rm_;
+  double rm_var_yawdd = std_rm_yawdd_ * std_rm_yawdd_;
+
+  q_cv_ << 0.5 * 0.5 * dt_4 * cos_2_yaw * cv_var_a, 0.5 * 0.5 * dt_4 * cos_sin * cv_var_a,
+      0.5 * dt_3 * cos_yaw * cv_var_a, 0, 0, 0.5 * 0.5 * dt_4 * cos_sin * cv_var_a,
+      0.5 * 0.5 * dt_4 * sin_2_yaw * cv_var_a, 0.5 * dt_3 * sin_yaw * cv_var_a, 0, 0, 0.5 * dt_3 * cos_yaw * cv_var_a,
+      0.5 * dt_3 * sin_yaw * cv_var_a, dt_2 * cv_var_a, 0, 0, 0, 0, 0, 0.5 * 0.5 * dt_4 * cv_var_yawdd,
+      0.5 * dt_3 * cv_var_yawdd, 0, 0, 0, 0.5 * dt_3 * cv_var_yawdd, dt_2 * cv_var_yawdd;
+  q_ctrv_ << 0.5 * 0.5 * dt_4 * cos_2_yaw * ctrv_var_a, 0.5 * 0.5 * dt_4 * cos_sin * ctrv_var_a,
+      0.5 * dt_3 * cos_yaw * ctrv_var_a, 0, 0, 0.5 * 0.5 * dt_4 * cos_sin * ctrv_var_a,
+      0.5 * 0.5 * dt_4 * sin_2_yaw * ctrv_var_a, 0.5 * dt_3 * sin_yaw * ctrv_var_a, 0, 0,
+      0.5 * dt_3 * cos_yaw * ctrv_var_a, 0.5 * dt_3 * sin_yaw * ctrv_var_a, dt_2 * ctrv_var_a, 0, 0, 0, 0, 0,
+      0.5 * 0.5 * dt_4 * ctrv_var_yawdd, 0.5 * dt_3 * ctrv_var_yawdd, 0, 0, 0, 0.5 * dt_3 * ctrv_var_yawdd,
+      dt_2 * ctrv_var_yawdd;
+  q_rm_ << 0.5 * 0.5 * dt_4 * cos_2_yaw * rm_var_a, 0.5 * 0.5 * dt_4 * cos_sin * rm_var_a,
+      0.5 * dt_3 * cos_yaw * rm_var_a, 0, 0, 0.5 * 0.5 * dt_4 * cos_sin * rm_var_a,
+      0.5 * 0.5 * dt_4 * sin_2_yaw * rm_var_a, 0.5 * dt_3 * sin_yaw * rm_var_a, 0, 0, 0.5 * dt_3 * cos_yaw * rm_var_a,
+      0.5 * dt_3 * sin_yaw * rm_var_a, dt_2 * rm_var_a, 0, 0, 0, 0, 0, 0.5 * 0.5 * dt_4 * rm_var_yawdd,
+      0.5 * dt_3 * rm_var_yawdd, 0, 0, 0, 0.5 * dt_3 * rm_var_yawdd, dt_2 * rm_var_yawdd;
+}
+
+void UKF::predictionMotion(const double delta_t, const int model_ind)
+{
+  /*****************************************************************************
+ *  Initialize model parameters
+ ****************************************************************************/
+  Eigen::MatrixXd x(x_cv_.rows(), 1);
+  Eigen::MatrixXd p(p_cv_.rows(), p_cv_.cols());
+  Eigen::MatrixXd q(p_cv_.rows(), p_cv_.cols());
+  Eigen::MatrixXd x_sig_pred(x_sig_pred_cv_.rows(), x_sig_pred_cv_.cols());
+  if (model_ind == MotionModel::CV)
+  {
+    x = x_cv_.col(0);
+    p = p_cv_;
+    q = q_cv_;
+    x_sig_pred = x_sig_pred_cv_;
+  }
+  else if (model_ind == MotionModel::CTRV)
+  {
+    x = x_ctrv_.col(0);
+    p = p_ctrv_;
+    q = q_ctrv_;
+    x_sig_pred = x_sig_pred_ctrv_;
+  }
+  else
+  {
+    x = x_rm_.col(0);
+    p = p_rm_;
+    q = q_rm_;
+    x_sig_pred = x_sig_pred_rm_;
+  }
+
+  /*****************************************************************************
+  *  Create Sigma Points
+  ****************************************************************************/
+
+  Eigen::MatrixXd x_sig = Eigen::MatrixXd(num_state_, 2 * num_state_ + 1);
+
+  // create square root matrix
+  Eigen::MatrixXd L = p.llt().matrixL();
+
+  // create augmented sigma points
+  x_sig.col(0) = x;
+  for (int i = 0; i < num_state_; i++)
+  {
+    Eigen::VectorXd pred1 = x + sqrt(lambda_ + num_state_) * L.col(i);
+    Eigen::VectorXd pred2 = x - sqrt(lambda_ + num_state_) * L.col(i);
+
+    while (pred1(3) > M_PI)
+      pred1(3) -= 2. * M_PI;
+    while (pred1(3) < -M_PI)
+      pred1(3) += 2. * M_PI;
+
+    while (pred2(3) > M_PI)
+      pred2(3) -= 2. * M_PI;
+    while (pred2(3) < -M_PI)
+      pred2(3) += 2. * M_PI;
+
+    x_sig.col(i + 1) = pred1;
+    x_sig.col(i + 1 + num_state_) = pred2;
+  }
+
+  /*****************************************************************************
+  *  Predict Sigma Points
+  ****************************************************************************/
+  // predict sigma points
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    // extract values for better readability
+    double p_x = x_sig(0, i);
+    double p_y = x_sig(1, i);
+    double v = x_sig(2, i);
+    double yaw = x_sig(3, i);
+    double yawd = x_sig(4, i);
+
+    std::vector<double> state(5);
+    if (model_ind == MotionModel::CV)
+      cv(p_x, p_y, v, yaw, yawd, delta_t, state);
+    else if (model_ind == MotionModel::CTRV)
+      ctrv(p_x, p_y, v, yaw, yawd, delta_t, state);
+    else
+      randomMotion(p_x, p_y, v, yaw, yawd, delta_t, state);
+
+    // write predicted sigma point into right column
+    x_sig_pred(0, i) = state[0];
+    x_sig_pred(1, i) = state[1];
+    x_sig_pred(2, i) = state[2];
+    x_sig_pred(3, i) = state[3];
+    x_sig_pred(4, i) = state[4];
+  }
+
+  /*****************************************************************************
+  *  Convert Predicted Sigma Points to Mean/Covariance
+  ****************************************************************************/
+  // predicted state mean
+  x.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {  // iterate over sigma points
+    x = x + weights_s_(i) * x_sig_pred.col(i);
+  }
+
+  while (x(3) > M_PI)
+    x(3) -= 2. * M_PI;
+  while (x(3) < -M_PI)
+    x(3) += 2. * M_PI;
+  // predicted state covariance matrix
+  p.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {  // iterate over sigma points
+    // state difference
+    Eigen::VectorXd x_diff = x_sig_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_c_(i) * x_diff * x_diff.transpose();
+  }
+
+  p = p + q;
+
+  /*****************************************************************************
+  *  Update model parameters
+  ****************************************************************************/
+  if (model_ind == MotionModel::CV)
+  {
+    x_cv_.col(0) = x;
+    p_cv_ = p;
+    x_sig_pred_cv_ = x_sig_pred;
+  }
+  else if (model_ind == MotionModel::CTRV)
+  {
+    x_ctrv_.col(0) = x;
+    p_ctrv_ = p;
+    x_sig_pred_ctrv_ = x_sig_pred;
+  }
+  else
+  {
+    x_rm_.col(0) = x;
+    p_rm_ = p;
+    x_sig_pred_rm_ = x_sig_pred;
+  }
+}
+
+void UKF::updateKalmanGain(const int motion_ind)
+{
+  Eigen::VectorXd x(x_cv_.rows());
+  Eigen::MatrixXd x_sig_pred(x_sig_pred_cv_.rows(), x_sig_pred_cv_.cols());
+  Eigen::VectorXd z_pred;
+  Eigen::MatrixXd s_pred;
+  int num_meas_state = 0;
+  if (motion_ind == MotionModel::CV)
+  {
+    x = x_cv_.col(0);
+    x_sig_pred = x_sig_pred_cv_;
+    if (is_direction_cv_available_)
+    {
+      num_meas_state = num_lidar_direction_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_lidar_direction_cv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_lidar_direction_cv_;
+    }
+    else
+    {
+      num_meas_state = num_lidar_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_cv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_cv_;
+    }
+  }
+  else if (motion_ind == MotionModel::CTRV)
+  {
+    x = x_ctrv_.col(0);
+    x_sig_pred = x_sig_pred_ctrv_;
+    if (is_direction_ctrv_available_)
+    {
+      num_meas_state = num_lidar_direction_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_lidar_direction_ctrv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_lidar_direction_ctrv_;
+    }
+    else
+    {
+      num_meas_state = num_lidar_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_ctrv_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_ctrv_;
+    }
+  }
+  else
+  {
+    x = x_rm_.col(0);
+    x_sig_pred = x_sig_pred_rm_;
+    if (is_direction_rm_available_)
+    {
+      num_meas_state = num_lidar_direction_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_lidar_direction_rm_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_lidar_direction_rm_;
+    }
+    else
+    {
+      num_meas_state = num_lidar_state_;
+      z_pred = Eigen::VectorXd(num_meas_state);
+      z_pred = z_pred_rm_;
+      s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+      s_pred = s_rm_;
+    }
+  }
+
+  Eigen::MatrixXd cross_covariance = Eigen::MatrixXd(num_state_, num_meas_state);
+  cross_covariance.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    Eigen::VectorXd z_sig_point(num_meas_state);
+    if (num_meas_state == num_lidar_direction_state_)
+    {
+      z_sig_point << x_sig_pred(0, i), x_sig_pred(1, i), x_sig_pred(3, i);
+    }
+    else
+    {
+      z_sig_point << x_sig_pred(0, i), x_sig_pred(1, i);
+    }
+    Eigen::VectorXd z_diff = z_sig_point - z_pred;
+    Eigen::VectorXd x_diff = x_sig_pred.col(i) - x;
+
+    x_diff(3) = normalizeAngle(x_diff(3));
+
+    if (num_meas_state == num_lidar_direction_state_)
+    {
+      z_diff(2) = normalizeAngle(z_diff(2));
+    }
+
+    cross_covariance = cross_covariance + weights_c_(i) * x_diff * z_diff.transpose();
+  }
+
+  Eigen::MatrixXd kalman_gain = cross_covariance * s_pred.inverse();
+
+  if (num_meas_state == num_lidar_direction_state_)
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      k_lidar_direction_cv_ = kalman_gain;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      k_lidar_direction_ctrv_ = kalman_gain;
+    }
+    else
+    {
+      k_lidar_direction_rm_ = kalman_gain;
+    }
+  }
+  else
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      k_cv_ = kalman_gain;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      k_ctrv_ = kalman_gain;
+    }
+    else
+    {
+      k_rm_ = kalman_gain;
+    }
+  }
+}
+
+void UKF::predictionLidarMeasurement(const int motion_ind, const int num_meas_state)
+{
+  Eigen::MatrixXd x_sig_pred(x_sig_pred_cv_.rows(), x_sig_pred_cv_.cols());
+  Eigen::MatrixXd covariance_r(num_meas_state, num_meas_state);
+  if (motion_ind == MotionModel::CV)
+  {
+    x_sig_pred = x_sig_pred_cv_;
+    if (num_meas_state == num_lidar_direction_state_)
+      covariance_r = lidar_direction_r_cv_;
+    else
+      covariance_r = r_cv_;
+  }
+  else if (motion_ind == MotionModel::CTRV)
+  {
+    x_sig_pred = x_sig_pred_ctrv_;
+    if (num_meas_state == num_lidar_direction_state_)
+      covariance_r = lidar_direction_r_ctrv_;
+    else
+      covariance_r = r_ctrv_;
+  }
+  else
+  {
+    x_sig_pred = x_sig_pred_rm_;
+    if (num_meas_state == num_lidar_direction_state_)
+      covariance_r = lidar_direction_r_rm_;
+    else
+      covariance_r = r_rm_;
+  }
+
+  Eigen::MatrixXd z_sig = Eigen::MatrixXd(num_meas_state, 2 * num_state_ + 1);
+
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    double p_x = x_sig_pred(0, i);
+    double p_y = x_sig_pred(1, i);
+
+    z_sig(0, i) = p_x;
+    z_sig(1, i) = p_y;
+
+    if (num_meas_state == num_lidar_direction_state_)
+    {
+      double p_yaw = x_sig_pred(3, i);
+      z_sig(2, i) = p_yaw;
+    }
+  }
+
+  Eigen::VectorXd z_pred = Eigen::VectorXd(num_meas_state);
+  z_pred.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    z_pred = z_pred + weights_s_(i) * z_sig.col(i);
+  }
+
+  if (num_meas_state == num_lidar_direction_state_)
+    z_pred(2) = normalizeAngle(z_pred(2));
+
+  Eigen::MatrixXd s_pred = Eigen::MatrixXd(num_meas_state, num_meas_state);
+  s_pred.fill(0.0);
+  for (int i = 0; i < 2 * num_state_ + 1; i++)
+  {
+    Eigen::VectorXd z_diff = z_sig.col(i) - z_pred;
+    if (num_meas_state == num_lidar_direction_state_)
+      z_diff(2) = normalizeAngle(z_diff(2));
+    s_pred = s_pred + weights_c_(i) * z_diff * z_diff.transpose();
+  }
+
+  // add measurement noise covariance matrix
+  s_pred += covariance_r;
+
+  if (num_meas_state == num_lidar_direction_state_)
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      z_pred_lidar_direction_cv_ = z_pred;
+      s_lidar_direction_cv_ = s_pred;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      z_pred_lidar_direction_ctrv_ = z_pred;
+      s_lidar_direction_ctrv_ = s_pred;
+    }
+    else
+    {
+      z_pred_lidar_direction_rm_ = z_pred;
+      s_lidar_direction_rm_ = s_pred;
+    }
+  }
+  else
+  {
+    if (motion_ind == MotionModel::CV)
+    {
+      z_pred_cv_ = z_pred;
+      s_cv_ = s_pred;
+    }
+    else if (motion_ind == MotionModel::CTRV)
+    {
+      z_pred_ctrv_ = z_pred;
+      s_ctrv_ = s_pred;
+    }
+    else
+    {
+      z_pred_rm_ = z_pred;
+      s_rm_ = s_pred;
+    }
+  }
+}
+
+double UKF::calculateNIS(const iv::fusion::fusionobject& in_object, const int motion_ind)
+{
+  Eigen::VectorXd z_pred = Eigen::VectorXd(num_lidar_direction_state_);
+  Eigen::MatrixXd s_pred = Eigen::MatrixXd(num_lidar_direction_state_, num_lidar_direction_state_);
+  if (motion_ind == MotionModel::CV)
+  {
+    z_pred = z_pred_lidar_direction_cv_;
+    s_pred = s_lidar_direction_cv_;
+  }
+  else if (motion_ind == MotionModel::CTRV)
+  {
+    z_pred = z_pred_lidar_direction_ctrv_;
+    s_pred = s_lidar_direction_ctrv_;
+  }
+  else
+  {
+    z_pred = z_pred_lidar_direction_rm_;
+    s_pred = s_lidar_direction_rm_;
+  }
+
+  // Pick up yaw estimation and yaw variance
+  double diff = in_object.yaw() - z_pred(2);
+  double nis = diff * s_pred(2, 2) * diff;
+
+  return nis;
+}
+
+bool UKF::isLaneDirectionAvailable(const iv::fusion::fusionobject& in_object, const int motion_ind,
+                                   const double lane_direction_chi_thres)
+{
+  predictionLidarMeasurement(motion_ind, num_lidar_direction_state_);
+
+  double lidar_direction_nis = calculateNIS(in_object, motion_ind);
+
+  bool is_direction_available = false;
+  if (lidar_direction_nis < lane_direction_chi_thres)
+  {
+    is_direction_available = true;
+  }
+  return is_direction_available;
+}
+
+void UKF::checkLaneDirectionAvailability(const iv::fusion::fusionobject& in_object,
+                                         const double lane_direction_chi_thres, const bool use_sukf)
+{
+  if (use_sukf)
+  {
+    is_direction_ctrv_available_ = isLaneDirectionAvailable(in_object, MotionModel::CTRV, lane_direction_chi_thres);
+  }
+  else
+  {
+    is_direction_cv_available_ = isLaneDirectionAvailable(in_object, MotionModel::CV, lane_direction_chi_thres);
+    is_direction_ctrv_available_ = isLaneDirectionAvailable(in_object, MotionModel::CTRV, lane_direction_chi_thres);
+  }
+}
+
+void UKF::prediction(const bool use_sukf, const bool has_subscribed_vectormap, const double dt)
+{
+  if (use_sukf)
+  {
+    predictionSUKF(dt, has_subscribed_vectormap);
+  }
+  else
+  {
+    predictionIMMUKF(dt, has_subscribed_vectormap);
+  }
+}
+
+void UKF::update(const bool use_sukf, const double detection_probability, const double gate_probability,
+                 const double gating_thres, const iv::fusion::fusionobjectarray & object_vec)
+{
+  if (use_sukf)
+  {
+    updateSUKF(object_vec);
+  }
+  else
+  {
+    updateIMMUKF(detection_probability, gate_probability, gating_thres, object_vec);
+  }
+}

src/fusion/lidar_radar_fusion_cnn_ukf/ukf.h

@@ -0,0 +1,313 @@
+/*
+ * Copyright 2018-2019 Autoware Foundation. All rights reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef OBJECT_TRACKING_UKF_H
+#define OBJECT_TRACKING_UKF_H
+
+#include "Eigen/Dense"
+//#include <ros/ros.h>
+#include <vector>
+#include <string>
+#include <fstream>
+#include <pcl/point_types.h>
+#include <pcl/point_cloud.h>
+
+
+
+#include "fusionobject.pb.h"
+#include "fusionobjectarray.pb.h"
+
+enum TrackingState : int
+{
+  Die = 0,     // No longer tracking
+  Init = 1,    // Start tracking
+  Stable = 4,  // Stable tracking
+  Occlusion = 5, // Lost 1 frame possibly by occlusion
+  Lost = 10,   // About to lose target
+};
+
+enum MotionModel : int
+{
+  CV = 0,    // constant velocity
+  CTRV = 1,  // constant turn rate and velocity
+  RM = 2,    // random motion
+};
+
+class UKF
+{
+  /*
+  cv: Constant Velocity
+  ctrv: Constatnt Turn Rate and Velocity
+  rm: Random Motion
+  */
+
+public:
+  int ukf_id_;
+
+  int num_state_;
+
+  int num_lidar_state_;
+
+  int num_lidar_direction_state_;
+
+  int num_motion_model_;
+
+  //* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
+  Eigen::MatrixXd x_merge_;
+
+  //* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
+  Eigen::MatrixXd x_cv_;
+
+  //* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
+  Eigen::MatrixXd x_ctrv_;
+
+  //* state vector: [pos1 pos2 vel_abs yaw_angle yaw_rate] in SI units and rad
+  Eigen::MatrixXd x_rm_;
+
+  //* state covariance matrix
+  Eigen::MatrixXd p_merge_;
+
+  //* state covariance matrix
+  Eigen::MatrixXd p_cv_;
+
+  //* state covariance matrix
+  Eigen::MatrixXd p_ctrv_;
+
+  //* state covariance matrix
+  Eigen::MatrixXd p_rm_;
+
+  //* predicted sigma points matrix
+  Eigen::MatrixXd x_sig_pred_cv_;
+
+  //* predicted sigma points matrix
+  Eigen::MatrixXd x_sig_pred_ctrv_;
+
+  //* predicted sigma points matrix
+  Eigen::MatrixXd x_sig_pred_rm_;
+
+  //* time when the state is true, in us
+  long long time_;
+
+  //* Process noise standard deviation longitudinal acceleration in m/s^2
+  double std_a_cv_;
+  double std_a_ctrv_;
+  double std_a_rm_;
+
+  // CTRV
+  double std_ctrv_yawdd_;
+  // CV
+  double std_cv_yawdd_;
+
+  double std_rm_yawdd_;
+
+  //* Laser measurement noise standard deviation position1 in m
+  double std_laspx_;
+
+  //* Laser measurement noise standard deviation position2 in m
+  double std_laspy_;
+
+  //* Weights of sigma points
+  Eigen::VectorXd weights_c_;
+  Eigen::VectorXd weights_s_;
+
+  //* Sigma point spreading parameter
+  double lambda_;
+
+  double mode_match_prob_cv2cv_;
+  double mode_match_prob_ctrv2cv_;
+  double mode_match_prob_rm2cv_;
+
+  double mode_match_prob_cv2ctrv_;
+  double mode_match_prob_ctrv2ctrv_;
+  double mode_match_prob_rm2ctrv_;
+
+  double mode_match_prob_cv2rm_;
+  double mode_match_prob_ctrv2rm_;
+  double mode_match_prob_rm2rm_;
+
+  double mode_match_prob_cv_;
+
+  double mode_match_prob_ctrv_;
+
+  double mode_match_prob_rm_;
+
+  double mode_prob_cv_;
+  double mode_prob_ctrv_;
+  double mode_prob_rm_;
+
+  std::vector<double> p1_;
+
+  std::vector<double> p2_;
+
+  std::vector<double> p3_;
+
+  Eigen::VectorXd z_pred_cv_;
+  Eigen::VectorXd z_pred_ctrv_;
+  Eigen::VectorXd z_pred_rm_;
+
+  Eigen::MatrixXd s_cv_;
+  Eigen::MatrixXd s_ctrv_;
+  Eigen::MatrixXd s_rm_;
+
+  Eigen::MatrixXd k_cv_;
+  Eigen::MatrixXd k_ctrv_;
+  Eigen::MatrixXd k_rm_;
+
+  double pd_;
+  double pg_;
+
+  int lifetime_;
+  bool is_static_;
+
+  // object msg information
+  bool is_stable_;
+  iv::fusion::fusionobject object_;
+  std::string label_;
+  double min_assiciation_distance_;
+
+  // for env classification
+  Eigen::VectorXd init_meas_;
+  std::vector<double> vel_history_;
+
+  double x_merge_yaw_;
+
+  int tracking_num_;
+
+  Eigen::VectorXd cv_meas_;
+  Eigen::VectorXd ctrv_meas_;
+  Eigen::VectorXd rm_meas_;
+
+  Eigen::MatrixXd q_cv_;
+  Eigen::MatrixXd q_ctrv_;
+  Eigen::MatrixXd q_rm_;
+
+  Eigen::MatrixXd r_cv_;
+  Eigen::MatrixXd r_ctrv_;
+  Eigen::MatrixXd r_rm_;
+
+  double nis_cv_;
+  double nis_ctrv_;
+  double nis_rm_;
+
+  Eigen::MatrixXd new_x_sig_cv_;
+  Eigen::MatrixXd new_x_sig_ctrv_;
+  Eigen::MatrixXd new_x_sig_rm_;
+
+  Eigen::MatrixXd new_z_sig_cv_;
+  Eigen::MatrixXd new_z_sig_ctrv_;
+  Eigen::MatrixXd new_z_sig_rm_;
+
+  Eigen::VectorXd new_z_pred_cv_;
+  Eigen::VectorXd new_z_pred_ctrv_;
+  Eigen::VectorXd new_z_pred_rm_;
+
+  Eigen::MatrixXd new_s_cv_;
+  Eigen::MatrixXd new_s_ctrv_;
+  Eigen::MatrixXd new_s_rm_;
+
+  // for lane direction combined filter
+  bool is_direction_cv_available_;
+  bool is_direction_ctrv_available_;
+  bool is_direction_rm_available_;
+  double std_lane_direction_;
+  Eigen::MatrixXd lidar_direction_r_cv_;
+  Eigen::MatrixXd lidar_direction_r_ctrv_;
+  Eigen::MatrixXd lidar_direction_r_rm_;
+
+  Eigen::VectorXd z_pred_lidar_direction_cv_;
+  Eigen::VectorXd z_pred_lidar_direction_ctrv_;
+  Eigen::VectorXd z_pred_lidar_direction_rm_;
+
+  Eigen::MatrixXd s_lidar_direction_cv_;
+  Eigen::MatrixXd s_lidar_direction_ctrv_;
+  Eigen::MatrixXd s_lidar_direction_rm_;
+
+  Eigen::MatrixXd k_lidar_direction_cv_;
+  Eigen::MatrixXd k_lidar_direction_ctrv_;
+  Eigen::MatrixXd k_lidar_direction_rm_;
+
+  Eigen::VectorXd lidar_direction_ctrv_meas_;
+
+  /**
+   * Constructor
+   */
+  UKF();
+
+  void updateYawWithHighProb();
+
+  void initialize(const Eigen::VectorXd& z, const double timestamp, const int target_ind);
+
+  void updateModeProb(const std::vector<double>& lambda_vec);
+
+  void mergeEstimationAndCovariance();
+
+  void mixingProbability();
+
+  void interaction();
+
+  void predictionSUKF(const double dt, const bool has_subscribed_vectormap);
+
+  void predictionIMMUKF(const double dt, const bool has_subscribed_vectormap);
+
+  void findMaxZandS(Eigen::VectorXd& max_det_z, Eigen::MatrixXd& max_det_s);
+
+  void updateMeasurementForCTRV(const iv::fusion::fusionobjectarray& object_vec);
+
+  void uppateForCTRV();
+
+  void updateEachMotion(const double detection_probability, const double gate_probability, const double gating_thres,
+                        const iv::fusion::fusionobjectarray& object_vec, std::vector<double>& lambda_vec);
+
+  void updateSUKF(const iv::fusion::fusionobjectarray& object_vec);
+
+  void updateIMMUKF(const double detection_probability, const double gate_probability, const double gating_thres,
+                    const iv::fusion::fusionobjectarray& object_vec);
+
+  void ctrv(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+            const double delta_t, std::vector<double>& state);
+
+  void cv(const double p_x, const double p_y, const double v, const double yaw, const double yawd, const double delta_t,
+          std::vector<double>& state);
+
+  void randomMotion(const double p_x, const double p_y, const double v, const double yaw, const double yawd,
+                    const double delta_t, std::vector<double>& state);
+
+  void initCovarQs(const double dt, const double yaw);
+
+  void predictionMotion(const double delta_t, const int model_ind);
+
+  void checkLaneDirectionAvailability(const iv::fusion::fusionobject& in_object,
+                                      const double lane_direction_chi_thres, const bool use_sukf);
+
+  void predictionLidarMeasurement(const int motion_ind, const int num_meas_state);
+
+  double calculateNIS(const iv::fusion::fusionobject& in_object, const int motion_ind);
+
+  bool isLaneDirectionAvailable(const iv::fusion::fusionobject& in_object, const int motion_ind,
+                                const double lane_direction_chi_thres);
+
+  // void updateKalmanGain(const int motion_ind, const int num_meas_state);
+  void updateKalmanGain(const int motion_ind);
+
+  double normalizeAngle(const double angle);
+
+  void update(const bool use_sukf, const double detection_probability, const double gate_probability,
+              const double gating_thres, const iv::fusion::fusionobjectarray& object_vec);
+
+  void prediction(const bool use_sukf, const bool has_subscribed_vectormap, const double dt);
+};
+
+#endif /* UKF_H */