Method update

tracking/src/track.cpp:142–250

    }

    void
    Track::update(
        double x,
        double y,
        double z,
        double height,
        double distance,
        double data_assocation_score,
        double confidence,
        double min_confidence,
        double min_confidence_detections,
        open_ptrack::detection::DetectionSource* detection_source,
        bool first_update)
    {
      //Update Kalman filter
      int difference;
      double vx, vy;
      if (velocity_in_motion_term_)
      {
        ros::Duration d(1.0);
        ros::Duration d2(2.0);

        double t = std::max(first_time_detected_.toSec(), (detection_source->getTime() - d).toSec());
//        std::cout << "t1: " << t << std::endl;
        t = std::min(t, last_time_detected_.toSec());
//        std::cout << "t2: " << t << std::endl;
        t = std::max(t, (detection_source->getTime() - d2).toSec());
//        std::cout << "t3: " << t << std::endl;
        double dt = t - last_time_predicted_.toSec();
//        std::cout << "dt: " << dt << std::endl;
        difference = int(round(dt / period_));
//        std::cout << "period: " << period_ << std::endl;
//        std::cout << "difference: " << difference << std::endl;
        int vIndex = (MAX_SIZE + last_time_predicted_index_ + difference) % MAX_SIZE;
//        std::cout << vIndex << std::endl;

//        difference = int(round(dt / period_));
//        int vIndex = (MAX_SIZE + last_time_predicted_index_ + difference) % MAX_SIZE;

        if(difference != 0)
        {
          vx = - (x - mahalanobis_map4d_[vIndex].x) / dt;
          vy = - (y - mahalanobis_map4d_[vIndex].y) / dt;
        }
        else
        {
          vx = mahalanobis_map4d_[vIndex].x;
          vy = mahalanobis_map4d_[vIndex].x;
        }
//        std::cout << "Mahalanobis: " << mahalanobis_map4d_[vIndex].x << "," << mahalanobis_map4d_[vIndex].y << std::endl;
      }

      // Update Kalman filter from the last time the track was visible:
      int framesLost = int(round((detection_source->getTime() - last_time_detected_).toSec() / period_)) - 1;

      for(int i = 0; i < framesLost; i++)
      {
        filter_->predict();