From 7769bef0db058a15d9087f1f86fdbff26bbcfd16 Mon Sep 17 00:00:00 2001 From: kechxu Date: Mon, 17 Jul 2017 16:18:29 -0700 Subject: [PATCH] kalman filter for lane in obstacle.cc --- .../prediction/common/prediction_gflags.cc | 4 + modules/prediction/common/prediction_gflags.h | 2 + .../container/obstacles/obstacle.cc | 133 +++++++++++++++++- .../prediction/container/obstacles/obstacle.h | 10 ++ 4 files changed, 147 insertions(+), 2 deletions(-) diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index 0817c9e3a4..a2c31f0517 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -29,3 +29,7 @@ DEFINE_double(min_acc, -4.0, "Lower bound of deceleration"); DEFINE_double(q_var, 0.01, "Processing noise covariance"); DEFINE_double(r_var, 0.25, "Measurement noise covariance"); DEFINE_double(p_var, 0.1, "Error covariance"); +DEFINE_double(go_approach_rate, 0.995, + "The rate to approach to the reference line of going straight"); +DEFINE_double(cutin_approach_rate, 0.9, + "The rate to approach to the reference line of lane change"); \ No newline at end of file diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index da6e8336f8..105d41bf84 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -29,5 +29,7 @@ DECLARE_double(min_acc); DECLARE_double(q_var); DECLARE_double(r_var); DECLARE_double(p_var); +DECLARE_double(go_approach_rate); +DECLARE_double(cutin_approach_rate); #endif // MODULES_PREDICTION_COMMON_PREDICTION_GFLAGS_H_ diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc index 79e87fee8e..f0822e9411 100644 --- a/modules/prediction/container/obstacles/obstacle.cc +++ b/modules/prediction/container/obstacles/obstacle.cc @@ -18,6 +18,7 @@ #include #include +#include #include "modules/common/log.h" #include "modules/common/math/math_utils.h" @@ -48,7 +49,6 @@ Obstacle::Obstacle() : kf_motion_tracker_(), kf_motion_tracker_enabled_(false), kf_lane_trackers_(0) { - } Obstacle::~Obstacle() { @@ -429,7 +429,7 @@ void Obstacle::UpdateMotionBelief(Feature* feature) { } void Obstacle::InitKFLaneTracker(const std::string& lane_id, - const double beta) { + const double beta) { KalmanFilter kf; // transition matrix: update delta_t at each processing step @@ -465,5 +465,134 @@ void Obstacle::InitKFLaneTracker(const std::string& lane_id, kf_lane_trackers_.emplace(lane_id, std::move(kf)); } +void Obstacle::UpdateKFLaneTrackers(Feature* feature) { + if (!feature->has_lane()) { + return; + } + std::unordered_set lane_ids; + for (auto& lane_feature : feature->lane().current_lane_feature()) { + if (!lane_feature.lane_id().empty()) { + lane_ids.insert(lane_feature.lane_id()); + } + } + for (auto& lane_feature : feature->lane().nearby_lane_feature()) { + if (!lane_feature.lane_id().empty()) { + lane_ids.insert(lane_feature.lane_id()); + } + } + if (lane_ids.empty()) { + return; + } + + auto iter = kf_lane_trackers_.begin(); + while (iter != kf_lane_trackers_.end()) { + if (lane_ids.find(iter->first) == lane_ids.end()) { + iter = kf_lane_trackers_.erase(iter); + } else { + ++iter; + } + } + + double ts = feature->timestamp(); + double speed = feature->timestamp(); + double acc = feature->acc(); + for (auto& lane_feature : feature->lane().current_lane_feature()) { + std::string lane_id = lane_feature.lane_id(); + if (lane_id.empty()) { + continue; + } + double s = lane_feature.lane_s(); + double l = lane_feature.lane_l(); + UpdateKFLaneTracker(lane_id, s, l, speed, acc, ts, + FLAGS_go_approach_rate); + } + for (auto& nearby_lane_feature : feature->lane().nearby_lane_feature()) { + std::string lane_id = nearby_lane_feature.lane_id(); + if (lane_id.empty()) { + continue; + } + double s = nearby_lane_feature.lane_s(); + double l = nearby_lane_feature.lane_l(); + UpdateKFLaneTracker(lane_id, s, l, speed, acc, ts, + FLAGS_cutin_approach_rate); + } + + UpdateLaneBelief(feature); +} + +void Obstacle::UpdateKFLaneTracker(const std::string& lane_id, + const double lane_s, const double lane_l, + const double lane_speed, + const double lane_acc, + const double timestamp, const double beta) { + KalmanFilter* kf_ptr = nullptr; + if (kf_lane_trackers_.find(lane_id) != kf_lane_trackers_.end()) { + kf_ptr = &kf_lane_trackers_[lane_id]; + if (kf_ptr != nullptr) { + double delta_ts = 0.0; + if (feature_history_.size() > 0) { + delta_ts = timestamp - feature_history_.front().timestamp(); + } + if (delta_ts > FLAGS_double_precision) { + auto F = kf_ptr->GetTransitionMatrix(); + F(0, 2) = delta_ts; + F(0, 3) = 0.5 * delta_ts * delta_ts; + F(2, 3) = delta_ts; + kf_ptr->SetTransitionMatrix(F); + kf_ptr->Predict(); + + Eigen::Matrix z; + z(0, 0) = lane_s; + z(1, 0) = lane_l; + kf_ptr->Correct(z); + } + } else { + kf_lane_trackers_.erase(lane_id); + } + } + + if (kf_lane_trackers_.find(lane_id) == kf_lane_trackers_.end()) { + InitKFLaneTracker(lane_id, beta); + kf_ptr = &kf_lane_trackers_[lane_id]; + if (kf_ptr != nullptr) { + Eigen::Matrix state; + state(0, 0) = lane_s; + state(1, 0) = lane_l; + state(2, 0) = lane_speed; + state(3, 0) = lane_acc; + + auto P = kf_ptr->GetStateCovariance(); + kf_ptr->SetStateEstimate(state, P); + } + } +} + +void Obstacle::UpdateLaneBelief(Feature* feature) { + if ((!feature->has_lane()) || (!feature->lane().has_lane_feature())) { + return; + } + const std::string& lane_id = feature->lane().lane_feature().lane_id(); + if (lane_id.empty()) { + return; + } + + KalmanFilter* kf_ptr = nullptr; + if (kf_lane_trackers_.find(lane_id) != kf_lane_trackers_.end()) { + kf_ptr = &kf_lane_trackers_[lane_id]; + } + if (kf_ptr == nullptr) { + return; + } + + double lane_speed = kf_ptr->GetStateEstimate()(2, 0); + double lane_acc = apollo::common::math::Clamp( + kf_ptr->GetStateEstimate()(2, 0), FLAGS_min_acc, FLAGS_max_acc); + feature->set_t_speed(lane_speed); + feature->set_t_acc(lane_acc); + + ADEBUG << "Obstacle [" << id_ << "] set tracked speed [" << lane_speed + << " and tracked acc [" << lane_acc << "]"; +} + } // namespace prediction } // namespace apollo diff --git a/modules/prediction/container/obstacles/obstacle.h b/modules/prediction/container/obstacles/obstacle.h index 3346f00d7a..753162d274 100644 --- a/modules/prediction/container/obstacles/obstacle.h +++ b/modules/prediction/container/obstacles/obstacle.h @@ -105,6 +105,16 @@ class Obstacle { void InitKFLaneTracker(const std::string& lane_id, const double beta); + void UpdateKFLaneTrackers(Feature* feature); + + void UpdateKFLaneTracker( + const std::string& lane_id, + const double lane_s, const double lane_l, + const double lane_speed, const double lane_acc, + const double timestamp, const double beta); + + void UpdateLaneBelief(Feature* feature); + private: int id_; apollo::perception::PerceptionObstacle::Type type_; -- GitLab