Control: Update filter for test

modules/control/controller/lat_controller.cc

@@ -375,62 +375,6 @@ Status LatController::Reset() {
   return Status::OK();
 }
 
-// state = [Lateral Error, Lateral Error Rate, Heading Error, Heading Error
-// Rate, Preview Lateral1, Preview Lateral2, ...]
-void LatController::UpdateState(SimpleLateralDebug *debug) {
-  TrajectoryPoint traj_point;
-  const auto &position = VehicleState::instance()->ComputeCOMPosition(lr_);
-  double raw_lateral_error = GetLateralError(position, &traj_point);
-
-  // lateral_error_ = lateral_rate_filter_.Filter(raw_lateral_error);
-  debug->set_lateral_error(lateral_error_filter_.Update(raw_lateral_error));
-
-  // ref_curvature_ = traj_point.kappa();
-  debug->set_curvature(traj_point.path_point().kappa());
-
-  // ref_heading_ = traj_point.theta;
-  debug->set_ref_heading(traj_point.path_point().theta());
-
-  // heading_error_ =
-  //    common::math::NormalizeAngle(VehicleState::instance()->heading() -
-  //    ref_heading_);
-  debug->set_heading_error(common::math::NormalizeAngle(
-      VehicleState::instance()->heading() - traj_point.path_point().theta()));
-
-  // Reverse heading error if vehicle is going in reverse
-  if (VehicleState::instance()->gear() == canbus::Chassis::GEAR_REVERSE) {
-    debug->set_heading_error(-debug->heading_error());
-  }
-
-  // heading_error_rate_ = (heading_error_ - previous_heading_error_) / ts_;
-  debug->set_heading_error_rate(
-      (debug->heading_error() - previous_heading_error_) / ts_);
-  // lateral_error_rate_ = (lateral_error_ - previous_lateral_error_) / ts_;
-  debug->set_lateral_error_rate(
-      (debug->lateral_error() - previous_lateral_error_) / ts_);
-
-  // Prepare for next iteration.
-  previous_heading_error_ = debug->heading_error();
-  previous_lateral_error_ = debug->lateral_error();
-
-  // State matrix update;
-  // First four elements are fixed;
-  matrix_state_(0, 0) = debug->lateral_error();
-  matrix_state_(1, 0) = debug->lateral_error_rate();
-  matrix_state_(2, 0) = debug->heading_error();
-  matrix_state_(3, 0) = debug->heading_error_rate();
-
-  // Next elements are depending on preview window size;
-  for (int i = 0; i < preview_window_; ++i) {
-    double preview_time = ts_ * (i + 1);
-    const auto &future_position_estimate =
-        VehicleState::instance()->EstimateFuturePosition(preview_time);
-    double preview_lateral = GetLateralError(future_position_estimate, nullptr);
-    matrix_state_(basic_state_size_ + i, 0) = preview_lateral;
-  }
-  // preview matrix update;
-}
-
 void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug) {
   if (FLAGS_use_relative_position) {
     ComputeLateralErrors(0.0, 0.0, VehicleState::instance()->heading(),
@@ -542,7 +486,7 @@ double LatController::GetLateralError(const common::math::Vec2d &point,
 void LatController::ComputeLateralErrors(
     const double x, const double y, const double theta, const double linear_v,
     const double angular_v, const TrajectoryAnalyzer &trajectory_analyzer,
-    SimpleLateralDebug *debug) const {
+    SimpleLateralDebug *debug) {
   // TODO(QiL): change this to conf.
   ::apollo::common::TrajectoryPoint target_point;
   if (FLAGS_use_relative_position) {
@@ -562,8 +506,17 @@ void LatController::ComputeLateralErrors(
   double cos_matched_theta = std::cos(target_point.path_point().theta());
   double sin_matched_theta = std::sin(target_point.path_point().theta());
   // d_error = cos_matched_theta * dy - sin_matched_theta * dx;
-  debug->set_lateral_error(cos_matched_theta * dy - sin_matched_theta * dx);
+  // lateral_error_ = lateral_rate_filter_.Filter(raw_lateral_error);
 
+  // TODO(QiL): Code reformat when done with test
+  double raw_lateral_error = cos_matched_theta * dy - sin_matched_theta * dx;
+  if (FLAGS_use_relative_position) {
+    double filtered_lateral_error =
+        lateral_error_filter_.Update(raw_lateral_error);
+    debug->set_lateral_error(filtered_lateral_error);
+  } else {
+    debug->set_lateral_error(raw_lateral_error);
+  }
   double delta_theta =
       common::math::NormalizeAngle(theta - target_point.path_point().theta());
   double sin_delta_theta = std::sin(delta_theta);
@@ -577,7 +530,7 @@ void LatController::ComputeLateralErrors(
   debug->set_heading_error_rate(angular_v - target_point.path_point().kappa() *
                                                 target_point.v());
 
-  // matched_theta = target_point.path_point().theta();
+  // matched_theta = 3.path_point().theta();
   debug->set_ref_heading(target_point.path_point().theta());
   // matched_kappa = target_point.path_point().kappa();
   debug->set_curvature(target_point.path_point().kappa());

modules/control/controller/lat_controller.h

@@ -101,8 +101,6 @@ class LatController : public Controller {
   std::string Name() const override;
 
  protected:
-  void UpdateState(SimpleLateralDebug *debug);
-
   void UpdateStateAnalyticalMatching(SimpleLateralDebug *debug);
 
   void UpdateMatrix();
@@ -118,7 +116,7 @@ class LatController : public Controller {
   void ComputeLateralErrors(const double x, const double y, const double theta,
                             const double linear_v, const double angular_v,
                             const TrajectoryAnalyzer &trajectory_analyzer,
-                            SimpleLateralDebug *debug) const;
+                            SimpleLateralDebug *debug);
   bool LoadControlConf(const ControlConf *control_conf);
   void InitializeFilters(const ControlConf *control_conf);
   void LoadLatGainScheduler(const LatControllerConf &lat_controller_conf);

modules/control/controller/lat_controller_test.cc

@@ -57,7 +57,7 @@ class LatControllerTest : public ::testing::Test, LatController {
   void ComputeLateralErrors(const double x, const double y, const double theta,
                             const double linear_v, const double angular_v,
                             const TrajectoryAnalyzer &trajectory_analyzer,
-                            SimpleLateralDebug *debug) const {
+                            SimpleLateralDebug *debug) {
     LatController::ComputeLateralErrors(x, y, theta, linear_v, angular_v,
                                         trajectory_analyzer, debug);
   }