linux版本模型预测控制mpc C++类

/* File $Id: QuadProg++.cc 232 2007-06-21 12:29:00Z digasper $ Author: Luca Di Gaspero DIEGM - University of Udine, Italy [email protected] http://www.diegm.uniud.it/digaspero/ This software may be modified and distributed under the terms of the MIT license. See the LICENSE file for details. */ #include <iostream> #include <algorithm> #include <cmath> #include <limits> #include <sstream> #include <stdexcept> #include "quadprog/QuadProg++.h" //#define TRACE_SOLVER namespace quadprogpp { // Utility functions for updating some data needed by the solution method void compute_d(Vector<double>& d, const Matrix<double>& J, const Vector<double>& np); void update_z(Vector<double>& z, const Matrix<double>& J, const Vector<double>& d, int iq); void update_r(const Matrix<double>& R, Vector<double>& r, const Vector<double>& d, int iq); bool add_constraint(Matrix<double>& R, Matrix<double>& J, Vector<double>& d, unsigned int& iq, double& rnorm); void delete_constraint(Matrix<double>& R, Matrix<double>& J, Vector<int>& A, Vector<double>& u, unsigned int n, int p, unsigned int& iq, int l); // Utility functions for computing the Cholesky decomposition and solving // linear systems void cholesky_decomposition(Matrix<double>& A); void cholesky_solve(const Matrix<double>& L, Vector<double>& x, const Vector<double>& b); void forward_elimination(const Matrix<double>& L, Vector<double>& y, const Vector<double>& b); void backward_elimination(const Matrix<double>& U, Vector<double>& x, const Vector<double>& y); // Utility functions for computing the scalar product and the euclidean // distance between two numbers double scalar_product(const Vector<double>& x, const Vector<double>& y); double distance(double a, double b); // Utility functions for printing vectors and matrices void print_matrix(const char* name, const Matrix<double>& A, int n = -1, int m = -1); template<typename T> void print_vector(const char* name, const Vector<T>& v, int n = -1); // The Solving function, implementing the Goldfarb-Idnani method double solve_quadprog(Matrix<double>& G, Vector<double>& g0, const Matrix<double>& CI, const Vector<double>& ci0, Vector<double>& x) { std::ostringstream msg; unsigned int n = G.ncols(), p = 0, m = CI.ncols(); if (G.nrows() != n) { msg << "The matrix G is not a squared matrix (" << G.nrows() << " x " << G.ncols() << ")"; std::cout<<msg.str()<<std::endl;; } if (CI.nrows() != n) { msg << "The matrix CI is incompatible (incorrect number of rows " << CI.nrows() << " , expecting " << n << ")"; std::cout<<msg.str()<<std::endl;; } if (ci0.size() != m) { msg << "The vector ci0 is incompatible (incorrect dimension " << ci0.size() << ", expecting " << m << ")"; std::cout<<msg.str()<<std::endl;; } x.resize(n); register unsigned int i, j, k, l; /* indices */ int ip; // this is the index of the constraint to be added to the active set Matrix<double> R(n, n); Matrix<double> J(n, n); Vector<double> s(m + p); Vector<double> z(n); Vector<double> r(m + p); Vector<double> d(n); Vector<double> np(n); Vector<double> u(m + p); Vector<double> x_old(n); Vector<double> u_old(m + p); double f_value, psi, c1, c2, sum, ss, R_norm; double inf; if (std::numeric_limits<double>::has_infinity) inf = std::numeric_limits<double>::infinity(); else inf = 1.0E300; double t, t1, t2; /* t is the step lenght, which is the minimum of the partial step length t1 * and the full step length t2 */ Vector<int> A(m + p), A_old(m + p), iai(m + p); unsigned int iq, iter = 0; Vector<bool> iaexcl(m + p); /* p is the number of equality constraints */ /* m is the number of inequality constraints */ #ifdef TRACE_SOLVER std::cout << std::endl << "Starting solve_quadprog" << std::endl; print_matrix("G", G); print_vector("g0", g0); print_matrix("CE", CE); print_vector("ce0", ce0); print_matrix("CI", CI); print_vector("ci0", ci0); #endif /* * Preprocessing phase */ /* compute the trace of the original matrix G */ c1 = 0.0; for (i = 0; i < n; i++) { c1 += G[i][i]; } /* decompose the matrix G in the form L^T L */ cholesky_decomposition(G); #ifdef TRACE_SOLVER print_matrix("G", G); #endif /* initialize the matrix R */ for (i = 0; i < n; i++) { d[i] = 0.0; for (j = 0; j < n; j++) R[i][j] = 0.0; } R_norm = 1.0; /* this variable will hold the norm of the matrix R */ /* compute the inverse of the factorized matrix G^-1, this is the initial value for H */ c2 = 0.0; for (i = 0; i < n; i++) { d[i] = 1.0; forward_elimination(G, z, d); for (j = 0; j < n; j++) J[i][j] = z[j]; c2 += z[i]; d[i] = 0.0; } #ifdef TRACE_SOLVER print_matrix("J", J); #endif /* c1 * c2 is an estimate for cond(G) */ /* * Find the unconstrained minimizer of the quadratic form 0.5 * x G x + g0 x * this is a feasible point in the dual space * x = G^-1 * g0 */ cholesky_solve(G, x, g0); for (i = 0; i < n; i++) x[i] = -x[i]; /* and compute the current solution value */ f_value = 0.5 * scalar_product(g0, x); #ifdef TRACE_SOLVER std::cout << "Unconstrained solution: " << f_value << std::endl; print_vector("x", x); #endif /* Add equality constraints to the working set A */ iq = 0; /* set iai = K \ A */ for (i = 0; i < m; i++) iai[i] = i; l1: iter++; #ifdef TRACE_SOLVER print_vector("x", x); #endif /* step 1: choose a violated constraint */ for (i = p; i < iq; i++) { ip = A[i]; iai[ip] = -1; } /* compute s[x] = ci^T * x + ci0 for all elements of K \ A */ ss = 0.0; psi = 0.0; /* this value will contain the sum of all infeasibilities */ ip = 0; /* ip will be the index of the chosen violated constraint */ for (i = 0; i < m; i++) { iaexcl[i] = true; sum = 0.0; for (j = 0; j < n; j++) sum += CI[j][i] * x[j]; sum += ci0[i]; s[i] = sum; psi += std::min(0.0, sum); } #ifdef TRACE_SOLVER print_vector("s", s, m); #endif if (fabs(psi) <= m * std::numeric_limits<double>::epsilon() * c1 * c2* 100.0) { /* numerically there are not infeasibilities anymore */ return f_value; } /* save old values for u and A */ for (i = 0; i < iq; i++) { u_old[i] = u[i]; A_old[i] = A[i]; } /* and for x */ for (i = 0; i < n; i++) x_old[i] = x[i]; l2: /* Step 2: check for feasibility and determine a new S-pair */ for (i = 0; i < m; i++) { if (s[i] < ss && iai[i] != -1 && iaexcl[i]) { ss = s[i]; ip = i; } } if (ss >= 0.0) { return f_value; } /* set np = n[ip] */ for (i = 0; i < n; i++) np[i] = CI[i][ip]; /* set u = [u 0]^T */ u[iq] = 0.0; /* add ip to the active set A */ A[iq] = ip; #ifdef TRACE_SOLVER std::cout << "Trying with constraint " << ip << std::endl; print_vector("np", np); #endif l2a:/* Step 2a: determine step direction */ /* compute z = H np: the step direction in the primal space (through J, see the paper) */ compute_d(d, J, np); update_z(z, J, d, iq); /* compute N* np (if q > 0): the negative of the step direction in the dual space */ update_r(R, r, d, iq); #ifdef TRACE_SOLVER std::cout << "Step direction z" << std::endl; print_vector("z", z); print_vector("r", r, iq + 1); print_vector("u", u, iq + 1); print_vector("d", d); print_vector("A", A, iq + 1); #endif /* Step 2b: compute step length */ l = 0; /* Compute t1: partial step length (maximum step in dual space without violating dual feasibility */ t1 = inf; /* +inf */ /* find the index l s.t. it reaches the minimum of u+[x] / r */ for (k = p; k < iq; k++) { if (r[k] > 0.0) { if (u[k] / r[k] < t1) { t1 = u[k] / r[k]; l = A[k]; } } } /* Compute t2: full step length (minimum step in primal space such that the con