From 47808b1f28c2089207862aba6eaea8c94d3dbfee Mon Sep 17 00:00:00 2001
From: kechxu <kechxu89@gmail.com>
Date: Tue, 17 Jul 2018 13:16:45 -0700
Subject: [PATCH] Prediction: fix broken prediction trajectory

---
 modules/prediction/common/prediction_util.cc | 19 +++++++++++++++----
 modules/prediction/common/prediction_util.h  |  4 ++--
 2 files changed, 17 insertions(+), 6 deletions(-)

diff --git a/modules/prediction/common/prediction_util.cc b/modules/prediction/common/prediction_util.cc
index 1d1a1d25e6..24f5a8c636 100644
--- a/modules/prediction/common/prediction_util.cc
+++ b/modules/prediction/common/prediction_util.cc
@@ -63,11 +63,17 @@ int SolveQuadraticEquation(const std::vector<double>& coefficients,
 double EvaluateQuinticPolynomial(
     const std::array<double, 6>& coeffs,
     const double t, const uint32_t order,
-    const double end_t, const double end_value) {
+    const double end_t, const double end_v) {
   if (t >= end_t) {
     switch (order) {
       case 0: {
-        return end_value;
+        double end_value = ((((coeffs[5] * end_t + coeffs[4]) * end_t +
+            coeffs[3]) * end_t + coeffs[2]) * end_t + coeffs[1]) * end_t +
+            coeffs[0];
+        return end_value + end_v * (t - end_t);
+      }
+      case 1: {
+        return end_v;
       }
       default: {
         return 0.0;
@@ -104,11 +110,16 @@ double EvaluateQuinticPolynomial(
 double EvaluateQuarticPolynomial(
     const std::array<double, 5>& coeffs,
     const double t, const uint32_t order,
-    const double end_t, const double end_value) {
+    const double end_t, const double end_v) {
   if (t >= end_t) {
     switch (order) {
       case 0: {
-        return end_value;
+        double end_value = (((coeffs[4] * end_t + coeffs[3]) * end_t +
+            coeffs[2]) * end_t + coeffs[1]) * end_t + coeffs[0];
+        return end_value + (t - end_t) * end_v;
+      }
+      case 1: {
+        return end_v;
       }
       default: {
         return 0.0;
diff --git a/modules/prediction/common/prediction_util.h b/modules/prediction/common/prediction_util.h
index dc6effca92..ed67f0b851 100644
--- a/modules/prediction/common/prediction_util.h
+++ b/modules/prediction/common/prediction_util.h
@@ -69,7 +69,7 @@ int SolveQuadraticEquation(const std::vector<double>& coefficients,
 double EvaluateQuinticPolynomial(
     const std::array<double, 6>& coeffs,
     const double t, const uint32_t order,
-    const double end_t, const double end_value);
+    const double end_t, const double end_v);
 
 /**
  * @brief Evaluate quartic polynomial.
@@ -80,7 +80,7 @@ double EvaluateQuinticPolynomial(
 double EvaluateQuarticPolynomial(
     const std::array<double, 5>& coeffs,
     const double t, const uint32_t order,
-    const double end_t, const double end_value);
+    const double end_t, const double end_v);
 
 }  // namespace math_util
 
-- 
GitLab