Revert "Control: Use dynamic time interval"

This reverts commit f9d8e733.

modules/control/conf/lincoln.pb.txt

@@ -13,7 +13,7 @@ max_steering_percentage_allowed: 100
 minimum_speed_resolution: 0.2
 lat_controller_conf {
   ts: 0.01
-  preview_time: 0.0
+  preview_window: 0
   cf: 155494.663
   cr: 155494.663
   mass_fl: 520
@@ -80,7 +80,7 @@ lon_controller_conf {
   throttle_deadzone: 18.0
   speed_controller_input_limit: 2.0
   station_error_limit: 2.0
-  preview_time: 0.20
+  preview_window: 20.0
   standstill_acceleration: -3.0
   station_pid_conf {
     integrator_enable: false

modules/control/controller/BUILD

@@ -33,6 +33,7 @@ cc_library(
         "//modules/control/common:interpolation_1d",
         "//modules/control/common:trajectory_analyzer",
         "//modules/control/filters:digital_filter",
+        "//modules/control/filters:digital_filter_coefficients",
         "//modules/control/filters:mean_filter",
         "//modules/control/proto:control_proto",
         "@eigen//:eigen",
@@ -58,6 +59,7 @@ cc_library(
         "//modules/control/common:pid_controller",
         "//modules/control/common:trajectory_analyzer",
         "//modules/control/filters:digital_filter",
+        "//modules/control/filters:digital_filter_coefficients",
         "//modules/localization/common:localization_common",
     ],
 )

modules/control/controller/lat_controller.cc

@@ -100,7 +100,7 @@ bool LatController::LoadControlConf(const ControlConf *control_conf) {
   CHECK_GT(ts_, 0.0) << "[LatController] Invalid control update interval.";
   cf_ = control_conf->lat_controller_conf().cf();
   cr_ = control_conf->lat_controller_conf().cr();
-  preview_time_ = control_conf->lat_controller_conf().preview_time();
+  preview_window_ = control_conf->lat_controller_conf().preview_window();
   wheelbase_ = vehicle_param_.wheel_base();
   steer_transmission_ratio_ = vehicle_param_.steer_ratio();
   steer_single_direction_max_degree_ =
@@ -156,8 +156,11 @@ void LatController::LogInitParameters() {
 
 void LatController::InitializeFilters(const ControlConf *control_conf) {
   // Low pass filter
-  digital_filter_.set_coefficients(ts_,
-                            control_conf->lat_controller_conf().cutoff_freq());
+  std::vector<double> den(3, 0.0);
+  std::vector<double> num(3, 0.0);
+  LpfCoefficients(ts_, control_conf->lat_controller_conf().cutoff_freq(), &den,
+                  &num);
+  digital_filter_.set_coefficients(den, num);
   // Mean filters
   /**
   heading_rate_filter_ = MeanFilter(
@@ -174,7 +177,7 @@ Status LatController::Init(const ControlConf *control_conf) {
                   "failed to load control_conf");
   }
   // Matrix init operations.
-  int matrix_size = basic_state_size_ + std::floor(preview_time_/ts_);
+  int matrix_size = basic_state_size_ + preview_window_;
   matrix_a_ = Matrix::Zero(basic_state_size_, basic_state_size_);
   matrix_ad_ = Matrix::Zero(basic_state_size_, basic_state_size_);
   matrix_adc_ = Matrix::Zero(matrix_size, matrix_size);
@@ -420,8 +423,7 @@ void LatController::UpdateState(SimpleLateralDebug *debug) {
   matrix_state_(3, 0) = debug->heading_error_rate();
 
   // Next elements are depending on preview window size;
-  int preview_window = std::floor(preview_time_ / ts_);
-  for (int i = 0; i < preview_window; ++i) {
+  for (int i = 0; i < preview_window_; ++i) {
     double preview_time = ts_ * (i + 1);
     const auto &future_position_estimate =
         VehicleState::instance()->EstimateFuturePosition(preview_time);
@@ -452,8 +454,7 @@ void LatController::UpdateStateAnalyticalMatching(SimpleLateralDebug *debug) {
   matrix_state_(3, 0) = debug->heading_error_rate();
 
   // Next elements are depending on preview window size;
-  int preview_window = std::floor(preview_time_ / ts_);
-  for (int i = 0; i < preview_window; ++i) {
+  for (int i = 0; i < preview_window_; ++i) {
     double preview_time = ts_ * (i + 1);
     auto preview_point =
         trajectory_analyzer_.QueryNearestPointByRelativeTime(preview_time);
@@ -487,11 +488,10 @@ void LatController::UpdateMatrixCompound() {
   // Initialize preview matrix
   matrix_adc_.block(0, 0, basic_state_size_, basic_state_size_) = matrix_ad_;
   matrix_bdc_.block(0, 0, basic_state_size_, 1) = matrix_bd_;
-  if (preview_time_ > 0.0) {
+  if (preview_window_ > 0) {
     matrix_bdc_(matrix_bdc_.rows() - 1, 0) = 1;
     // Update augument A matrix;
-    int preview_window = std::floor(preview_time_ / ts_);
-    for (int i = 0; i < preview_window - 1; ++i) {
+    for (int i = 0; i < preview_window_ - 1; ++i) {
       matrix_adc_(basic_state_size_ + i, basic_state_size_ + 1 + i) = 1;
     }
   }

modules/control/controller/lat_controller.h

@@ -33,6 +33,7 @@
 #include "modules/control/common/trajectory_analyzer.h"
 #include "modules/control/controller/controller.h"
 #include "modules/control/filters/digital_filter.h"
+#include "modules/control/filters/digital_filter_coefficients.h"
 #include "modules/control/filters/mean_filter.h"
 
 /**
@@ -158,8 +159,8 @@ class LatController : public Controller {
   // limit steering to maximum theoretical lateral acceleration
   double max_lat_acc_ = 0.0;
 
-  // look ahead time (preview controller)
-  double preview_time_ = 0;
+  // number of control cycles look ahead (preview controller)
+  int preview_window_ = 0;
   // number of states without previews, includes
   // lateral error, lateral error rate, heading error, heading error rate
   const int basic_state_size_ = 4;

modules/control/controller/lon_controller.cc

@@ -103,18 +103,22 @@ Status LonController::Init(const ControlConf *control_conf) {
   station_pid_controller_.Init(lon_controller_conf.station_pid_conf());
   speed_pid_controller_.Init(lon_controller_conf.low_speed_pid_conf());
 
-  digital_filter_pitch_angle_.set_coefficients(lon_controller_conf.ts(),
-       lon_controller_conf.pitch_angle_filter_conf().cutoff_freq());
+  SetDigitalFilterPitchAngle(lon_controller_conf);
 
   LoadControlCalibrationTable(lon_controller_conf);
-
-  previous_control_time_ = Clock::NowInSecond();
-
   controller_initialized_ = true;
 
   return Status::OK();
 }
 
+void LonController::SetDigitalFilterPitchAngle(
+    const LonControllerConf &lon_controller_conf) {
+  double cutoff_freq =
+      lon_controller_conf.pitch_angle_filter_conf().cutoff_freq();
+  double ts = lon_controller_conf.ts();
+  SetDigitalFilter(ts, cutoff_freq, &digital_filter_pitch_angle_);
+}
+
 void LonController::LoadControlCalibrationTable(
     const LonControllerConf &lon_controller_conf) {
   const auto &control_table = lon_controller_conf.calibration_table();
@@ -140,10 +144,6 @@ Status LonController::ComputeControlCommand(
   localization_ = localization;
   chassis_ = chassis;
 
-  current_control_time_ = Clock::NowInSecond();
-  dt_ = current_control_time_ - previous_control_time_;
-  previous_control_time_ = current_control_time_;
-
   trajectory_message_ = planning_published_trajectory;
   if (!control_interpolation_) {
     AERROR << "Fail to initialize calibration table.";
@@ -164,7 +164,8 @@ Status LonController::ComputeControlCommand(
 
   double brake_cmd = 0.0;
   double throttle_cmd = 0.0;
-  double preview_time = lon_controller_conf.preview_time();
+  double ts = lon_controller_conf.ts();
+  double preview_time = lon_controller_conf.preview_window() * ts;
 
   if (preview_time < 0.0) {
     const auto error_msg = apollo::common::util::StrCat(
@@ -185,7 +186,7 @@ Status LonController::ComputeControlCommand(
         debug->station_error(), -station_error_limit, station_error_limit);
   }
   double speed_offset =
-      station_pid_controller_.Control(station_error_limited, dt_);
+      station_pid_controller_.Control(station_error_limited, ts);
 
   double speed_controller_input = 0.0;
   double speed_controller_input_limit =
@@ -205,11 +206,11 @@ Status LonController::ComputeControlCommand(
       lon_controller_conf.switch_speed()) {
     speed_pid_controller_.SetPID(lon_controller_conf.low_speed_pid_conf());
     acceleration_cmd_closeloop =
-        speed_pid_controller_.Control(speed_controller_input_limited, dt_);
+        speed_pid_controller_.Control(speed_controller_input_limited, ts);
   } else {
     speed_pid_controller_.SetPID(lon_controller_conf.high_speed_pid_conf());
     acceleration_cmd_closeloop =
-        speed_pid_controller_.Control(speed_controller_input_limited, dt_);
+        speed_pid_controller_.Control(speed_controller_input_limited, ts);
   }
 
   double acceleration_cmd =
@@ -341,5 +342,13 @@ void LonController::ComputeLongitudinalErrors(
   debug->set_preview_acceleration_reference(preview_point.a());
 }
 
+void LonController::SetDigitalFilter(double ts, double cutoff_freq,
+                                     DigitalFilter *digital_filter) {
+  std::vector<double> denominators;
+  std::vector<double> numerators;
+  LpfCoefficients(ts, cutoff_freq, &denominators, &numerators);
+  digital_filter->set_coefficients(denominators, numerators);
+}
+
 }  // namespace control
 }  // namespace apollo

modules/control/controller/lon_controller.h

@@ -33,6 +33,7 @@
 #include "modules/control/common/trajectory_analyzer.h"
 #include "modules/control/controller/controller.h"
 #include "modules/control/filters/digital_filter.h"
+#include "modules/control/filters/digital_filter_coefficients.h"
 
 /**
  * @namespace apollo::control
@@ -103,9 +104,14 @@ class LonController : public Controller {
                                  SimpleLongitudinalDebug *debug);
 
  private:
+  void SetDigitalFilterPitchAngle(const LonControllerConf &lon_controller_conf);
+
   void LoadControlCalibrationTable(
       const LonControllerConf &lon_controller_conf);
 
+  void SetDigitalFilter(double ts, double cutoff_freq,
+                        DigitalFilter *digital_filter);
+
   void CloseLogFile();
 
   const ::apollo::localization::LocalizationEstimate *localization_ = nullptr;
@@ -125,10 +131,6 @@ class LonController : public Controller {
 
   DigitalFilter digital_filter_pitch_angle_;
 
-  double current_control_time_ = 0.0;
-  double previous_control_time_ = 0.0;
-  double dt_ = 0.0;
-
   const ControlConf *control_conf_ = nullptr;
 };
 }  // namespace control

modules/control/controller/lon_controller_test.cc

@@ -121,7 +121,8 @@ TEST_F(LonControllerTest, ComputeLongitudinalErrors) {
   vehicle_state->Update(localization_pb, chassis_pb);
   TrajectoryAnalyzer trajectory_analyzer(&trajectory_pb);
 
-  double preview_time = longitudinal_conf_.preview_time();
+  double ts = longitudinal_conf_.ts();
+  double preview_time = longitudinal_conf_.preview_window() * ts;
 
   SimpleLongitudinalDebug debug;
   ComputeLongitudinalErrors(&trajectory_analyzer, preview_time, &debug);

modules/control/filters/BUILD

@@ -11,6 +11,7 @@ cc_library(
         "digital_filter.h",
     ],
     deps = [
+        ":digital_filter_coefficients",
         "//modules/common:log",
     ],
 )
@@ -28,10 +29,21 @@ cc_library(
     ],
 )
 
+cc_library(
+    name = "digital_filter_coefficients",
+    srcs = [
+        "digital_filter_coefficients.cc",
+    ],
+    hdrs = [
+        "digital_filter_coefficients.h",
+    ],
+)
+
 cc_library(
     name = "filters",
     deps = [
         ":digital_filter",
+        ":digital_filter_coefficients",
         ":mean_filter",
     ],
 )
@@ -44,6 +56,7 @@ cc_test(
     ],
     deps = [
         ":digital_filter",
+        ":digital_filter_coefficients",
         "//modules/common",
         "@gtest//:main",
         "@ros//:ros_common",
@@ -62,4 +75,16 @@ cc_test(
     ],
 )
 
+cc_test(
+    name = "digital_filter_coefficients_test",
+    size = "small",
+    srcs = [
+        "digital_filter_coefficients_test.cc",
+    ],
+    deps = [
+        ":digital_filter_coefficients",
+        "@gtest//:main",
+    ],
+)
+
 cpplint()

modules/control/filters/digital_filter.cc

@@ -29,37 +29,27 @@ const double kDoubleEpsilon = 1.0e-6;
 namespace apollo {
 namespace control {
 
-DigitalFilter::DigitalFilter(const double ts, const double cutoff_freq) {
+DigitalFilter::DigitalFilter(const std::vector<double> &denominators,
+                             const std::vector<double> &numerators) {
   dead_zone_ = 0.0;
   last_ = 0.0;
-
-  set_coefficients(ts, cutoff_freq);
+  set_coefficients(denominators, numerators);
 }
 
-void DigitalFilter::set_coefficients(const double ts,
-                                     const double cutoff_freq) {
-  denominators_.clear();
-  numerators_.clear();
-  denominators_.reserve(3);
-  numerators_.reserve(3);
-
-  double wa = 2.0 * M_PI * cutoff_freq;  // Analog frequency in rad/s
-  double alpha = wa * ts / 2.0;          // tan(Wd/2), Wd is discrete frequency
-  double alpha_sqr = alpha * alpha;
-  double tmp_term = std::sqrt(2.0) * alpha + alpha_sqr;
-  double gain = alpha_sqr / (1.0 + tmp_term);
-
-  denominators_.push_back(1.0);
-  denominators_.push_back(2.0 * (alpha_sqr - 1.0) / (1.0 + tmp_term));
-  denominators_.push_back((1.0 - std::sqrt(2.0) * alpha + alpha_sqr) /
-                          (1.0 + tmp_term));
-
-  numerators_.push_back(gain);
-  numerators_.push_back(2.0 * gain);
-  numerators_.push_back(gain);
+void DigitalFilter::set_denominators(const std::vector<double> &denominators) {
+  denominators_ = denominators;
+  y_values_.resize(denominators_.size(), 0.0);
+}
 
+void DigitalFilter::set_numerators(const std::vector<double> &numerators) {
+  numerators_ = numerators;
   x_values_.resize(numerators_.size(), 0.0);
-  y_values_.resize(denominators_.size(), 0.0);
+}
+
+void DigitalFilter::set_coefficients(const std::vector<double> &denominators,
+                                     const std::vector<double> &numerators) {
+  set_denominators(denominators);
+  set_numerators(numerators);
 }
 
 void DigitalFilter::set_dead_zone(const double deadzone) {

modules/control/filters/digital_filter.h

@@ -47,7 +47,8 @@ class DigitalFilter {
    * @param denominators The denominators of the DigitalFilter.
    * @param numerators The numerators of the DigitalFilter.
    */
-  DigitalFilter(const double ts, const double cutoff_freq);
+  DigitalFilter(const std::vector<double> &denominators,
+                const std::vector<double> &numerators);
 
   /**
    * @brief Default destructor.
@@ -70,12 +71,25 @@ class DigitalFilter {
    * Output: y[0]
    */
 
+  /**
+   * @brief set denominators by an input vector
+   * @param denominators The denominators of filter
+   */
+  void set_denominators(const std::vector<double> &denominators);
+
+  /**
+   * @brief set numerators by an input vector
+   * @param numerators The numerators of filter
+   */
+  void set_numerators(const std::vector<double> &numerators);
+
   /**
    * @brief set denominators and numerators
    * @param denominators The denominators of filter
    * @param numerators The numerators of filter
    */
-  void set_coefficients(const double ts, const double cutoff_freq);
+  void set_coefficients(const std::vector<double> &denominators,
+                        const std::vector<double> &numerators);
 
   /**
    * @brief set filter deadzone

modules/control/filters/digital_filter_coefficients.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. 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 "modules/control/filters/digital_filter_coefficients.h"
+
+#include <cmath>
+#include <vector>
+
+namespace apollo {
+namespace control {
+
+void LpfCoefficients(const double ts, const double cutoff_freq,
+                     std::vector<double> *denominators,
+                     std::vector<double> *numerators) {
+  denominators->clear();
+  numerators->clear();
+  denominators->reserve(3);
+  numerators->reserve(3);
+
+  double wa = 2.0 * M_PI * cutoff_freq;  // Analog frequency in rad/s
+  double alpha = wa * ts / 2.0;          // tan(Wd/2), Wd is discrete frequency
+  double alpha_sqr = alpha * alpha;
+  double tmp_term = std::sqrt(2.0) * alpha + alpha_sqr;
+  double gain = alpha_sqr / (1.0 + tmp_term);
+
+  denominators->push_back(1.0);
+  denominators->push_back(2.0 * (alpha_sqr - 1.0) / (1.0 + tmp_term));
+  denominators->push_back((1.0 - std::sqrt(2.0) * alpha + alpha_sqr) /
+                          (1.0 + tmp_term));
+
+  numerators->push_back(gain);
+  numerators->push_back(2.0 * gain);
+  numerators->push_back(gain);
+
+  return;
+}
+
+}  // namespace control
+}  // namespace apollo

modules/control/filters/digital_filter_coefficients.h

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. 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.
+ *****************************************************************************/
+
+/**
+ * @file digital_filter_coefficients.h
+ * @brief Functions to generate coefficients for digital filter.
+ */
+
+#ifndef MODULES_CONTROL_FILTERS_DIGITAL_FILTER_COEFFICIENTS_H_
+#define MODULES_CONTROL_FILTERS_DIGITAL_FILTER_COEFFICIENTS_H_
+
+#include <vector>
+
+/**
+ * @namespace apollo::control
+ * @brief apollo::control
+ */
+namespace apollo {
+namespace control {
+
+/**
+ * @brief Get low-pass coefficients for digital filter.
+ * @param ts Time interval between signals.
+ * @param cutoff_freq Cutoff of frequency to filter high-frequency signals out.
+ * @param denominators Denominator coefficients for digital filter.
+ * @param numerators Numerator coefficients for digital filter.
+ */
+void LpfCoefficients(const double ts, const double cutoff_freq,
+                     std::vector<double> *denominators,
+                     std::vector<double> *numerators);
+
+}  // namespace control
+}  // namespace apollo
+
+#endif  // MODULES_CONTROL_FILTERS_DIGITAL_FILTER_COEFFICIENTS_H_

modules/control/filters/digital_filter_coefficients_test.cc

+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. 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 "modules/control/filters/digital_filter_coefficients.h"
+
+#include "gtest/gtest.h"
+
+namespace apollo {
+namespace control {
+
+class DigitalFilterCoefficientsTest : public ::testing::Test {
+ public:
+  virtual void SetUp() {}
+};
+
+TEST_F(DigitalFilterCoefficientsTest, LpfCoefficients) {
+  double ts = 0.01;
+  double cutoff_freq = 20;
+  std::vector<double> den;
+  std::vector<double> num;
+  LpfCoefficients(ts, cutoff_freq, &den, &num);
+  EXPECT_EQ(den.size(), 3);
+  EXPECT_EQ(num.size(), 3);
+  EXPECT_NEAR(num[0], 0.1729, 0.01);
+  EXPECT_NEAR(num[1], 0.3458, 0.01);
+  EXPECT_NEAR(num[2], 0.1729, 0.01);
+  EXPECT_NEAR(den[0], 1.0, 0.01);
+  EXPECT_NEAR(den[2], 0.2217, 0.01);
+}
+
+}  // namespace control
+}  // namespace apollo

modules/control/filters/digital_filter_test.cc

@@ -30,16 +30,33 @@ class DigitalFilterTest : public ::testing::Test {
 
 TEST_F(DigitalFilterTest, SetGet) {
   DigitalFilter digital_filter;
-  digital_filter.set_coefficients(0.01, 20);
+  std::vector<double> numerators = {1.0, 2.0, 3.0};
+  std::vector<double> denominators = {4.0, 5.0, 6.0};
+  digital_filter.set_denominators(denominators);
+  digital_filter.set_numerators(numerators);
   std::vector<double> denominators_got = digital_filter.denominators();
   std::vector<double> numerators_got = digital_filter.numerators();
-  EXPECT_EQ(numerators_got.size(), 3);
-  EXPECT_EQ(denominators_got.size(), 3);
-  EXPECT_NEAR(numerators_got[0], 0.1729, 0.01);
-  EXPECT_NEAR(numerators_got[1], 0.3458, 0.01);
-  EXPECT_NEAR(numerators_got[2], 0.1729, 0.01);
-  EXPECT_NEAR(denominators_got[0], 1.0, 0.01);
-  EXPECT_NEAR(denominators_got[2], 0.2217, 0.01);
+  EXPECT_EQ(numerators_got.size(), numerators.size());
+  EXPECT_EQ(denominators_got.size(), denominators.size());
+  for (size_t i = 0; i < numerators.size(); ++i) {
+    EXPECT_DOUBLE_EQ(numerators_got[i], numerators[i]);
+  }
+  for (size_t i = 0; i < denominators.size(); ++i) {
+    EXPECT_DOUBLE_EQ(denominators_got[i], denominators[i]);
+  }
+  digital_filter.set_coefficients(denominators, numerators);
+  denominators_got.clear();
+  denominators_got = digital_filter.denominators();
+  numerators_got.clear();
+  numerators_got = digital_filter.numerators();
+  EXPECT_EQ(numerators_got.size(), numerators.size());
+  EXPECT_EQ(denominators_got.size(), denominators.size());
+  for (size_t i = 0; i < numerators.size(); ++i) {
+    EXPECT_DOUBLE_EQ(numerators_got[i], numerators[i]);
+  }
+  for (size_t i = 0; i < denominators.size(); ++i) {
+    EXPECT_DOUBLE_EQ(denominators_got[i], denominators[i]);
+  }
 
   double dead_zone = 1.0;
   digital_filter.set_dead_zone(dead_zone);
@@ -47,7 +64,9 @@ TEST_F(DigitalFilterTest, SetGet) {
 }
 
 TEST_F(DigitalFilterTest, FilterOff) {
-  DigitalFilter digital_filter(0.1, 0);
+  std::vector<double> numerators = {0.0, 0.0, 0.0};
+  std::vector<double> denominators = {1.0, 0.0, 0.0};
+  DigitalFilter digital_filter(denominators, numerators);
 
   const std::vector<double> step_input(100, 1.0);
   std::vector<double> rand_input(100, 1.0);
@@ -67,17 +86,21 @@ TEST_F(DigitalFilterTest, FilterOff) {
 }
 
 TEST_F(DigitalFilterTest, MovingAverage) {
-  DigitalFilter digital_filter(0.25, 1);
+  std::vector<double> numerators = {0.25, 0.25, 0.25, 0.25};
+  std::vector<double> denominators = {1.0, 0.0, 0.0};
+  DigitalFilter digital_filter;
+  digital_filter.set_numerators(numerators);
+  digital_filter.set_denominators(denominators);
 
   const std::vector<double> step_input(100, 1.0);
   // Check step input, transients.
   for (size_t i = 0; i < 4; ++i) {
     double expected_filter_out = (i + 1) * 0.25;
-    EXPECT_NEAR(digital_filter.Filter(step_input[i]), expected_filter_out, 0.5);
+    EXPECT_FLOAT_EQ(digital_filter.Filter(step_input[i]), expected_filter_out);
   }
   // Check step input, steady state
   for (size_t i = 4; i < step_input.size(); ++i) {
-    EXPECT_NEAR(digital_filter.Filter(step_input[i]), 1.0, 0.1);
+    EXPECT_FLOAT_EQ(digital_filter.Filter(step_input[i]), 1.0);
   }
 }
 

modules/control/proto/lat_controller_conf.proto

@@ -5,7 +5,8 @@ package apollo.control;
 // simple optimal steer control param
 message LatControllerConf {
   optional double ts = 1;            // sample time (dt) 0.01 now, configurable
-  optional double preview_time = 2;
+  // preview window n, preview time = preview window * ts
+  optional int32 preview_window = 2;
   optional double cf = 3;
   optional double cr = 4;            // N/rad
   optional int32 mass_fl = 5;

modules/control/proto/lon_controller_conf.proto

@@ -18,7 +18,7 @@ message LonControllerConf {
   optional double throttle_deadzone = 3;
   optional double speed_controller_input_limit = 4;
   optional double station_error_limit = 5;
-  optional double preview_time = 6;
+  optional double preview_window = 6;
   optional double standstill_acceleration = 7;
 
   optional PidConf station_pid_conf = 8;

modules/control/testdata/conf/lincoln.pb.txt

@@ -12,7 +12,7 @@ active_controllers: LON_CONTROLLER
 max_steering_percentage_allowed: 100
 lat_controller_conf {
   ts: 0.01
-  preview_time: 0.0
+  preview_window: 0
   cf: 155494.663
   cr: 155494.663
   mass_fl: 520
@@ -80,7 +80,7 @@ lon_controller_conf {
   throttle_deadzone: 0.2
   speed_controller_input_limit: 3.0
   station_error_limit: 3.0
-  preview_time: 0.40
+  preview_window: 40.0
   standstill_acceleration: -3.0
   station_pid_conf {
     integrator_enable: false