/************************************************************************************************** * Class for MPC with constraint * * The plant to be controlled is a Linear Time-Invariant System: * x(k+1) = A*x(k) + B*u(k) ; x = Nx1, u = Mx1 * z(k) = C*x(k) ; z = Zx1 * * ** Calculate prediction of z(k+1..k+Hp) constants ************************************************ * * Prediction of state variable of the system: * z(k+1..k+Hp) = (CPSI)*x(k) + (COMEGA)*u(k-1) + (CTHETA)*dU(k..k+Hu-1) ...{MPC_1} * * Constants: * CPSI = [CA C(A^2) ... C(A^Hp)]' : (Hp*N)xN * COMEGA = [CB C(B+A*B) ... C*Sigma(i=0->Hp-1)A^i*B]' : (Hp*N)xM * CTHETA = [ CB 0 .... 0 ] * [ C(B+A*B) CB . 0 ] * [ . . . CB ] : (Hp*N)x(Hu*M) * [ . . . . ] * [C*Sigma(i=0->Hp-1)(A^i*B) . .... C*Sigma(i=0->Hp-Hu)A^i*B] * * ** Calculate QPs offline optimization variable *************************************************** * * Calculate Qqp and its inverse: * Qqp = 2H = 2*(CTHETA'*Q*CTHETA + R) ...{MPC_2} * Qqp_inv = Qqp^(-1) * * Calculate the constant portion of optimization variable G in QP form Cqp: * Cqp = -G = -2.*CTHETA'*Q*E(k) ...{MPC_3} * * Calculate the left side of the contraints equation ...{MPC_4} * ** MPC update algorithm ************************************************************************** * * Formulation of plant error prediction * E(k) = SP(k) - CPSI*x(k) - COMEGA*u(k-1) ...{MPC_5} * * Calculate online portion of optimization variable G in QP form Cqp: * Cqp = Cqp,const * *E(k) ...{MPC_6} * * Calculate the right side of the contraints equation ...{MPC_7} * * MPC solution: * (a) For unconstrained MPC: * d [dU(k)'*H*dU(k) - G'*dU(k)] * ----------------------------- = 0 --> 2*H*dU(k)-G = 0 * d[dU(k)] * * --> dU(k)_optimal = 1/2 * H^-1 * G * --> https://github.com/pronenewbits/Arduino_Unconstrained_MPC_Library * * (b) For constrained MPC (quadrating programming): * min dU(k)'*H*dU(k) - G'*dU(k) ; subject to inequality constraints * dU(k) * * Reconditioning H & G matrices into a standard QP form: * x_opt = arg Min 1/2*x'*Q*x + c'*x ; subject to ineqLHS*x <= ineqRHS * x * * Then solve by Active Set: * dU_opt(k) = ActiveSet(Qqp, Qqp,inv, cqp, ineqLHS, ineqRHS) ...{MPC_8} * * Integrate the du(k) to get u(k): * u(k) = u(k-1) + du(k) ...{MPC_9} * * * * See https://github.com/pronenewbits for more! *************************************************************************************************/ #include "mpc.h" MPC::MPC(Matrix &A, Matrix &B, Matrix &C, float_prec _bobotQ, float_prec _bobotR, void (*vCreateConstraintsLHS)(Matrix &, const Matrix &), void (*vCreateConstraintsRHS)(Matrix &, const Matrix &, const Matrix &, const Matrix &, const Matrix &)) { vReInit(A, B, C, _bobotQ, _bobotR, vCreateConstraintsLHS, vCreateConstraintsRHS); } void MPC::vReInit(Matrix &A, Matrix &B, Matrix &C, float_prec _bobotQ, float_prec _bobotR, void (*vCreateConstraintsLHS)(Matrix &, const Matrix &), void (*vCreateConstraintsRHS)(Matrix &, const Matrix &, const Matrix &, const Matrix &, const Matrix &)) { this->A = A; this->B = B; this->C = C; this->vCreateConstraintsLHS = vCreateConstraintsLHS; this->vCreateConstraintsRHS = vCreateConstraintsRHS; CnstLHS = Matrix(0,0); CnstRHS = Matrix(0,0); Q.vSetDiag(_bobotQ); R.vSetDiag(_bobotR); /* Calculate prediction of z(k+1..k+Hp) constants * * Prediction of state variable of the system: * z(k+1..k+Hp) = (CPSI)*x(k) + (COMEGA)*u(k-1) + (CTHETA)*dU(k..k+Hu-1) ...{MPC_1} * * Constants: * CPSI = [CA C(A^2) ... C(A^Hp)]' : (Hp*N)xN * COMEGA = [CB C(B+A*B) ... C*Sigma(i=0->Hp-1)A^i*B]' : (Hp*N)xM * CTHETA = [ CB 0 .... 0 ] * [ C(B+A*B) CB . 0 ] * [ . . . CB ] : (Hp*N)x(Hu*M) * [ . . . . ] * [C*Sigma(i=0->Hp-1)(A^i*B) . .... C*Sigma(i=0->Hp-Hu)A^i*B] * */ Matrix _Apow(SS_X_LEN, SS_X_LEN); /* CPSI : [ C * A ] * [ C * A^2 ] * [ . ] : (Hp*N) x N * [ . ] * [ C * A^Hp ] */ _Apow = A; for (int16_t _i = 0; _i < MPC_HP_LEN; _i++) { CPSI = CPSI.InsertSubMatrix((C*_Apow), _i*SS_Z_LEN, 0); _Apow = _Apow * A; } /* COMEGA : [ C * (B) ] * [ C * (B+A*B) ] * [ . ] : (Hp*N) x M * [ . ] * [ C * Sigma(i=0->Hp-1)A^i*B] */ Matrix _tempSigma(SS_X_LEN, SS_U_LEN); _Apow.vSetIdentity(); _tempSigma = B; for (int16_t _i = 0; _i < MPC_HP_LEN; _i++) { COMEGA = COMEGA.InsertSubMatrix((C*_tempSigma), _i*SS_Z_LEN, 0); _Apow = _Apow * A; _tempSigma = _tempSigma + (_Apow*B); } /* CTHETA : [ C * (B) 0 .... 0 ] * [ C * (B+A*B) C * (B) . 0 ] * [ . . . C * (B) ]: (Hp*N)x(Hu*M) * [ . . . . ] * [C * Sigma(i=0->Hp-1)A^i*B . .... C * Sigma(i=0->Hp-Hu)A^i*B] * * : [COMEGA [0 COMEGA(0:(len(COMEGA)-len(B)),:)]' .... [0..0 COMEGA(0:(len(COMEGA)-((Hp-Hu)*len(B))),:)]'] */ for (int16_t _i = 0; _i < MPC_HU_LEN; _i++) { CTHETA = CTHETA.InsertSubMatrix(COMEGA, _i*SS_Z_LEN, _i*SS_U_LEN, (MPC_HP_LEN*SS_Z_LEN)-(_i*SS_Z_LEN), SS_U_LEN); } /* Calculate offline optimization variable H in QP form (Qqp): * Qqp = 2H = 2*(CTHETA'*Q*CTHETA + R) ...{MPC_2} * * Note: we need to reconditioning into standard QP form (Q_qp = 2H_mpc, c_qp = -G_mpc) */ Qqp = 2*((CTHETA.Transpose()) * Q * CTHETA + R); Qqp_INV = Qqp.Invers(); /* Calculate the constant portion of optimization variable G in QP form Cqp: * Cqp = -G = -2.*CTHETA'*Q*E(k) ...{MPC_3} * \-----v-----/ * This portion */ cqp_const = -2.0 * (CTHETA.Transpose()) * Q; /* The left side of the contraints equation is constant. Calculate at initialization ...{MPC_4} */ vCreateConstraintsLHS(CnstLHS, CTHETA); } bool MPC::bUpdate(Matrix &SP, Matrix &x, Matrix &u) { /* Formulation of plant error prediction: * E(k) = SP(k) - CPSI*x(k) - COMEGA*u(k-1) ...{MPC_5} */ Err = SP - CPSI*x - COMEGA*u; /* Calculate online portion of optimization variable G in QP form Cqp: * Cqp = Cqp,const * *E(k) ...{MPC_6} */ cqp = cqp_const * Err; /* Create the inequality constraints ...{MPC_7} */ vCreateConstraintsRHS(CnstRHS, COMEGA, CPSI, u, x); #if (0) /* Experiment: disable constraints, the MPC will behave like unconstrained MPC */ #warning("Contstraints bypassed (no constraints)"); CnstLHS = Matrix(0,0); CnstRHS = Matrix(0,0); #endif /* Formulation of the optimal control problem: * * For constrained MPC (quadrating programming): * min dU(k)'*H*dU(k) - G'*dU(k) ; subject to inequality constraints * dU(k) * * Reconditioning H & G matrices into a standard QP form: * x_opt = arg Min 1/2*x'*Q*x + c'*x ; subject to ineqLHS*x <= ineqRHS * x * * Then solve by Active Set: * dU_opt(k) = ActiveSet(Qqp, Qqp,inv, cqp, ineqLHS, ineqRHS) ...{MPC_8} * * * 1/2*Q = H --> Q = 2*H (the reconditioning has been done when we create H & G) * c' = -G' --> c = -G */ if (!bActiveSet(DU, Qqp, Qqp_INV, cqp, CnstLHS, CnstRHS, cntIterActiveSet)) { /* return false; */ DU.vSetToZero(); return false; } /* Integrate the du(k) to get u(k): * u(k) = u(k-1) + du(k) ...{MPC_9} */ Matrix DU_Out(SS_U_LEN, 1); for (int16_t _i = 0; _i < SS_U_LEN; _i++) { DU_Out[_i][0] = DU[_i][0]; } u = u + DU_Out; return true; } /* Active Set solver for Quadratic Programming problem in the form: * * x_opt = arg Min 1/2*x'*Q*x + c'*x ; subject to ineqLHS*x <= ineqRHS * x * * * The Active Set: search x, by solving this minimization problem: * * min. 1/2*dx'*Q*dx + (Q*x+c)'*dx , subject to ineqLHS_dx*x = 0 * dx * * Integrate x(iter+1) = x(iter) + dx(iter) * * Until KKT conditions is satisfied: * 1. Q*dx + ineqLHS_dx*(dLambda) = -(Q*x+c) (dLambda = Lagrange multiplier of above minimization solution) * 2. -ineqLHS_dx*dx = 0 * */ bool MPC::bActiveSet(Matrix &x, const Matrix &Q, const Matrix &Qinv, const Matrix &c, const Matrix &ineqLHS, const Matrix &ineqRHS, int16_t &_i16iterActiveSet) { bool _flagConstActive[ineqRHS.i16getRow()]; /* Contains information about which inequality is active (if true, then that inequality is active). */ bool _dlambdaPos, _dxNol; Matrix dx(x.i16getRow(), 1, Matrix::NoInitMatZero); Matrix dLambda(0, 0, Matrix::NoInitMatZero); /* TODO: Make sure x initial value is inside feasible region (e.g. using Linear Programming). * For now we set it to zero --> no mathematical guarantee! */ x.vSetToZero(); /* In the beginning, every constraints is non-active */ for (int16_t _i = 0; _i < ineqRHS.i16getRow(); _i++) { _flagConstActive[_i] = false; } _i16iterActiveSet = 0; /* Reset counter iteration Active Set */ do { /* Construct active set matrix Aw (_ineqActiveLHS) */ uint16_t _u16cntConstActive = 0; for (int16_t _i = 0; _i < ineqLHS.i16getRow(); _i++) { if (_flagConstActive[_i] == true) { _u16cntConstActive++; } } int16_t _indexConstActive[_u16cntConstActive]; /* Contains information about index of the active inequality constraints in ineqLHS */ Matrix _ineqActiveLHS(_u16cntConstActive, ineqLHS.i16getCol(), Matrix::NoInitMatZero); /* Aggregation of the row vectors of ineqLHS that is 'active', i.e. the Aw matrix */ for (int16_t _i = 0, _iterConstActive = 0; _i < ineqLHS.i16getRow(); _i++) { if (_flagConstActive[_i] == true) { _ineqActiveLHS = _ineqActiveLHS.InsertSubMatrix(ineqLHS, _iterConstActive, 0, _i, 0, 1, ineqLHS.i16getCol()); _indexConstActive[_iterConstActive] = _i; _iterConstActive++; ASSERT((_iterConstActive <= _u16cntConstActive), "Bug on the active set: create _ineqActiveLHS"); } } /* Solve KKT system using CholeskyDec + Forward/Back subtitution */ Matrix _g(-((Q*x)+c)); if (_u16cntConstActive > 0) { Matrix _ineqActiveLHStr(_ineqActiveLHS.Transpose()); Matrix _KKT_AGinAtr_chol((_ineqActiveLHS*Qinv*_ineqActiveLHStr).CholeskyDec()); dLambda = dLambda.ForwardSubtitution(_KKT_AGinAtr_chol, (_ineqActiveLHS*Qinv*_g)); dLambda = dLambda.BackSubtitution(_KKT_AGinAtr_chol.Transpose(), dLambda); dx = (Qinv * (_g-(_ineqActiveLHStr*dLambda))); } else { dx = (Qinv * _g); } /* Check for Karush–Kuhn–Tucker conditions ------------------------------------------------------------------------------------------------ */ /* Karush–Kuhn–Tucker conditions: * 1. dx == 0 * 2. dLambda >= 0 */ /* Search for dx == 0 ----------------- */ _dxNol = true; for (int16_t _i = 0; _i < Q.i16getRow(); _i++) { if (fabs(dx[_i][0]) > float_prec(float_prec_ZERO_ECO)) { _dxNol = false; break; } } /* Search for dLambda >= 0 ------------ */ float_prec _lowestLambda = 0.0; int16_t _idxIneqLowestLambda = -1; _dlambdaPos = true; for (int16_t _i = 0; _i < _u16cntConstActive; _i++) { if (dLambda[_i][0] < _lowestLambda) { /* Some constraints become inactive, search constraint with lowest Lagrange multiplier so we can remove it */ _lowestLambda = dLambda[_i][0]; _idxIneqLowestLambda = _indexConstActive[_i]; _dlambdaPos = false; } } /* Check it! -------------------------- */ if (_dxNol && _dlambdaPos) { break; } if (!_dlambdaPos) { /* There is a constraint that become inactive ----------------------------------------------------------------------------------------- */ /* remove the constraint that have lowest Lagrange multiplier */ ASSERT((_idxIneqLowestLambda != -1), "Bug on the active set: remove ineq active"); _flagConstActive[_idxIneqLowestLambda] = false; } if (!_dxNol) { /* It means the x(k+1) will move ------------------------------------------------------------------------------------------------------ */ /* Search for violated constraints. If any, then get the lowest scalar value _alpha that makes * the searching direction feasible again */ float_prec _alphaMin = 1.0; int16_t _idxAlphaMin = -1; Matrix _ineqNonActiveLHS_dx(ineqLHS * dx); Matrix _ineqNonActiveLHS_x(ineqLHS * x); for (int16_t _i = 0; _i < ineqLHS.i16getRow(); _i++) { if (_flagConstActive[_i] == false) { /* For all constraints that is not active, we check if that constraint is violated */ if (_ineqNonActiveLHS_dx[_i][0] > float_prec_ZERO_ECO) { /* We have a violated constraint. Make that constraint a candidate of active constraint */ float_prec _alphaCandidate = - (_ineqNonActiveLHS_x[_i][0] - ineqRHS[_i][0]) / _ineqNonActiveLHS_dx[_i][0]; if (_alphaCandidate < _alphaMin) { _alphaMin = _alphaCandidate; _idxAlphaMin = _i; } } } } if (_idxAlphaMin != -1) { _flagConstActive[_idxAlphaMin] = true; } /* Should we do ASSERT(_alphaMin > 0.0) here? */ if (_alphaMin > 0) { x = x + (dx * _alphaMin); } } /* Check for maximum iteration conditions ------------------------------------------------------------------------------------------------- */ _i16iterActiveSet++; if (_i16iterActiveSet > MPC_MAXIMUM_ACTIVE_SET_ITERATION) { /* Maximum number of iteration reached, terminate and use last solution */ break; } } while (1); return true; }