Common: added KF status check

modules/common/math/BUILD

@@ -167,7 +167,6 @@ cc_library(
     deps = [
         "//modules/common:log",
         "//modules/common/math:matrix_operations",
-        "//modules/common/util:string_util",
         "@eigen//:eigen",
     ],
 )

modules/common/math/kalman_filter.h

@@ -22,13 +22,13 @@
 #ifndef MODULES_COMMON_MATH_KALMAN_FILTER_H_
 #define MODULES_COMMON_MATH_KALMAN_FILTER_H_
 
+#include <sstream>
 #include <string>
 
 #include "Eigen/Dense"
 
 #include "modules/common/log.h"
 #include "modules/common/math/matrix_operations.h"
-#include "modules/common/util/string_util.h"
 
 /**
  * @namespace apollo::common::math
@@ -205,6 +205,12 @@ class KalmanFilter {
    */
   std::string DebugString() const;
 
+  /**
+   * @brief Get initialization state of the filter
+   * @return True if the filter is initialized
+   */
+  bool IsInitialized() { return is_initialized_; }
+
  private:
   // Mean of current state belief distribution
   Eigen::Matrix<T, XN, 1> x_;
@@ -266,14 +272,15 @@ inline void KalmanFilter<T, XN, ZN, UN>::Correct(
 template <typename T, unsigned int XN, unsigned int ZN, unsigned int UN>
 inline std::string KalmanFilter<T, XN, ZN, UN>::DebugString() const {
   Eigen::IOFormat clean_fmt(4, 0, ", ", " ", "[", "]");
-  return util::StrCat(
-      "F = ", F_.format(clean_fmt), "\n"
-      "B = ", B_.format(clean_fmt), "\n"
-      "H = ", H_.format(clean_fmt), "\n"
-      "Q = ", Q_.format(clean_fmt), "\n"
-      "R = ", R_.format(clean_fmt), "\n"
-      "x = ", x_.format(clean_fmt), "\n"
-      "P = ", P_.format(clean_fmt), "\n");
+  std::stringstream ss;
+  ss << "F = " << F_.format(clean_fmt) << "\n"
+     << "B = " << B_.format(clean_fmt) << "\n"
+     << "H = " << H_.format(clean_fmt) << "\n"
+     << "Q = " << Q_.format(clean_fmt) << "\n"
+     << "R = " << R_.format(clean_fmt) << "\n"
+     << "x = " << x_.format(clean_fmt) << "\n"
+     << "P = " << P_.format(clean_fmt) << "\n";
+  return ss.str();
 }
 
 }  // namespace math

modules/prediction/container/obstacles/obstacle.cc

@@ -165,8 +165,8 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
   SetVelocity(perception_obstacle, &feature);
   SetAcceleration(&feature);
   SetTheta(perception_obstacle, &feature);
-  if (!kf_motion_tracker_enabled_) {
-    InitKFMotionTracker(&feature);
+  if (!kf_motion_tracker_.IsInitialized()) {
+    InitKFMotionTracker(feature);
   }
   UpdateKFMotionTracker(&feature);
   SetCurrentLanes(&feature);
@@ -174,8 +174,8 @@ void Obstacle::Insert(const PerceptionObstacle& perception_obstacle,
   SetLaneGraphFeature(&feature);
   UpdateKFLaneTrackers(&feature);
   if (type_ == PerceptionObstacle::PEDESTRIAN) {
-    if (!kf_pedestrian_tracker_enabled_) {
-      InitKFPedestrianTracker(&feature);
+    if (!kf_pedestrian_tracker_.IsInitialized()) {
+      InitKFPedestrianTracker(feature);
     }
     UpdateKFPedestrianTracker(&feature);
   }
@@ -420,7 +420,7 @@ void Obstacle::SetLengthWidthHeight(
          << std::setprecision(6) << height << "].";
 }
 
-void Obstacle::InitKFMotionTracker(Feature* feature) {
+void Obstacle::InitKFMotionTracker(const Feature& feature) {
   double t = FLAGS_prediction_freq;
   // Set transition matrix F
   // constant acceleration dynamic model
@@ -475,16 +475,14 @@ void Obstacle::InitKFMotionTracker(Feature* feature) {
 
   // Set initial state
   Eigen::Matrix<double, 6, 1> x;
-  x(0, 0) = feature->position().x();
-  x(1, 0) = feature->position().y();
-  x(2, 0) = feature->velocity().x();
-  x(3, 0) = feature->velocity().y();
-  x(4, 0) = feature->acceleration().x();
-  x(5, 0) = feature->acceleration().y();
+  x(0, 0) = feature.position().x();
+  x(1, 0) = feature.position().y();
+  x(2, 0) = feature.velocity().x();
+  x(3, 0) = feature.velocity().y();
+  x(4, 0) = feature.acceleration().x();
+  x(5, 0) = feature.acceleration().y();
 
   kf_motion_tracker_.SetStateEstimate(x, P);
-
-  kf_motion_tracker_enabled_ = true;
 }
 
 void Obstacle::UpdateKFMotionTracker(Feature* feature) {
@@ -704,7 +702,7 @@ void Obstacle::UpdateLaneBelief(Feature* feature) {
          << std::fixed << std::setprecision(6) << lane_acc << "]";
 }
 
-void Obstacle::InitKFPedestrianTracker(Feature* feature) {
+void Obstacle::InitKFPedestrianTracker(const Feature& feature) {
   // Set transition matrix F
   Eigen::Matrix<double, 2, 2> F;
   F.setIdentity();
@@ -740,12 +738,10 @@ void Obstacle::InitKFPedestrianTracker(Feature* feature) {
   // Set initial state
   Eigen::Matrix<double, 2, 1> x;
   x.setZero();
-  x(0, 0) = feature->position().x();
-  x(1, 0) = feature->position().y();
+  x(0, 0) = feature.position().x();
+  x(1, 0) = feature.position().y();
 
   kf_pedestrian_tracker_.SetStateEstimate(x, P);
-
-  kf_pedestrian_tracker_enabled_ = true;
 }
 
 void Obstacle::UpdateKFPedestrianTracker(Feature* feature) {

modules/prediction/container/obstacles/obstacle.h

@@ -182,7 +182,7 @@ class Obstacle {
       const perception::PerceptionObstacle& perception_obstacle,
       Feature* feature);
 
-  void InitKFMotionTracker(Feature* feature);
+  void InitKFMotionTracker(const Feature& feature);
 
   void UpdateKFMotionTracker(Feature* feature);
 
@@ -207,7 +207,7 @@ class Obstacle {
 
   void SetLanePoints(Feature* feature);
 
-  void InitKFPedestrianTracker(Feature* feature);
+  void InitKFPedestrianTracker(const Feature& feature);
 
   void UpdateKFPedestrianTracker(Feature* feature);
 
@@ -224,8 +224,6 @@ class Obstacle {
   std::deque<Feature> feature_history_;
   common::math::KalmanFilter<double, 6, 2, 0> kf_motion_tracker_;
   common::math::KalmanFilter<double, 2, 2, 4> kf_pedestrian_tracker_;
-  bool kf_motion_tracker_enabled_ = false;
-  bool kf_pedestrian_tracker_enabled_ = false;
   std::unordered_map<std::string, common::math::KalmanFilter<double, 4, 2, 0>>
       kf_lane_trackers_;
   std::vector<std::shared_ptr<const hdmap::LaneInfo>> current_lanes_;