Update mpc solver and test file

modules/common/math/mpc_solver.cc

@@ -6,8 +6,7 @@
  * 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
+ * * 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
@@ -25,7 +24,8 @@ namespace math {
 
 using Matrix = Eigen::MatrixXd;
 
-// Linear MPC solver, for single actuator
+// discrete linear predictive control solver, with control format
+// x(i + 1) = A * x(i) + B * u (i) + C
 void SolveLinearMPC(const Matrix &matrix_a,
         const Matrix &matrix_b,
         const Matrix &matrix_c,
@@ -46,50 +46,50 @@ void SolveLinearMPC(const Matrix &matrix_a,
         return;
     }
 
-    // Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr, vector of matrix A power
-    Matrix matrix_k = Matrix::Zero(matrix_b.rows() * control->size(), matrix_b.cols() * control->size());
-    Matrix matrix_m = Matrix::Zero(matrix_b.rows() * control->size(), 1);
-    Matrix matrix_t = matrix_m;
-    Matrix matrix_v = Matrix::Zero((*control)[0].rows() * control->size(), 1);
-    Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows());
-    Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols());
-    Matrix matrix_ll = Matrix::Zero(control->size() * matrix_lower.rows(), 1);
-    Matrix matrix_uu = Matrix::Zero(control->size() * matrix_upper.rows(), 1);
-    std::vector<Matrix> matrix_a_power(control->size());
+    const unsigned int horizon = reference.size();
+
+    // Update augment reference matrix_t
+    Matrix matrix_t = Matrix::Zero(matrix_b.rows() * horizon, 1);
+    for (unsigned int j = 0; j < horizon; ++j) {
+        matrix_t.block(j * reference[0].size(), 0, reference[0].size(), 1) = reference[j];
+    }
+
+    // Update augment control matrix_v
+    Matrix matrix_v = Matrix::Zero((*control)[0].rows() * horizon, 1);
+    for (unsigned int j = 0; j < horizon; ++j) {
+        matrix_v.block(j * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = (*control)[j];
+    }
 
-    // Compute power of matrix_a
+    std::vector<Matrix> matrix_a_power(horizon);
     matrix_a_power[0] = matrix_a;
     for (unsigned int i = 1; i < matrix_a_power.size(); ++i) {
         matrix_a_power[i] = matrix_a * matrix_a_power[i-1];
     }
 
-    // Compute matrix_k
-    for (unsigned int r = 0; r < control->size(); ++r) {
+    Matrix matrix_k = Matrix::Zero(matrix_b.rows() * horizon, matrix_b.cols() * control->size());
+    for (unsigned int r = 0; r < horizon; ++r) {
         for (unsigned int c = 0; c <= r; ++c) {
             matrix_k.block(r * matrix_b.rows(), c * matrix_b.cols(), matrix_b.rows(), \
                     matrix_b.cols()) = matrix_a_power[r-c] * matrix_b;
         }
     }
 
+    // Initialize matrix_k, matrix_m, matrix_t and matrix_v, matrix_qq, matrix_rr, vector of matrix A power
+    Matrix matrix_m = Matrix::Zero(matrix_b.rows() * horizon, 1);
+    Matrix matrix_qq = Matrix::Zero(matrix_k.rows(), matrix_k.rows());
+    Matrix matrix_rr = Matrix::Zero(matrix_k.cols(), matrix_k.cols());
+    Matrix matrix_ll = Matrix::Zero(horizon * matrix_lower.rows(), 1);
+    Matrix matrix_uu = Matrix::Zero(horizon * matrix_upper.rows(), 1);
+
     // Compute matrix_m
     matrix_m.block(0, 0, matrix_a.rows(), 1) = matrix_a * matrix_initial_state + matrix_c;
-    for (unsigned int i = 1; i < control->size(); ++i) {
-        matrix_m.block(i * matrix_a.rows(), 0, matrix_a.rows(), 1) = matrix_a * matrix_m.block((i-1) * matrix_a.rows(), 0, matrix_a.rows(), 1) + matrix_c;
-    }
-
-    // compute matrix_t
-    for (unsigned int j = 0; j < reference.size(); ++j) {
-        matrix_t.block(j * reference[0].size(), 0, reference[0].size(), 1) = reference[j];
-    }
-
-    // compute matrix_v
-    for (unsigned int j = 0; j < control->size(); ++j) {
-      //  matrix_v.block(j * control->size(), 0, control->size(), 1) = control[j];
-        matrix_v.block(j * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = (*control)[j];
+    for (unsigned int i = 1; i < horizon; ++i) {
+        matrix_m.block(i * matrix_a.rows(), 0, matrix_a.rows(), 1) = matrix_a \
+                * matrix_m.block((i-1) * matrix_a.rows(), 0, matrix_a.rows(), 1) + matrix_c;
     }
 
-    // compute matrix_ll, matrix_uu, matrix_qq, matrix_rr together
-    for (unsigned int i = 0; i < control->size(); ++i) {
+    // Compute matrix_ll, matrix_uu, matrix_qq, matrix_rr
+    for (unsigned int i = 0; i < horizon; ++i) {
         matrix_ll.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = matrix_lower;
         matrix_uu.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1) = matrix_upper;
         matrix_qq.block(i * matrix_q.rows(), i * matrix_q.rows(), \
@@ -98,13 +98,13 @@ void SolveLinearMPC(const Matrix &matrix_a,
                     matrix_r.rows(), matrix_r.rows()) = matrix_r;
     }
 
-    // update matrix_m1, matrix_m2, convert MPC problem to QP problem done
+    // Update matrix_m1, matrix_m2, convert MPC problem to QP problem done
     Matrix matrix_m1 = matrix_k.transpose() * matrix_qq * matrix_k + matrix_rr;
     Matrix matrix_m2 = matrix_k.transpose() * matrix_qq * (matrix_m - matrix_t);
 
     // Method 2: QP_SMO_Solver
     SolveQPSMO(matrix_m1, matrix_m2, matrix_ll, matrix_uu, eps, max_iter, &matrix_v);
-    for (unsigned int i = 0; i < control->size(); ++i) {
+    for (unsigned int i = 0; i < horizon; ++i) {
         (*control)[i] = matrix_v.block(i * (*control)[0].rows(), 0, (*control)[0].rows(), 1);
     }
 }
@@ -117,10 +117,10 @@ void SolveQPSMO (
         const double& eps,
         const int& max_iter,
         Matrix* matrix_v) {
-    // Warning: Skipped the sanity check since this is designed for solely used by mpc_solver, if you want to
-    // use it for other purpose, force sanity check at the beginning
+    // Warning: Skipped the sanity check since this is designed for solely used by mpc_solver
+    // TODO: Replace this with available QP solver
     Matrix matrix_df = matrix_q * (* matrix_v) + matrix_b;
-    Matrix matrix_qq = matrix_q.inverse();
+    Matrix matrix_q_inv = matrix_q.inverse();
     for (int iter = 0; iter < max_iter; ++iter) {
             double max_df = 0;
             int best_r = 0;
@@ -143,7 +143,7 @@ void SolveQPSMO (
             {
                 const double old_alpha = (*matrix_v)(r);
                 (*matrix_v)(r) = -(matrix_df(r) - matrix_q(r, r) * (*matrix_v)(r))
-                    * matrix_qq(r);
+                    * matrix_q_inv(r);
                 if ((*matrix_v)(r) < matrix_lower(r)) {
                     (*matrix_v)(r) = matrix_lower(r);
                 }
@@ -158,10 +158,7 @@ void SolveQPSMO (
                 }
             }
 
-    AERROR << "max_df is: " << max_df << std::endl;
             if (max_df < eps) {
-    AERROR << "max_df is less than eps: " << max_df << std::endl;
-    AERROR << "iter now is: " << iter << std::endl;
                 break;
             }
         }

modules/common/math/mpc_solver_test.cc

@@ -26,8 +26,7 @@ namespace math {
 TEST(MPCSolverTest, MPC) {
     const int STATES = 4;
     const int CONTROLS = 1;
-    const int PREVIEW_HORIZON = 30;
-    const int CONTROL_HORIZON = 30;
+    const int HORIZON = 10;
     const double EPS = 0.01;
     const int MAX_ITER = 100;
 
@@ -61,7 +60,6 @@ TEST(MPCSolverTest, MPC) {
     Eigen::MatrixXd lower_bound(CONTROLS, 1);
     lower_bound << -0.2;
 
-    std::cout << "lower_bound size: " << lower_bound.size() << std::endl;
     Eigen::MatrixXd upper_bound(CONTROLS, 1);
     upper_bound << 0.6;
 
@@ -77,18 +75,75 @@ TEST(MPCSolverTest, MPC) {
                        0,
                        0;
 
-    std::vector<Eigen::MatrixXd> reference(PREVIEW_HORIZON, reference_state);
+    std::vector<Eigen::MatrixXd> reference(HORIZON, reference_state);
 
     Eigen::MatrixXd control_matrix(CONTROLS, 1);
     control_matrix << 0;
 
-    std::vector<Eigen::MatrixXd> control(CONTROL_HORIZON, control_matrix);
-    solve_linear_mpc(A, B, C, Q, R, lower_bound, upper_bound, initial_state, reference, EPS, MAX_ITER, &control);
-    for (int i = 0; i < control.size(); ++i) {
-        EXPECT_FLOAT_EQ(0.6, control[0](0));
+    std::vector<Eigen::MatrixXd> control(HORIZON, control_matrix);
+
+    for (unsigned int i = 0; i < control.size(); ++i) {
+        for (unsigned int i = 1; i < control.size(); ++i) {
+            control[i-1] = control[i];
+        }
+        SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state, reference, EPS, MAX_ITER, &control);
+        EXPECT_FLOAT_EQ(upper_bound(0), control[0](0));
+        initial_state = A * initial_state + B * control[0](0) + C;
+    }
+
+    Eigen::MatrixXd initial_state1(STATES, 1);
+    initial_state1 << 300,
+                      300,
+                      0,
+                      0;
+
+    Eigen::MatrixXd reference_state1(STATES, 1);
+    reference_state1 << 200,
+                        200,
+                        0,
+                        0;
+
+    std::vector<Eigen::MatrixXd> reference1(HORIZON, reference_state1);
+
+    control_matrix << 0;
+    std::vector<Eigen::MatrixXd> control1(HORIZON, control_matrix);
+
+    for (unsigned int i = 0; i < control1.size(); ++i) {
+        for (unsigned int i = 1; i < control.size(); ++i) {
+            control1[i-1] = control1[i];
+        }
+        SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state1, reference1, EPS, MAX_ITER, &control1);
+        EXPECT_FLOAT_EQ(lower_bound(0), control1[0](0));
+        initial_state1 = A * initial_state1 + B * control1[0](0) + C;
     }
-}
 
+    Eigen::MatrixXd initial_state2(STATES, 1);
+    initial_state2 << 300,
+                      300,
+                      0,
+                      0;
+
+    Eigen::MatrixXd reference_state2(STATES, 1);
+    reference_state2 << 300,
+                        300,
+                        0,
+                        0;
+
+    std::vector<Eigen::MatrixXd> reference2(HORIZON, reference_state2);
+
+    control_matrix << 0;
+    std::vector<Eigen::MatrixXd> control2(HORIZON, control_matrix);
+
+    for (unsigned int i = 0; i < control2.size(); ++i) {
+        for (unsigned int i = 1; i < control.size(); ++i) {
+            control2[i-1] = control2[i];
+        }
+        SolveLinearMPC(A, B, C, Q, R, lower_bound, upper_bound, initial_state2, reference2, EPS, MAX_ITER, &control2);
+        EXPECT_FLOAT_EQ(0.0, control2[0](0));
+        initial_state2 = A * initial_state2 + B * control2[0](0) + C;
+    }
+
+}
 }  // namespace math
 }  // namespace common
 }  // namespace apollo