├── .gitattributes ├── All .png ├── All 1 3D.png ├── All 1.png ├── All 3d.png ├── All Final 1 (different x2 x3) 1.png ├── All Final 1 (different x2 x3).png ├── All Final.png ├── Both N = 3 (feasibility Region).png ├── Capture.PNG ├── Capture1.PNG ├── Final ├── Change in state.png ├── input.png ├── m1,m2,11.png ├── m1,m2.png ├── n = 5 3d.png ├── n = 5.png ├── output.png └── untitled.png ├── N = 20 2d.png ├── N = 20 3d.png ├── N=3 (jack).fig ├── README.md ├── S03examples.m ├── check_ABQR.m ├── constraint_mats.m ├── cost_mats.m ├── main.m ├── main1.m ├── my plot.PNG ├── mynewscript.m ├── myscript_33.m ├── myscript_4.m ├── myscript_5.m ├── myscript_6.m ├── n = 10 2d.png ├── n = 10 3d.png ├── n = 5 2d.png ├── n = 5 3d.png ├── predict_mats.m ├── qx0.PNG ├── sync_session_11_7.m └── sync_session_16_8.m /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /All .png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All .png -------------------------------------------------------------------------------- /All 1 3D.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All 1 3D.png -------------------------------------------------------------------------------- /All 1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All 1.png -------------------------------------------------------------------------------- /All 3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All 3d.png -------------------------------------------------------------------------------- /All Final 1 (different x2 x3) 1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All Final 1 (different x2 x3) 1.png -------------------------------------------------------------------------------- /All Final 1 (different x2 x3).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All Final 1 (different x2 x3).png -------------------------------------------------------------------------------- /All Final.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/All Final.png -------------------------------------------------------------------------------- /Both N = 3 (feasibility Region).png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Both N = 3 (feasibility Region).png -------------------------------------------------------------------------------- /Capture.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Capture.PNG -------------------------------------------------------------------------------- /Capture1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Capture1.PNG -------------------------------------------------------------------------------- /Final/Change in state.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/Change in state.png -------------------------------------------------------------------------------- /Final/input.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/input.png -------------------------------------------------------------------------------- /Final/m1,m2,11.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/m1,m2,11.png -------------------------------------------------------------------------------- /Final/m1,m2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/m1,m2.png -------------------------------------------------------------------------------- /Final/n = 5 3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/n = 5 3d.png -------------------------------------------------------------------------------- /Final/n = 5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/n = 5.png -------------------------------------------------------------------------------- /Final/output.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/output.png -------------------------------------------------------------------------------- /Final/untitled.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/Final/untitled.png -------------------------------------------------------------------------------- /N = 20 2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/N = 20 2d.png -------------------------------------------------------------------------------- /N = 20 3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/N = 20 3d.png -------------------------------------------------------------------------------- /N=3 (jack).fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/N=3 (jack).fig -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Model-Predictiive-Control--MPC- 2 | Frequency Control in a power system 3 | 4 | Abstract— A constrained dual-mode Linear Quadratic Model Predictive Control (LQ-MPC) was designed to adjust the reference power, 〖∆p〗^ref in response to frequency deviation and meet the desired specification. Relevant MPC parameters were evaluated to ensure guaranteed stability and recursive feasibility. 5 | -------------------------------------------------------------------------------- /S03examples.m: -------------------------------------------------------------------------------- 1 | %% S03EXAMPLES.M 2 | % 3 | % Script to support ACS6116 synchronous session 3. 4 | % 5 | % This is rough and uncommented code -- use at your own risk! I take no 6 | % responsibility for errors. 7 | % 8 | % (c) P. Trodden, 2021. 9 | 10 | %% 1: Q, R ≻ 0 and (A, B) stabilizable. 11 | display('Example 1') 12 | A = [1 1; 0 0.9]; 13 | B = [1; 0]; 14 | Q = eye(2); 15 | R = 1; 16 | 17 | check_ABQR(A,B,Q,R); 18 | 19 | %% 2: Q ≽ 0, R ≻ 0, (A, B) reachable, but (Q1/2, A) not detectable 20 | display('Example 2') 21 | A = [1 1; 0 1]; 22 | B = [1; 1]; 23 | Q = 0*eye(2); 24 | R = 1; 25 | 26 | check_ABQR(A,B,Q,R); 27 | 28 | %% 3: Q ≽ 0, R ≻ 0, (A, B) stabilizable, and (Q1/2, A) detectable 29 | display('Example 3') 30 | A = [0.9 1; 0 0.9]; 31 | B = [1; 0]; 32 | Q = 0*eye(2); 33 | R = 1; 34 | 35 | check_ABQR(A,B,Q,R); 36 | 37 | %% 4: Q, R ≻ 0, (A, B) not stabilizable 38 | display('Example 4') 39 | A = [1 1; 0 1.1]; 40 | B = [1; 0]; 41 | Q = eye(2); 42 | R = 1; 43 | 44 | check_ABQR(A,B,Q,R); 45 | 46 | %% 5: Q, R ≽ 0, (A, B) reachable 47 | display('Example 5') 48 | A = [0.9 1; 0 0.9]; 49 | B = [0; 1]; 50 | Q = 0*eye(2); 51 | R = 0*1; 52 | 53 | check_ABQR(A,B,Q,R); 54 | 55 | %% 6: Q ≽ 0, R ≻ 0, (A, B) controllable, and (Q1/2, A) detectable 56 | display('Example 6') 57 | A = [1 1; 0 0]; 58 | B = [1; 0]; 59 | C = [0 1]; 60 | Q = C'*C; 61 | R = 1; 62 | 63 | check_ABQR(A,B,Q,R); 64 | 65 | %% function for checking properties 66 | check_ABQR(A,B,Q,R); 67 | 68 | function check_ABQR(A,B,Q,R) 69 | 70 | if min(eig(R)) > 0 71 | display('R is positive definite') 72 | elseif min(eig(R)) == 0 73 | display('R is positive semidefinite -- DLQR.M will fail!') 74 | else 75 | display('R is not positive semidefinite -- DLQR.M will fail!') 76 | end 77 | 78 | if min(eig(Q)) > 0 79 | display('Q is positive definite') 80 | elseif min(eig(Q)) == 0 81 | display('Q is positive semidefinite') 82 | % check detectability 83 | [ABAR,BBAR,CBAR,T,K] = obsvf(A,B,sqrtm(Q)); 84 | nobs = size(A,2) - sum(K); 85 | if max(abs(eig(ABAR(1:nobs,1:nobs)))) == 0 86 | display('(Q^0.5,A) is constructible') 87 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 88 | display('(Q^0.5,A) is detectable') 89 | else 90 | display('(Q^0.5,A) is not detectable -- DLQR.M will fail!') 91 | end 92 | 93 | 94 | else 95 | display('Q is not positive semidefinite -- DQLR.M will fail!') 96 | end 97 | 98 | r = rank(ctrb(A,B)); 99 | if r == 2 100 | display('(A,B) is reachable') 101 | else 102 | display('(A,B) is not reachable') 103 | % check stabilizability 104 | [ABAR,BBAR,CBAR,T,K] = ctrbf(A,B,sqrtm(Q)); 105 | nrea = size(A,2) - sum(K); 106 | if max(abs(eig(ABAR(1:nrea,1:nrea)))) == 0 107 | display('(A,B) is controllable') 108 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 109 | display('(A,B) is stabilizable') 110 | else 111 | display('(A,B) is not stabilizable -- DLQR.M will fail!') 112 | end 113 | end 114 | 115 | end -------------------------------------------------------------------------------- /check_ABQR.m: -------------------------------------------------------------------------------- 1 | %% function for checking properties 2 | 3 | function check_ABQR(A,B,Q,R) 4 | 5 | if min(eig(R)) > 0 6 | display('R is positive definite') 7 | elseif min(eig(R)) == 0 8 | display('R is positive semidefinite -- DLQR.M will fail!') 9 | else 10 | display('R is not positive semidefinite -- DLQR.M will fail!') 11 | end 12 | 13 | if min(eig(Q)) > 0 14 | display('Q is positive definite') 15 | elseif min(eig(Q)) == 0 16 | display('Q is positive semidefinite') 17 | % check detectability 18 | [ABAR,BBAR,CBAR,T,K] = obsvf(A,B,sqrtm(Q)); 19 | nobs = size(A,2) - sum(K); 20 | if max(abs(eig(ABAR(1:nobs,1:nobs)))) == 0 21 | display('(Q^0.5,A) is constructible') 22 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 23 | display('(Q^0.5,A) is detectable') 24 | else 25 | display('(Q^0.5,A) is not detectable -- DLQR.M will fail!') 26 | end 27 | 28 | 29 | else 30 | display('Q is not positive semidefinite -- DQLR.M will fail!') 31 | end 32 | 33 | r = rank(ctrb(A,B)); 34 | if r == 2 35 | display('(A,B) is reachable') 36 | else 37 | display('(A,B) is not reachable') 38 | % check stabilizability 39 | [ABAR,BBAR,CBAR,T,K] = ctrbf(A,B,sqrtm(Q)); 40 | nrea = size(A,2) - sum(K); 41 | if max(abs(eig(ABAR(1:nrea,1:nrea)))) == 0 42 | display('(A,B) is controllable') 43 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 44 | display('(A,B) is stabilizable') 45 | else 46 | display('(A,B) is not stabilizable -- DLQR.M will fail!') 47 | end 48 | end 49 | 50 | end -------------------------------------------------------------------------------- /constraint_mats.m: -------------------------------------------------------------------------------- 1 | function [Pc, qc, Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf) 2 | % 3 | % CONSTRAINTS_MATS.M returns the MPC constraints matrices for a system that 4 | % is subject to constraints 5 | % 6 | % Pu*u(k+j|k) <= qu 7 | % Px*x(k+j|k) <= qx 8 | % Pxf*x(k+N|k) <= qxf 9 | % 10 | % That is, the matrices Pc, qc and Sc from 11 | % 12 | % Pc*U(k) <= qc + Sc*x(k) 13 | % 14 | % USAGE: 15 | % 16 | % [Pc,qc,Sc] = constraint_mats(F,G,Pu,qu,Px,qx,Pxf,qxf) 17 | % 18 | % where F, G are the prediction matrices obtained from PREDICT_MATS.M 19 | % 20 | % P. Trodden, 2017. 21 | 22 | % input dimension 23 | m = size(Pu,2); 24 | 25 | % state dimension 26 | n = size(F,2); 27 | 28 | % horizon length 29 | N = size(F,1)/n; 30 | 31 | % number of input constraints 32 | ncu = numel(qu); 33 | 34 | % number of state constraints 35 | ncx = numel(qx); 36 | 37 | % number of terminal constraints 38 | ncf = numel(qxf); 39 | 40 | % if state constraints exist, but terminal ones do not, then extend the 41 | % former to the latter 42 | if ncf == 0 && ncx > 0 43 | Pxf = Px; 44 | qxf = qx; 45 | ncf = ncx; 46 | end 47 | 48 | %% Input constraints 49 | 50 | % Build "tilde" (stacked) matrices for constraints over horizon 51 | Pu_tilde = kron(eye(N),Pu); 52 | qu_tilde = kron(ones(N,1),qu); 53 | Scu = zeros(ncu*N,n); 54 | 55 | %% State constraints 56 | 57 | % Build "tilde" (stacked) matrices for constraints over horizon 58 | Px0_tilde = [Px; zeros(ncx*(N-1) + ncf,n)]; 59 | if ncx > 0 60 | Px_tilde = [kron(eye(N-1),Px) zeros(ncx*(N-1),n)]; 61 | else 62 | Px_tilde = zeros(ncx,n*N); 63 | end 64 | Pxf_tilde = [zeros(ncf,n*(N-1)) Pxf]; 65 | Px_tilde = [zeros(ncx,n*N); Px_tilde; Pxf_tilde]; 66 | qx_tilde = [kron(ones(N,1),qx); qxf]; 67 | 68 | %% Final stack 69 | if isempty(Px_tilde) 70 | Pc = Pu_tilde; 71 | qc = qu_tilde; 72 | Sc = Scu; 73 | else 74 | % eliminate x for final form 75 | Pc = [Pu_tilde; Px_tilde*G]; 76 | qc = [qu_tilde; qx_tilde]; 77 | Sc = [Scu; -Px0_tilde - Px_tilde*F]; 78 | end 79 | -------------------------------------------------------------------------------- /cost_mats.m: -------------------------------------------------------------------------------- 1 | function [H,L,M] = cost_mats(F,G,Q,R,P) 2 | % 3 | % COST_MATS.M returns the MPC cost matrices for a system 4 | % 5 | % X = F*x + G*U 6 | % 7 | % with finite-horizon objective function 8 | % 9 | % J = 0.5*sum_{j=0}^{N-1} x(j)'*Q*x(j) + u(j)'*R*u(j) + x(N)'*P*x(N) 10 | % 11 | % That is, the matrices H, L and M in 12 | % 13 | % J = 0.5*U'*H*U + x'*L'*U + x'*M*x 14 | % 15 | % USAGE: 16 | % 17 | % [H,L,M] = cost_mats(F,G,Q,R,P) 18 | % 19 | % P. Trodden, 2015. 20 | 21 | % get dimensions 22 | n = size(F,2); 23 | N = size(F,1)/n; 24 | 25 | % diagonalize Q and R 26 | Qd = kron(eye(N-1),Q); 27 | Qd = blkdiag(Qd,P); 28 | Rd = kron(eye(N),R); 29 | 30 | % Hessian 31 | H = 2*G'*Qd*G + 2*Rd; 32 | 33 | % Linear term 34 | L = 2*G'*Qd*F; 35 | 36 | % Constant term 37 | M = F'*Qd*F + Q; 38 | 39 | % make sure the Hessian is symmetric 40 | H=(H+H')/2; 41 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | % Author: Awabullah Syed 2 | % Date: April 28th 2021 3 | % MPC contoller for Frequency control in a power system 4 | % Description: the primary frequency control loop present in the system 5 | % does not offer adequate control. therefore, the aim is to design a 6 | % secondary freqency control loop that will adjust the reference power, 7 | % delta_p^ref 8 | %------------------------Tuning MPC--------------------------------------% 9 | x0 = [0;0;0.29]; %choose any value 10 | Ts = 0.1; 11 | N = 5; % Increasing Horizon, reduces cost value, J 12 | Q_design = 1000; 13 | % R_design = 10; 14 | % ----------------------------System model paramters---------------------% 15 | M_sys =10; 16 | D_sys = 0.8; 17 | T_t = 0.5; 18 | T_g = 0.2; 19 | R_sys = 0.1; 20 | R = 0.25; 21 | %-------------------System Matrices---------------------------------------% 22 | r = 0.3; %reference input (can be set to any reasonable value) 23 | syms A_sys B_sys C_sys D 24 | A_sys = [-D_sys/M_sys 1/M_sys 0; 25 | 0 -1/T_t 1/T_t; 26 | -1/(R_sys*T_g) 0 -1/T_g;]; %A, Dynamic Matrix 27 | B_sys = [0 ; 0 ; 1/T_g]; % B matrix of the system 28 | C_sys = [50 0 0]; D = 0; 29 | sys = ss(A_sys,B_sys,C_sys,D_sys); %State space 30 | sysd = c2d(sys,Ts,'zoh'); %Discrete 31 | A = sysd.A; 32 | B = sysd.B; 33 | C = sysd.C; 34 | D = sysd.D; 35 | %----------------------------Dimensions----------------------------------% 36 | n = size(A,1); 37 | m = size(B,2); 38 | %--------------------------Cost Matrices---------------------------------% 39 | Q = C'*C*Q_design; 40 | check_ABQR(A,B,Q,R_sys); %if stable without terminal cost 41 | check_ABQR(A,B,Q,R); %if stable without terminal cost 42 | % Q = (C'*C)*Q; % Q^(1/2) % 43 | %-----------------------------K -----------------------------------------% 44 | %eig_con = [-1 -1.23 -5.5]; %Desired Eigenvalues for mode 2 45 | K = -acker(A,B,[0 0 0]); % Gain k 46 | P = dlyap((A+B*K)',Q+K'*R*K); % Lynaponouv (Q & R) - Tuning Parameter) 47 | %--------------------------Constraints------------------------------------% 48 | % input constant 49 | umin = -0.5; 50 | umax = 0.5; 51 | Pu = [eye(m) ; -eye(m)]; 52 | Qu = [umax;-umin]; 53 | % State Constraint 54 | xmin = -0.5; 55 | xmax = 0.5; 56 | % px = [eye(n); eye(n)]; 57 | px = [C;-C]; %[eye(3).*C; -eye(3).*C]; 58 | % qx = [xmax ; xmin]; 59 | qx = [xmax ; -xmin]; %[ones(3,1)*xmax; -ones(3,1)*xmin]; 60 | %---------------------- mode 1 extend------------------------------------% 61 | Maux = []; %Extended Mode-1 constraints based on K 62 | for i =0:n-1 %N 63 | Maux = [Maux;(A+B*K)^(i)]; 64 | end 65 | Mm = kron(eye(n),[px; Pu*K]); % N 66 | pxf = Mm*Maux; 67 | qxf = kron(ones(3,1),[qx; Qu]); %qx; 68 | %-------------------MPC Prediction Matrices-------------------------------% 69 | % prediction matrices 70 | [F,G] = predict_mats(A,B,N); 71 | [H, L, M] = cost_mats(F,G,Q,R,P); %Cost Matrix 72 | [Pc,qc,Sc] = constraint_mats(F,G,Pu,Qu,px,qx,pxf,qxf); 73 | %-------------------------Checking Stability----------------------------% 74 | % optimal policy 75 | S = -H\L; 76 | % optimal feedback law 77 | KN = S(1,:); 78 | % form closed-loop system 79 | Phi = A+B*KN; 80 | % stability check 81 | rho = max(abs(eig(Phi))); 82 | if rho >= 1 83 | display('System with terminal P is not stable') 84 | else 85 | display('System with terminal P is stable') 86 | end 87 | %------------------Mode 2------------------------------------------------% 88 | % Obtain a plot of x2(k+i|k) versus x1(k+i|k) 89 | XOpt = (F + G*S)*x0; 90 | % Xopt contains x(k+1|k), x(k+2|k), ..., x(k+N|k) 91 | Xopt = [x0; XOpt]; 92 | 93 | % extract x1 and x2 elements 94 | X1opt = Xopt(1:3:end,:); 95 | X2opt = Xopt(2:3:end,:); 96 | X3opt = Xopt(3:3:end,:); 97 | 98 | % Xoptodd = Xopt(1:2:end,:); 99 | % Xopteven = Xopt(2:2:end,:); 100 | 101 | % x(k+N|k) is final 2 elements of Xopt 102 | xN = Xopt(end-n+1:end,:); 103 | % x(k+N+1|k) = A*x(k+N|k0) + B*K*x(k+N|k) = (A+B*K)*x(k+N|k) 104 | % x(k+N+2|k) = (A+B*K)*x(k+N+1|k) = (A+B*K)^2 * x(k+N|k) 105 | % x(k+N+i|k) = (A+B*K)^i * x(k+N|k) 106 | % mode-2 107 | 108 | N2 = 5; % Horizon Length 109 | 110 | [F2, G2] = predict_mats(A+B*K,B,N2); 111 | % F2 is [(A+BK); (A+BK1)^2; ...; (A+BK)^N2] 112 | % mode 2 state predictions 113 | Xopt2 = [xN; F2*xN;]; 114 | % extract x1 and x2 elements 115 | X1opt2 = Xopt2(1:3:end,:); 116 | X2opt2 = Xopt2(2:3:end,:); 117 | X3opt2 = Xopt2(3:3:end,:); 118 | 119 | % Xopt2odd = Xopt2(1:2:end,:); 120 | % Xopt2even = Xopt2(2:2:end,:); 121 | 122 | figure 123 | plot3(X1opt,X2opt,X3opt,'bo-',X1opt2,X2opt2,X3opt2,'r*-'); 124 | legend({'mode-1','mode-2'}); 125 | xlabel('x1'); ylabel('x2'); zlabel('x3') 126 | title ('MPC, Mode-1 | Mode-2') 127 | 128 | % figure 129 | % plot(Xoptodd,Xopteven,'bo-'),hold on 130 | % plot(Xopt2odd,Xopt2even,'r*-'); hold off 131 | % title ('States, Horizon = 3'); 132 | % xlabel('x1'); 133 | % ylabel('x2') 134 | % legend({'mode 1','mode 2'},'Location','southeast') 135 | %-----------------Constrained Control law u(k)Kn(x(k))-------------------% 136 | nk = 15; %Simulation steps 137 | x = x0; 138 | xss = zeros(n,nk); 139 | uss = zeros(m,nk); 140 | opt = optimoptions('quadprog','ConstraintTolerance',1e-25); 141 | 142 | for k = 1:nk+1 %( Addition of ys) 143 | % store x 144 | xss(:,k) = x; 145 | % solve the MPC problem 146 | [Uopt, fval, flag] = quadprog(H,L*x,Pc,qc+Sc*x,[],[],[],[],[],opt); 147 | if flag < 1 148 | disp(['Optimization infeasible at k = ' num2str(k)]) 149 | break 150 | end 151 | % extract the first control 152 | u = Uopt(1:m); 153 | % store u 154 | uss(:,k) = u; 155 | % apply to the plant 156 | x = A*x + B*u; 157 | ys(:,k) = C*xss(:,k); 158 | end 159 | figure(2) %Output / States Changes 160 | plot((0:k-1),xss(:,1:k)'); hold on 161 | plot(0:k-1,repmat(0.01,1,k),'c--') 162 | hold on 163 | plot(0:k-1,repmat(-0.01,1,k),'c--') 164 | hold off 165 | % plot ((0:k-1),xmin); 166 | % plot ((0:k-1),xmax); 167 | title('Change in State per time step (k)') 168 | xlabel('Time step k'); 169 | ylabel('States') 170 | legend('\Deltaw','\Deltap^m','\Deltap^v') ; 171 | % legend('Output','xmin constraint','xmax constraint'); 172 | hold off 173 | 174 | figure(3) % Input 175 | stairs((0:k-1),uss(:,1:k)'); hold on 176 | % constraints 177 | plot(0:k-1,repmat(0.5,1,k),'c--') 178 | 179 | plot(0:k-1,repmat(-0.5,1,k),'c--') 180 | hold off 181 | % plot((0:k-1),umin,'o') 182 | % plot ((0:k-1),umax,'o') 183 | title ('Input Step Changes') 184 | xlabel('Time step k') 185 | ylabel('Change in Reference Power '); 186 | legend('Input','umax','umin');hold off 187 | 188 | figure (4) % Output 189 | stairs([0:k-1],ys(:,1:k)') 190 | hold on 191 | % constraints 192 | plot(0:k-1,repmat(0.01,1,k),'c--') 193 | hold on 194 | plot(0:k-1,repmat(-0.01,1,k),'c--') 195 | hold off 196 | xlabel('Time Step (k) ') 197 | ylabel('y (Hz)') 198 | title('Output signal per time step') 199 | %% 200 | PxN = pxf; %check these 201 | qxN = qxf; 202 | 203 | % build MPC problem matrices 204 | [F, G] = predict_mats(A,B,N); 205 | [H, L, M] = cost_mats(F,G,Q,R,P); 206 | [Pc,qc,Sc] = constraint_mats(F,G,Pu,Qu,px,qx,PxN,qxN); 207 | 208 | figure(6) %feasbility region 209 | hold on 210 | % [x1,x2,x3] = meshgrid(-0.5:0.01:0.5); 211 | for x1 = -0.01:0.001:0.01 212 | for x2 = -10:0.1:10 213 | for x3 = -10:0.1:10 214 | x = [x1; x2;x3]; 215 | % solve the MPC problem 216 | [Uopt, fval, flag] = quadprog(H,L*x,Pc,qc+Sc*x); 217 | if flag >= 1 218 | plot3(x1,x2,x3,'b.') 219 | end 220 | end 221 | 222 | end 223 | end 224 | xlabel('x1'); ylabel('x2'); zlabel('x3'); 225 | title('Operating Region for N = 5'); 226 | % surf(x1,x2,x3,'b.'); 227 | % A = [ 1 1 ; 0 1]; %lambda(A+BK) = 0 228 | % B = [0.5;1]; 229 | % k = -[1 1.5]; 230 | % K = A+B*k 231 | % eig(K) 232 | %% function for checking properties 233 | check_ABQR(A,B,Q,R); 234 | 235 | function check_ABQR(A,B,Q,R) 236 | 237 | if min(eig(R)) > 0 238 | display('R is positive definite') 239 | elseif min(eig(R)) == 0 240 | display('R is positive semidefinite -- DLQR.M will fail!') 241 | else 242 | display('R is not positive semidefinite -- DLQR.M will fail!') 243 | end 244 | 245 | if min(eig(Q)) > 0 246 | display('Q is positive definite') 247 | elseif min(eig(Q)) == 0 248 | display('Q is positive semidefinite') 249 | % check detectability 250 | [ABAR,BBAR,CBAR,T,K] = obsvf(A,B,sqrtm(Q)); 251 | nobs = size(A,2) - sum(K); 252 | if max(abs(eig(ABAR(1:nobs,1:nobs)))) == 0 253 | display('(Q^0.5,A) is constructible') 254 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 255 | display('(Q^0.5,A) is detectable') 256 | else 257 | display('(Q^0.5,A) is not detectable -- DLQR.M will fail!') 258 | end 259 | 260 | 261 | else 262 | display('Q is not positive semidefinite -- DQLR.M will fail!') 263 | end 264 | 265 | r = rank(ctrb(A,B)); 266 | if r == 2 267 | display('(A,B) is reachable') 268 | else 269 | display('(A,B) is not reachable') 270 | % check stabilizability 271 | [ABAR,BBAR,CBAR,T,K] = ctrbf(A,B,sqrtm(Q)); 272 | nrea = size(A,2) - sum(K); 273 | if max(abs(eig(ABAR(1:nrea,1:nrea)))) == 0 274 | display('(A,B) is controllable') 275 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 276 | display('(A,B) is stabilizable') 277 | else 278 | display('(A,B) is not stabilizable -- DLQR.M will fail!') 279 | end 280 | end 281 | 282 | end -------------------------------------------------------------------------------- /main1.m: -------------------------------------------------------------------------------- 1 | % Author: Awabullah Syed 2 | % Date: April 28th 2021 3 | % MPC contoller for Frequency control in a power system 4 | % Description: the primary frequency control loop present in the system 5 | % does not offer adequate control. therefore, the aim is to design a 6 | % secondary freqency control loop that will adjust the reference power, 7 | % delta_p^ref 8 | %------------------------Tuning MPC--------------------------------------% 9 | x0 = [0;0;0.29]; %choose any value 10 | Ts = 0.1; 11 | N = 3;% Increasing Horizon, reduces cost value, J 12 | N_1 = 4; 13 | N_2 = 5; 14 | N_3 = 10; 15 | Q_design = 20; 16 | R_design = 100; 17 | % ----------------------------System model paramters---------------------% 18 | M_sys =10; 19 | D_sys = 0.8; 20 | T_t = 0.5; 21 | T_g = 0.2; 22 | R_sys = 0.1; 23 | R = 100; 24 | %-------------------System Matrices---------------------------------------% 25 | r = 0.3; %reference input (can be set to any reasonable value) 26 | syms A_sys B_sys C_sys D 27 | A_sys = [-D_sys/M_sys 1/M_sys 0; 28 | 0 -1/T_t 1/T_t; 29 | -1/(R_sys*T_g) 0 -1/T_g;]; %A, Dynamic Matrix 30 | B_sys = [0 ; 0 ; 1/T_g]; % B matrix of the system 31 | C_sys = [50 0 0]; D = 0; 32 | sys = ss(A_sys,B_sys,C_sys,D_sys); %State space 33 | sysd = c2d(sys,Ts,'zoh'); %Discrete 34 | A = sysd.A; 35 | B = sysd.B; 36 | C = sysd.C; 37 | D = sysd.D; 38 | %----------------------------Dimensions----------------------------------% 39 | n = size(A,1); 40 | m = size(B,2); 41 | %--------------------------Cost Matrices---------------------------------% 42 | Q = C'*C % *Q_design; 43 | check_ABQR(A,B,Q,R_sys); %if stable without terminal cost 44 | check_ABQR(A,B,Q,R); %if stable without terminal cost 45 | % Q = (C'*C)*Q; % Q^(1/2) % 46 | %-----------------------------K -----------------------------------------% 47 | %eig_con = [-1 -1.23 -5.5]; %Desired Eigenvalues for mode 2 48 | K = -acker(A,B,[0 0 0]); % Gain k 49 | P = dlyap((A+B*K)',Q+K'*R*K); % Lynaponouv (Q & R) - Tuning Parameter) 50 | %--------------------------Constraints------------------------------------% 51 | % input constant 52 | umin = -0.5; 53 | umax = 0.5; 54 | Pu = [eye(m) ; -eye(m)]; 55 | Qu = [umax;-umin]; 56 | % State Constraint 57 | xmin = -0.5; 58 | xmax = 0.5; 59 | % px = [eye(n); eye(n)]; 60 | px = [C;-C]; %[eye(3).*C; -eye(3).*C]; 61 | % qx = [xmax ; xmin]; 62 | qx = [xmax ; -xmin]; %[ones(3,1)*xmax; -ones(3,1)*xmin]; 63 | %---------------------- mode 1 extend------------------------------------% 64 | Maux = []; %Extended Mode-1 constraints based on K 65 | for i =0:n-1 %N 66 | Maux = [Maux;(A+B*K)^(i)]; 67 | end 68 | Mm = kron(eye(n),[px; Pu*K]); % N 69 | pxf = Mm*Maux; 70 | qxf = kron(ones(3,1),[qx; Qu]); %qx; 71 | %-------------------MPC Prediction Matrices-------------------------------% 72 | % prediction matrices 73 | [F,G] = predict_mats(A,B,N); 74 | [H, L, M] = cost_mats(F,G,Q,R,P); %Cost Matrix 75 | [Pc,qc,Sc] = constraint_mats(F,G,Pu,Qu,px,qx,pxf,qxf); 76 | % Prediction matrices (N_1) 77 | [F_1,G_1] = predict_mats(A,B,N_1); 78 | [H_1, L_1, M_1] = cost_mats(F_1,G_1,Q,R,P); %Cost Matrix 79 | [Pc_1,qc_1,Sc_1] = constraint_mats(F_1,G_1,Pu,Qu,px,qx,pxf,qxf); 80 | % Prediction matrices (N_2) 81 | [F_2,G_2] = predict_mats(A,B,N_2); 82 | [H_2, L_2, M_2] = cost_mats(F_2,G_2,Q,R,P); %Cost Matrix 83 | [Pc_2,qc_2,Sc_2] = constraint_mats(F_2,G_2,Pu,Qu,px,qx,pxf,qxf); 84 | % Prediction matrices (N_3) 85 | [F_3,G_3] = predict_mats(A,B,N_3); 86 | [H_3, L_3, M_3] = cost_mats(F_3,G_3,Q,R,P); %Cost Matrix 87 | [Pc_3,qc_3,Sc_3] = constraint_mats(F_3,G_3,Pu,Qu,px,qx,pxf,qxf); 88 | %-------------------------Checking Stability----------------------------% 89 | % optimal policy 90 | S = -H\L; 91 | S_1 = -H_1\L_1; 92 | S_2 = -H_2\L_2; 93 | S_3 = -H_3\L_3; 94 | % optimal feedback law 95 | KN = S(1,:); 96 | KN_1 = S_1(1,:); 97 | KN_2 = S_2(1,:); 98 | KN_3 = S_3(1,:); 99 | % form closed-loop system 100 | Phi = A+B*KN; 101 | % not needed to check 102 | % stability check 103 | rho = max(abs(eig(Phi))); 104 | if rho >= 1 105 | display('System with terminal P is not stable') 106 | else 107 | display('System with terminal P is stable') 108 | end 109 | %------------------Mode 2------------------------------------------------% 110 | % Obtain a plot of x2(k+i|k) versus x1(k+i|k) 111 | XOpt = (F + G*S)*x0; 112 | XOpt_1 = (F_1 + G_1*S_1)*x0; 113 | XOpt_2 = (F_2 + G_2*S_2)*x0; 114 | XOpt_3 = (F_3 + G_3*S_3)*x0; 115 | % Xopt contains x(k+1|k), x(k+2|k), ..., x(k+N|k) 116 | Xopt = [x0; XOpt]; 117 | Xopt_1 = [x0; XOpt_1]; 118 | Xopt_2 = [x0; XOpt_2]; 119 | Xopt_3 = [x0; XOpt_3]; 120 | 121 | % extract x1 and x2 elements 122 | X1opt = Xopt(1:3:end,:); 123 | X2opt = Xopt(2:3:end,:); 124 | X3opt = Xopt(3:3:end,:); 125 | 126 | X1opt_1 = Xopt_1(1:3:end,:); 127 | X2opt_1 = Xopt_1(2:3:end,:); 128 | X3opt_1 = Xopt_1(3:3:end,:); 129 | 130 | X1opt_2 = Xopt_2(1:3:end,:); 131 | X2opt_2 = Xopt_2(2:3:end,:); 132 | X3opt_2 = Xopt_2(3:3:end,:); 133 | 134 | X1opt_3 = Xopt_3(1:3:end,:); 135 | X2opt_3 = Xopt_3(2:3:end,:); 136 | X3opt_3 = Xopt_3(3:3:end,:); 137 | 138 | % Xoptodd = Xopt(1:2:end,:); 139 | % Xopteven = Xopt(2:2:end,:); 140 | 141 | % x(k+N|k) is final 2 elements of Xopt 142 | xN = Xopt(end-n+1:end,:); 143 | xN_1 = Xopt_1(end-n+1:end,:); 144 | xN_2 = Xopt_2(end-n+1:end,:); 145 | xN_3 = Xopt_3(end-n+1:end,:); 146 | % x(k+N+1|k) = A*x(k+N|k0) + B*K*x(k+N|k) = (A+B*K)*x(k+N|k) 147 | % x(k+N+2|k) = (A+B*K)*x(k+N+1|k) = (A+B*K)^2 * x(k+N|k) 148 | % x(k+N+i|k) = (A+B*K)^i * x(k+N|k) 149 | % mode-2 150 | 151 | N2 = 3; % Horizon Length 152 | N2_1 = 4; 153 | N2_2 = 5; 154 | N2_3 = 10; 155 | 156 | [F2, G2] = predict_mats(A+B*K,B,N2); 157 | [F2_1, G2_1] = predict_mats(A+B*K,B,N2_1); 158 | [F2_2, G2_2] = predict_mats(A+B*K,B,N2_2); 159 | [F2_3, G2_3] = predict_mats(A+B*K,B,N2_3); 160 | % F2 is [(A+BK); (A+BK1)^2; ...; (A+BK)^N2] 161 | % mode 2 state predictions 162 | Xopt2 = [xN; F2*xN;]; 163 | Xopt2_1 = [xN_1; F2_1*xN_1;]; 164 | Xopt2_2 = [xN_2; F2_2*xN_2;]; 165 | Xopt2_3 = [xN_3; F2_3*xN_3;]; 166 | 167 | % extract x1 and x2 elements 168 | X1opt2 = Xopt2(1:3:end,:); 169 | X2opt2 = Xopt2(2:3:end,:); 170 | X3opt2 = Xopt2(3:3:end,:); 171 | 172 | X1opt2_1 = Xopt2_1(1:3:end,:); 173 | X2opt2_1 = Xopt2_1(2:3:end,:); 174 | X3opt2_1 = Xopt2_1(3:3:end,:); 175 | 176 | X1opt2_2 = Xopt2_2(1:3:end,:); 177 | X2opt2_2 = Xopt2_2(2:3:end,:); 178 | X3opt2_2 = Xopt2_2(3:3:end,:); 179 | 180 | X1opt2_3 = Xopt2_3(1:3:end,:); 181 | X2opt2_3 = Xopt2_3(2:3:end,:); 182 | X3opt2_3 = Xopt2_3(3:3:end,:); 183 | 184 | % Xopt2odd = Xopt2(1:2:end,:); 185 | % Xopt2even = Xopt2(2:2:end,:); 186 | 187 | figure % (Didnt work) 188 | % plot3(X1opt,X2opt,X3opt,'bo-',X1opt2,X2opt2,X3opt2,'r*-'); 189 | plot3(X1opt,X2opt,X3opt,X1opt2,X2opt2,X3opt2); hold on 190 | plot3(X1opt_1,X2opt_1,X3opt_1,X1opt2_1,X2opt2_1,X3opt2_1); 191 | plot3(X1opt_2,X2opt_2,X3opt_2,X1opt2_2,X2opt2_2,X3opt2_2); 192 | legend({'N = 3','N = 5','N = 10'}); 193 | xlabel('x1'); ylabel('x2'); 194 | % title ('MPC, Mode-1 | Mode-2'); 195 | title ('MPC, N = 3 | 5 | 10'); hold off 196 | 197 | % figure 198 | % plot(Xoptodd,Xopteven,'bo-'),hold on 199 | % plot(Xopt2odd,Xopt2even,'r*-'); hold off 200 | % title ('States, Horizon = 3'); 201 | % xlabel('x1'); 202 | % ylabel('x2') 203 | % legend({'mode 1','mode 2'},'Location','southeast') 204 | %% 205 | %-----------------Constrained Control law u(k)Kn(x(k))-------------------% 206 | nk = 15; %Simulation steps 207 | x = x0; 208 | x1 = x0; 209 | x2 = x0; 210 | x3 = x0; 211 | 212 | xss = zeros(n,nk); 213 | xss1 = zeros(n,nk); 214 | xss2 = zeros(n,nk); 215 | xss3 = zeros(n,nk); 216 | 217 | uss = zeros(m,nk); 218 | uss1 = zeros(m,nk); 219 | uss2 = zeros(m,nk); 220 | uss3 = zeros(m,nk); 221 | opt = optimoptions('quadprog','ConstraintTolerance',1e-25); 222 | 223 | for k = 1:nk+1 %( Addition of ys) 224 | % store x 225 | xss(:,k) = x; 226 | % solve the MPC problem 227 | [Uopt, fval, flag] = quadprog(H,L*x,Pc,qc+Sc*x,[],[],[],[],[],opt); 228 | if flag < 1 229 | disp(['Optimization infeasible at k = ' num2str(k)]) 230 | break 231 | end 232 | % extract the first control 233 | u = Uopt(1:m); 234 | % store u 235 | uss(:,k) = u; 236 | % apply to the plant 237 | x = A*x + B*u; 238 | ys(:,k) = C*xss(:,k); 239 | end 240 | for k1 = 1:nk+1 %( Addition of ys) 241 | % store x 242 | xss1(:,k1) = x1; 243 | % solve the MPC problem 244 | [Uopt1, fval1, flag1] = quadprog(H_1,L_1*x1,Pc_1,qc_1+Sc_1*x1,[],[],[],[],[],opt); 245 | if flag1 < 1 246 | disp(['Optimization infeasible at k = ' num2str(k)]) 247 | break 248 | end 249 | % extract the first control 250 | u1 = Uopt1(1:m); 251 | % store u 252 | uss1(:,k1) = u1; 253 | % apply to the plant 254 | x1 = A*x1 + B*u1; 255 | ys1(:,k1) = C*xss1(:,k1); 256 | end 257 | for k2 = 1:nk+1 %( Addition of ys) 258 | % store x 259 | xss2(:,k2) = x2; 260 | % solve the MPC problem 261 | [Uopt2, fval2, flag2] = quadprog(H_2,L_2*x2,Pc_2,qc_2+Sc_2*x2,[],[],[],[],[],opt); 262 | if flag2 < 1 263 | disp(['Optimization infeasible at k = ' num2str(k)]) 264 | break 265 | end 266 | % extract the first control 267 | u2 = Uopt2(1:m); 268 | % store u 269 | uss2(:,k2) = u2; 270 | % apply to the plant 271 | x2 = A*x2 + B*u2; 272 | ys2(:,k2) = C*xss2(:,k2); 273 | end 274 | for k3 = 1:nk+1 %( Addition of ys) 275 | % store x 276 | xss3(:,k3) = x3; 277 | % solve the MPC problem 278 | [Uopt3, fval3, flag3] = quadprog(H_3,L_3*x3,Pc_3,qc_3+Sc_3*x3,[],[],[],[],[],opt); 279 | if flag3 < 1 280 | disp(['Optimization infeasible at k = ' num2str(k)]) 281 | break 282 | end 283 | % extract the first control 284 | u3 = Uopt3(1:m); 285 | % store u 286 | uss3(:,k3) = u3; 287 | % apply to the plant 288 | x3 = A*x3 + B*u3; 289 | ys3(:,k3) = C*xss3(:,k3); 290 | end 291 | figure(2) %Output / States Changes 292 | plot((0:k-1),xss(:,1:k)'); hold on 293 | plot(0:k-1,repmat(0.01,1,k),'c--') 294 | hold on 295 | plot(0:k-1,repmat(-0.01,1,k),'c--') 296 | hold off 297 | % plot ((0:k-1),xmin); 298 | % plot ((0:k-1),xmax); 299 | title('State') 300 | xlabel('Time step k'); 301 | ylabel('States') 302 | % legend('Output','xmin constraint','xmax constraint'); 303 | hold off 304 | 305 | figure(3) % Input 306 | stairs((0:k-1),uss(:,1:k)'); hold on 307 | plot((0:k-1),umin,'o') 308 | plot ((0:k-1),umax,'o') 309 | title ('Input u, abs <= 0.5') 310 | xlabel('Time step k') 311 | ylabel('u(k)'); 312 | legend('Input','umin constraint','umax constraint');hold off 313 | 314 | figure (4) % Output 315 | stairs([0:k-1],ys(:,1:k)') 316 | hold on 317 | % constraints 318 | plot(0:k-1,repmat(0.01,1,k),'c--') 319 | hold on 320 | plot(0:k-1,repmat(-0.01,1,k),'c--') 321 | hold off 322 | xlabel('Time Step K ') 323 | ylabel('Hz') 324 | title('Output, MPC Implemented') 325 | %% 326 | PxN = pxf; %check these 327 | qxN = qxf; 328 | 329 | % build MPC problem matrices 330 | [F, G] = predict_mats(A,B,N); 331 | [H, L, M] = cost_mats(F,G,Q,R,P); 332 | [Pc,qc,Sc] = constraint_mats(F,G,Pu,Qu,px,qx,PxN,qxN); 333 | 334 | [F1, G1] = predict_mats(A,B,N_1); 335 | [H1, L1, M1] = cost_mats(F1,G1,Q,R,P); 336 | [Pc1,qc1,Sc1] = constraint_mats(F1,G1,Pu,Qu,px,qx,PxN,qxN); 337 | 338 | [F2, G2] = predict_mats(A,B,N_2); 339 | [H2, L2, M2] = cost_mats(F2,G2,Q,R,P); 340 | [Pc2,qc2,Sc2] = constraint_mats(F2,G2,Pu,Qu,px,qx,PxN,qxN); 341 | 342 | [F3, G3] = predict_mats(A,B,N_3); 343 | [H3, L3, M3] = cost_mats(F3,G3,Q,R,P); 344 | [Pc3,qc3,Sc3] = constraint_mats(F3,G3,Pu,Qu,px,qx,PxN,qxN); 345 | 346 | figure(6) %feasbility region 347 | hold on 348 | % [x1,x2,x3] = meshgrid(-0.5:0.01:0.5); 349 | for x1 = -0.01:0.001:0.01 350 | for x2 = -10:0.1:10 351 | for x3 = -10:0.1:10 352 | x = [x1; x2;x3]; 353 | % solve the MPC problem 354 | [Uopt, fval, flag] = quadprog(H,L*x,Pc,qc+Sc*x); 355 | [Uopt1, fval1, flag1] = quadprog(H1,L1*x,Pc1,qc1+Sc1*x); 356 | [Uopt2, fval2, flag2] = quadprog(H2,L2*x,Pc2,qc2+Sc2*x); 357 | [Uopt3, fval3, flag3] = quadprog(H3,L3*x,Pc3,qc3+Sc3*x); 358 | if flag3 >= 1 % N = 10 359 | plot3(x1,x2,x3,'k.'); hold on 360 | end 361 | if flag2 >= 1 % N = 5 362 | plot3(x1,x2,x3,'r.') 363 | end 364 | if flag1 >= 1 % N = 4 365 | plot3(x1,x2,x3,'g.') 366 | end 367 | if flag >= 1 % N =3 368 | plot3(x1,x2,x3,'c.'); 369 | 370 | end 371 | end 372 | end 373 | end 374 | xlabel('x1'); ylabel('x2'); zlabel('x3'); title('Operating Region'); 375 | legend({'N = 10','N = 5','N = 4','N = 3'}) 376 | % surf(x1,x2,x3,'b.'); 377 | % A = [ 1 1 ; 0 1]; %lambda(A+BK) = 0 378 | % B = [0.5;1]; 379 | % k = -[1 1.5]; 380 | % K = A+B*k 381 | % eig(K) 382 | %% function for checking properties 383 | check_ABQR(A,B,Q,R); 384 | 385 | function check_ABQR(A,B,Q,R) 386 | 387 | if min(eig(R)) > 0 388 | display('R is positive definite') 389 | elseif min(eig(R)) == 0 390 | display('R is positive semidefinite -- DLQR.M will fail!') 391 | else 392 | display('R is not positive semidefinite -- DLQR.M will fail!') 393 | end 394 | 395 | if min(eig(Q)) > 0 396 | display('Q is positive definite') 397 | elseif min(eig(Q)) == 0 398 | display('Q is positive semidefinite') 399 | % check detectability 400 | [ABAR,BBAR,CBAR,T,K] = obsvf(A,B,sqrtm(Q)); 401 | nobs = size(A,2) - sum(K); 402 | if max(abs(eig(ABAR(1:nobs,1:nobs)))) == 0 403 | display('(Q^0.5,A) is constructible') 404 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 405 | display('(Q^0.5,A) is detectable') 406 | else 407 | display('(Q^0.5,A) is not detectable -- DLQR.M will fail!') 408 | end 409 | 410 | 411 | else 412 | display('Q is not positive semidefinite -- DQLR.M will fail!') 413 | end 414 | 415 | r = rank(ctrb(A,B)); 416 | if r == 2 417 | display('(A,B) is reachable') 418 | else 419 | display('(A,B) is not reachable') 420 | % check stabilizability 421 | [ABAR,BBAR,CBAR,T,K] = ctrbf(A,B,sqrtm(Q)); 422 | nrea = size(A,2) - sum(K); 423 | if max(abs(eig(ABAR(1:nrea,1:nrea)))) == 0 424 | display('(A,B) is controllable') 425 | elseif max(abs(eig(ABAR(~K,~K)))) < 1 426 | display('(A,B) is stabilizable') 427 | else 428 | display('(A,B) is not stabilizable -- DLQR.M will fail!') 429 | end 430 | end 431 | 432 | end -------------------------------------------------------------------------------- /my plot.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/my plot.PNG -------------------------------------------------------------------------------- /mynewscript.m: -------------------------------------------------------------------------------- 1 | % system dimensions 2 | n = 3; % no. of states 3 | m = 1; % no. of inputs 4 | 5 | % input limits 6 | umax = 1; 7 | umin = -2; 8 | 9 | % state limits 10 | xmax = [2; 1; 4]; 11 | xmin = [-3; -2; -5]; 12 | 13 | % matrices Pu*u <= qu 14 | % 1*u <= 1 (row 1) 15 | % -1*u <= 2 (row 2) [which implies 1*u >= -2] 16 | Pu = [eye(m); -eye(m)]; 17 | qu = [umax; -umin]; 18 | Px = [eye(n); -eye(n)]; 19 | qx = [xmax; -xmin]; 20 | Pxf = Px; 21 | qxf = qx; 22 | 23 | plotregion(-Px,-qx) -------------------------------------------------------------------------------- /myscript_33.m: -------------------------------------------------------------------------------- 1 | %% Problem setup 2 | 3 | % system matrices 4 | A = [3 1; 2.01 1.99]; 5 | B = [0.1; 2.1]; 6 | C = [-0.35 1]; 7 | 8 | % controller parameters 9 | Q = C'*C; 10 | 11 | % change these 12 | R = 100; 13 | P = 0*Q; 14 | N = 3; 15 | 16 | %% Synchronous session 6 17 | % prediction matrices 18 | [F,G] = predict_mats(A,B,N); 19 | 20 | % cost matrices 21 | [H,L,M] = cost_mats(F,G,Q,R,P); 22 | 23 | % optimal policy 24 | S = -H\L; 25 | 26 | % optimal feedback law 27 | KN = S(1,:); 28 | 29 | % form closed-loop system 30 | Phi = A+B*KN; 31 | 32 | % stability check 33 | rho = max(abs(eig(Phi))); 34 | 35 | if rho >= 1 36 | display('System is not stable') 37 | else 38 | display('System is stable') 39 | end 40 | 41 | %% Synchronous session 7 42 | 43 | % dimensions 44 | n = size(A,2); 45 | m = size(B,2); 46 | 47 | % constraints 48 | Px = zeros(n,n*N); 49 | Px(:,(end-n+1):end) = eye(n); 50 | 51 | % E and D, so that E*U = -D*x 52 | E = Px*G; 53 | D = Px*F; 54 | 55 | % KKT system (LHS) 56 | curlyK = [H E'; E zeros(n)]; 57 | 58 | % solve KKT system curlyK*[sol] = -[L; D]*x 59 | T = -curlyK\[L; D]; 60 | 61 | % so that solution [uopt; lambdaopt] = T*x 62 | KNbar = T(1:m,:); 63 | 64 | % form closed-loop system 65 | Phibar = A+B*KNbar; 66 | 67 | % stability check 68 | rho = max(abs(eig(Phibar))); 69 | 70 | if rho >= 1 71 | display('System with terminal constraint is not stable') 72 | else 73 | display('System with terminal constraint is stable') 74 | end 75 | 76 | %% Synchronous session 8 77 | 78 | % find a stabilizing K for (A,B) 79 | %K = -dlqr(A,B,Q,R); 80 | desired_poles = [0 0]; 81 | K = -acker(A,B,desired_poles); 82 | 83 | % solve Lyapunov equation 84 | P = dlyap((A+B*K)',Q + K'*R*K); 85 | 86 | % cost matrices 87 | [H,L,M] = cost_mats(F,G,Q,R,P); 88 | 89 | % optimal policy 90 | S = -H\L; 91 | 92 | % optimal feedback law 93 | KN = S(1,:); 94 | 95 | % form closed-loop system 96 | Phi = A+B*KN; 97 | 98 | % stability check 99 | rho = max(abs(eig(Phi))); 100 | 101 | if rho >= 1 102 | display('System with terminal P is not stable') 103 | else 104 | display('System with terminal P is stable') 105 | end 106 | 107 | %% Synchronous session 10 108 | 109 | % initial state 110 | x = [-3; 0]; 111 | 112 | % Obtain a plot of x2(k+i|k) versus x1(k+i|k) 113 | %Uopt = S*x; 114 | %Xopt = F*x + G*Uopt; 115 | Xopt = (F + G*S)*x; 116 | 117 | % Xopt contains x(k+1|k), x(k+2|k), ..., x(k+N|k) 118 | Xopt = [x; Xopt]; 119 | 120 | % extract x1 and x2 elements 121 | Xoptodd = Xopt(1:2:end,:); 122 | Xopteven = Xopt(2:2:end,:); 123 | 124 | 125 | 126 | % x(k+N|k) is final 2 elements of Xopt 127 | xN = Xopt(end-n+1:end,:); 128 | 129 | % x(k+N+1|k) = A*x(k+N|k) + B*K*x(k+N|k) = (A+B*K)*x(k+N|k) 130 | % x(k+N+2|k) = (A+B*K)*x(k+N+1|k) = (A+B*K)^2 * x(k+N|k) 131 | % 132 | % x(k+N+i|k) = (A+B*K)^i * x(k+N|k) 133 | 134 | % mode-2 horizon (how many steps I want to see) 135 | N2 = 2; 136 | [F2, G2] = predict_mats(A+B*K,B,N2); 137 | 138 | % F2 is [(A+BK); (A+BK)^2; ...; (A+BK)^N2] 139 | 140 | % mode 2 state predictions 141 | Xopt2 = [xN; F2*xN;]; 142 | % extract x1 and x2 elements 143 | Xopt2odd = Xopt2(1:2:end,:); 144 | Xopt2even = Xopt2(2:2:end,:); 145 | 146 | % plot mode 1 147 | figure 148 | plot(Xoptodd,Xopteven,'bo-'), hold on 149 | plot(Xopt2odd,Xopt2even,'r*-') 150 | -------------------------------------------------------------------------------- /myscript_4.m: -------------------------------------------------------------------------------- 1 | %% Problem setup 2 | 3 | % system matrices 4 | A = [3 1; 2.01 1.99]; 5 | B = [0.1; 2.1]; 6 | C = [-0.35 1]; 7 | 8 | % controller parameters 9 | Q = C'*C; 10 | 11 | % change these 12 | R = 100; 13 | P = 0*Q; 14 | N = 3; 15 | 16 | %% Synchronous session 6 17 | % prediction matrices 18 | [F,G] = predict_mats(A,B,N); 19 | 20 | % cost matrices 21 | [H,L,M] = cost_mats(F,G,Q,R,P); 22 | 23 | % optimal policy 24 | S = -H\L; 25 | 26 | % optimal feedback law 27 | KN = S(1,:); 28 | 29 | % form closed-loop system 30 | Phi = A+B*KN; 31 | 32 | % stability check 33 | rho = max(abs(eig(Phi))); 34 | 35 | if rho >= 1 36 | display('System is not stable') 37 | else 38 | display('System is stable') 39 | end 40 | 41 | %% Synchronous session 7 42 | 43 | % dimensions 44 | n = size(A,2); 45 | m = size(B,2); 46 | 47 | % constraints 48 | Px = zeros(n,n*N); 49 | Px(:,(end-n+1):end) = eye(n); 50 | 51 | % E and D, so that E*U = -D*x 52 | E = Px*G; 53 | D = Px*F; 54 | 55 | % KKT system (LHS) 56 | curlyK = [H E'; E zeros(n)]; 57 | 58 | % solve KKT system curlyK*[sol] = -[L; D]*x 59 | T = -curlyK\[L; D]; 60 | 61 | % so that solution [uopt; lambdaopt] = T*x 62 | KNbar = T(1:m,:); 63 | 64 | % form closed-loop system 65 | Phibar = A+B*KNbar; 66 | 67 | % stability check 68 | rho = max(abs(eig(Phibar))); 69 | 70 | if rho >= 1 71 | display('System with terminal constraint is not stable') 72 | else 73 | display('System with terminal constraint is stable') 74 | end 75 | 76 | %% Synchronous session 8 77 | 78 | % find a stabilizing K for (A,B) 79 | %K = -dlqr(A,B,Q,R); 80 | desired_poles = [0 0]; 81 | K = -acker(A,B,desired_poles); 82 | 83 | % solve Lyapunov equation 84 | P = dlyap((A+B*K)',Q + K'*R*K); 85 | 86 | % cost matrices 87 | [H,L,M] = cost_mats(F,G,Q,R,P); 88 | 89 | % optimal policy 90 | S = -H\L; 91 | 92 | % optimal feedback law 93 | KN = S(1,:); 94 | 95 | % form closed-loop system 96 | Phi = A+B*KN; 97 | 98 | % stability check 99 | rho = max(abs(eig(Phi))); 100 | 101 | if rho >= 1 102 | display('System with terminal P is not stable') 103 | else 104 | display('System with terminal P is stable') 105 | end 106 | 107 | %% Synchronous session 10 108 | 109 | % initial state 110 | x = [-3; 0]; 111 | 112 | % Obtain a plot of x2(k+i|k) versus x1(k+i|k) 113 | %Uopt = S*x; 114 | %Xopt = F*x + G*Uopt; 115 | Xopt = (F + G*S)*x; 116 | 117 | % Xopt contains x(k+1|k), x(k+2|k), ..., x(k+N|k) 118 | Xopt = [x; Xopt]; 119 | 120 | % extract x1 and x2 elements 121 | Xoptodd = Xopt(1:2:end,:); 122 | Xopteven = Xopt(2:2:end,:); 123 | 124 | 125 | 126 | % x(k+N|k) is final 2 elements of Xopt 127 | xN = Xopt(end-n+1:end,:); 128 | 129 | % x(k+N+1|k) = A*x(k+N|k) + B*K*x(k+N|k) = (A+B*K)*x(k+N|k) 130 | % x(k+N+2|k) = (A+B*K)*x(k+N+1|k) = (A+B*K)^2 * x(k+N|k) 131 | % 132 | % x(k+N+i|k) = (A+B*K)^i * x(k+N|k) 133 | 134 | % mode-2 horizon (how many steps I want to see) 135 | N2 = 2; 136 | [F2, G2] = predict_mats(A+B*K,B,N2); 137 | 138 | % F2 is [(A+BK); (A+BK)^2; ...; (A+BK)^N2] 139 | 140 | % mode 2 state predictions 141 | Xopt2 = [xN; F2*xN;]; 142 | % extract x1 and x2 elements 143 | Xopt2odd = Xopt2(1:2:end,:); 144 | Xopt2even = Xopt2(2:2:end,:); 145 | 146 | % plot mode 1 147 | figure 148 | plot(Xoptodd,Xopteven,'bo-'), hold on 149 | plot(Xopt2odd,Xopt2even,'r*-') 150 | -------------------------------------------------------------------------------- /myscript_5.m: -------------------------------------------------------------------------------- 1 | %% Problem setup 2 | 3 | % system matrices 4 | A = [3 1; 2.01 1.99]; 5 | B = [0.1; 2.1]; 6 | C = [-0.35 1]; 7 | 8 | % controller parameters 9 | Q = C'*C; 10 | 11 | % change these 12 | R = 100; 13 | P = 0*Q; 14 | N = 3; 15 | 16 | %% Synchronous session 6 17 | % prediction matrices 18 | [F,G] = predict_mats(A,B,N); 19 | 20 | % cost matrices 21 | [H,L,M] = cost_mats(F,G,Q,R,P); 22 | 23 | % optimal policy 24 | S = -H\L; 25 | 26 | % optimal feedback law 27 | KN = S(1,:); 28 | 29 | % form closed-loop system 30 | Phi = A+B*KN; 31 | 32 | % stability check 33 | rho = max(abs(eig(Phi))); 34 | 35 | if rho >= 1 36 | display('System is not stable') 37 | else 38 | display('System is stable') 39 | end 40 | 41 | %% Synchronous session 7 42 | 43 | % dimensions 44 | n = size(A,2); 45 | m = size(B,2); 46 | 47 | % constraints 48 | Px = zeros(n,n*N); 49 | Px(:,(end-n+1):end) = eye(n); 50 | 51 | % E and D, so that E*U = -D*x 52 | E = Px*G; 53 | D = Px*F; 54 | 55 | % KKT system (LHS) 56 | curlyK = [H E'; E zeros(n)]; 57 | 58 | % solve KKT system curlyK*[sol] = -[L; D]*x 59 | T = -curlyK\[L; D]; 60 | 61 | % so that solution [uopt; lambdaopt] = T*x 62 | KNbar = T(1:m,:); 63 | 64 | % form closed-loop system 65 | Phibar = A+B*KNbar; 66 | 67 | % stability check 68 | rho = max(abs(eig(Phibar))); 69 | 70 | if rho >= 1 71 | display('System with terminal constraint is not stable') 72 | else 73 | display('System with terminal constraint is stable') 74 | end 75 | 76 | %% Synchronous session 8 77 | 78 | % find a stabilizing K for (A,B) 79 | %K = -dlqr(A,B,Q,R); 80 | desired_poles = [0 0]; 81 | K = -acker(A,B,desired_poles); 82 | 83 | % solve Lyapunov equation 84 | P = dlyap((A+B*K)',Q + K'*R*K); 85 | 86 | % cost matrices 87 | [H,L,M] = cost_mats(F,G,Q,R,P); 88 | 89 | % optimal policy 90 | S = -H\L; 91 | 92 | % optimal feedback law 93 | KN = S(1,:); 94 | 95 | % form closed-loop system 96 | Phi = A+B*KN; 97 | 98 | % stability check 99 | rho = max(abs(eig(Phi))); 100 | 101 | if rho >= 1 102 | display('System with terminal P is not stable') 103 | else 104 | display('System with terminal P is stable') 105 | end 106 | 107 | %% Synchronous session 10 108 | 109 | % initial state 110 | x = [-3; 0]; 111 | 112 | % Obtain a plot of x2(k+i|k) versus x1(k+i|k) 113 | %Uopt = S*x; 114 | %Xopt = F*x + G*Uopt; 115 | Xopt = (F + G*S)*x; 116 | 117 | % Xopt contains x(k+1|k), x(k+2|k), ..., x(k+N|k) 118 | Xopt = [x; Xopt]; 119 | 120 | % extract x1 and x2 elements 121 | Xoptodd = Xopt(1:2:end,:); 122 | Xopteven = Xopt(2:2:end,:); 123 | 124 | 125 | 126 | % x(k+N|k) is final 2 elements of Xopt 127 | xN = Xopt(end-n+1:end,:); 128 | 129 | % x(k+N+1|k) = A*x(k+N|k) + B*K*x(k+N|k) = (A+B*K)*x(k+N|k) 130 | % x(k+N+2|k) = (A+B*K)*x(k+N+1|k) = (A+B*K)^2 * x(k+N|k) 131 | % 132 | % x(k+N+i|k) = (A+B*K)^i * x(k+N|k) 133 | 134 | % mode-2 horizon (how many steps I want to see) 135 | N2 = 2; 136 | [F2, G2] = predict_mats(A+B*K,B,N2); 137 | 138 | % F2 is [(A+BK); (A+BK)^2; ...; (A+BK)^N2] 139 | 140 | % mode 2 state predictions 141 | Xopt2 = [xN; F2*xN;]; 142 | % extract x1 and x2 elements 143 | Xopt2odd = Xopt2(1:2:end,:); 144 | Xopt2even = Xopt2(2:2:end,:); 145 | 146 | % plot mode 1 147 | figure 148 | plot(Xoptodd,Xopteven,'bo-'), hold on 149 | plot(Xopt2odd,Xopt2even,'r*-') 150 | -------------------------------------------------------------------------------- /myscript_6.m: -------------------------------------------------------------------------------- 1 | %% Problem setup 2 | 3 | % system matrices 4 | A = [3 1; 2.01 1.99]; 5 | B = [0.1; 2.1]; 6 | C = [-0.35 1]; 7 | 8 | % controller parameters 9 | Q = C'*C; 10 | 11 | % change these 12 | R = 100; 13 | P = 0*Q; 14 | N = 3; 15 | 16 | %% Synchronous session 6 17 | % prediction matrices 18 | [F,G] = predict_mats(A,B,N); 19 | 20 | % cost matrices 21 | [H,L,M] = cost_mats(F,G,Q,R,P); 22 | 23 | % optimal policy 24 | S = -H\L; 25 | 26 | % optimal feedback law 27 | KN = S(1,:); 28 | 29 | % form closed-loop system 30 | Phi = A+B*KN; 31 | 32 | % stability check 33 | rho = max(abs(eig(Phi))); 34 | 35 | if rho >= 1 36 | display('System is not stable') 37 | else 38 | display('System is stable') 39 | end 40 | 41 | %% Synchronous session 7 42 | 43 | % dimensions 44 | n = size(A,2); 45 | m = size(B,2); 46 | 47 | % constraints 48 | Px = zeros(n,n*N); 49 | Px(:,(end-n+1):end) = eye(n); 50 | 51 | % E and D, so that E*U = -D*x 52 | E = Px*G; 53 | D = Px*F; 54 | 55 | % KKT system (LHS) 56 | curlyK = [H E'; E zeros(n)]; 57 | 58 | % solve KKT system curlyK*[sol] = -[L; D]*x 59 | T = -curlyK\[L; D]; 60 | 61 | % so that solution [uopt; lambdaopt] = T*x 62 | KNbar = T(1:m,:); 63 | 64 | % form closed-loop system 65 | Phibar = A+B*KNbar; 66 | 67 | % stability check 68 | rho = max(abs(eig(Phibar))); 69 | 70 | if rho >= 1 71 | display('System with terminal constraint is not stable') 72 | else 73 | display('System with terminal constraint is stable') 74 | end 75 | 76 | %% Synchronous session 8 77 | 78 | % find a stabilizing K for (A,B) 79 | %K = -dlqr(A,B,Q,R); 80 | desired_poles = [0 0]; 81 | K = -acker(A,B,desired_poles); 82 | 83 | % solve Lyapunov equation 84 | P = dlyap((A+B*K)',Q + K'*R*K); 85 | 86 | % cost matrices 87 | [H,L,M] = cost_mats(F,G,Q,R,P); 88 | 89 | % optimal policy 90 | S = -H\L; 91 | 92 | % optimal feedback law 93 | KN = S(1,:); 94 | 95 | % form closed-loop system 96 | Phi = A+B*KN; 97 | 98 | % stability check 99 | rho = max(abs(eig(Phi))); 100 | 101 | if rho >= 1 102 | display('System with terminal P is not stable') 103 | else 104 | display('System with terminal P is stable') 105 | end 106 | 107 | %% Synchronous session 10 108 | 109 | % initial state 110 | x = [-3; 0]; 111 | 112 | % Obtain a plot of x2(k+i|k) versus x1(k+i|k) 113 | %Uopt = S*x; 114 | %Xopt = F*x + G*Uopt; 115 | Xopt = (F + G*S)*x; 116 | 117 | % Xopt contains x(k+1|k), x(k+2|k), ..., x(k+N|k) 118 | Xopt = [x; Xopt]; 119 | 120 | % extract x1 and x2 elements 121 | Xoptodd = Xopt(1:2:end,:); 122 | Xopteven = Xopt(2:2:end,:); 123 | 124 | 125 | 126 | % x(k+N|k) is final 2 elements of Xopt 127 | xN = Xopt(end-n+1:end,:); 128 | 129 | % x(k+N+1|k) = A*x(k+N|k) + B*K*x(k+N|k) = (A+B*K)*x(k+N|k) 130 | % x(k+N+2|k) = (A+B*K)*x(k+N+1|k) = (A+B*K)^2 * x(k+N|k) 131 | % 132 | % x(k+N+i|k) = (A+B*K)^i * x(k+N|k) 133 | 134 | % mode-2 horizon (how many steps I want to see) 135 | N2 = 2; 136 | [F2, G2] = predict_mats(A+B*K,B,N2); 137 | 138 | % F2 is [(A+BK); (A+BK)^2; ...; (A+BK)^N2] 139 | 140 | % mode 2 state predictions 141 | Xopt2 = [xN; F2*xN;]; 142 | % extract x1 and x2 elements 143 | Xopt2odd = Xopt2(1:2:end,:); 144 | Xopt2even = Xopt2(2:2:end,:); 145 | 146 | % plot mode 1 147 | figure 148 | plot(Xoptodd,Xopteven,'bo-'), hold on 149 | plot(Xopt2odd,Xopt2even,'r*-') 150 | -------------------------------------------------------------------------------- /n = 10 2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/n = 10 2d.png -------------------------------------------------------------------------------- /n = 10 3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/n = 10 3d.png -------------------------------------------------------------------------------- /n = 5 2d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/n = 5 2d.png -------------------------------------------------------------------------------- /n = 5 3d.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/n = 5 3d.png -------------------------------------------------------------------------------- /predict_mats.m: -------------------------------------------------------------------------------- 1 | function [F,G] = predict_mats(A,B,N) 2 | % 3 | % PREDICT_MATS.M returns the MPC prediction matrices for a system 4 | % 5 | % x^+ = A*x + B*u 6 | % 7 | % That is, the matrices F and G from the equation 8 | % 9 | % X = F*x + G*U 10 | % 11 | % USAGE: 12 | % 13 | % [F,G] = predict_mats(A,B,N) 14 | % 15 | % where N is prediction horizon length. 16 | % 17 | % P. Trodden, 2015. 18 | 19 | % dimensions 20 | n = size(A,1); 21 | m = size(B,2); 22 | 23 | % allocate 24 | F = zeros(n*N,n); 25 | G = zeros(n*N,m*(N-1)); 26 | 27 | % form row by row 28 | for i = 1:N 29 | % F 30 | F(n*(i-1)+(1:n),:) = A^i; 31 | 32 | % G 33 | % form element by element 34 | for j = 1:i 35 | G(n*(i-1)+(1:n),m*(j-1)+(1:m)) = A^(i-j)*B; 36 | end 37 | end 38 | -------------------------------------------------------------------------------- /qx0.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/awabsyed1/Model-Predictiive-Control--MPC-/747219a33d53f07e0e9b3dee1a29be00953456d5/qx0.PNG -------------------------------------------------------------------------------- /sync_session_11_7.m: -------------------------------------------------------------------------------- 1 | clear 2 | %% Problem setup 3 | 4 | % system matrices 5 | A = [3 1; 2.01 1.99]; 6 | B = [0.1; 2.1]; 7 | C = [-0.35 1]; 8 | 9 | % controller parameters 10 | Q = C'*C; 11 | 12 | % change these 13 | R = 100; 14 | P = 0*Q; 15 | N = 3; 16 | 17 | % find a stabilizing K for (A,B) 18 | %K = -dlqr(A,B,Q,R); 19 | desired_poles = [0 0]; 20 | K = -acker(A,B,desired_poles); 21 | 22 | % solve Lyapunov equation 23 | P = dlyap((A+B*K)',Q + K'*R*K); 24 | 25 | % prediction matrices 26 | [F,G] = predict_mats(A,B,N); 27 | 28 | % cost matrices 29 | [H,L,M] = cost_mats(F,G,Q,R,P); 30 | 31 | % optimal policy 32 | S = -H\L; 33 | 34 | % optimal feedback law 35 | KN = S(1,:); 36 | 37 | % form closed-loop system 38 | Phi = A+B*KN; 39 | 40 | % stability check 41 | rho = max(abs(eig(Phi))); 42 | 43 | if rho >= 1 44 | display('System with terminal P is not stable') 45 | else 46 | display('System with terminal P is stable') 47 | end 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /sync_session_16_8.m: -------------------------------------------------------------------------------- 1 | %% ACS6116 Synchronous Session 16 2 | 3 | %% Problem setup 4 | clear 5 | 6 | % System matrices 7 | A = [1 1; 0 2]; 8 | B = [0; 0.5]; 9 | C = [1 0]; 10 | 11 | % dimensions 12 | n = size(A,1); 13 | m = size(B,2); 14 | 15 | % initial state 16 | x0 = [3; 0]; 17 | 18 | % horizon N 19 | N = 3; 20 | 21 | % cost matrices 22 | Q = C'*C; 23 | R = 1; 24 | K = [-2 -6]; 25 | P = dlyap((A+B*K)',Q+K'*R*K); 26 | 27 | 28 | %% Ex 5.1 29 | umin = -1.5; 30 | umax = +1.5; 31 | xmin = [-10; -5]; 32 | xmax = [+10; +5]; 33 | 34 | % build constraint matrices 35 | Pu = [eye(m); -eye(m)]; 36 | qu = [umax; -umin]; 37 | Px = [eye(n); -eye(n)]; 38 | qx = [xmax; -xmin]; 39 | PxN = Px; 40 | qxN = qx; 41 | 42 | % build MPC problem matrices 43 | [F, G] = predict_mats(A,B,N); 44 | [H, L, M] = cost_mats(F,G,Q,R,P); 45 | [Pc,qc,Sc] = constraint_mats(F,G,Pu,qu,Px,qx,PxN,qxN); 46 | %[Pc,qc,Sc] = constraint_mats(F,G,Pu,qu,[],[],[],[]); 47 | 48 | %% Ex 5.2 49 | nk = 20; 50 | x = x0; 51 | xs = zeros(n,nk); 52 | us = zeros(m,nk); 53 | 54 | for k = 1:nk+1 55 | 56 | % store x 57 | xs(:,k) = x; 58 | 59 | % solve the MPC problem 60 | [Uopt, fval, flag] = quadprog(H,L*x,Pc,qc+Sc*x); 61 | 62 | if flag < 1 63 | disp(['Optimization infeasible at k = ' num2str(k)]) 64 | break 65 | end 66 | 67 | % extract the first control 68 | u = Uopt(1:m); 69 | 70 | % store u 71 | us(:,k) = u; 72 | 73 | % apply to the plant 74 | x = A*x + B*u; 75 | 76 | end 77 | 78 | figure(1) 79 | plot((0:k-1),xs(:,1:k)') 80 | 81 | figure(2) 82 | stairs((0:k-1),us(:,1:k)') 83 | 84 | %% Ex 5.2(h) 85 | 86 | PxN = Px; 87 | qxN = 0*qx; 88 | 89 | % build MPC problem matrices 90 | [F, G] = predict_mats(A,B,N); 91 | [H, L, M] = cost_mats(F,G,Q,R,P); 92 | [Pc,qc,Sc] = constraint_mats(F,G,Pu,qu,Px,qx,PxN,qxN); 93 | 94 | figure(3) 95 | hold on 96 | 97 | for x1 = -10:0.2:10 98 | for x2 = -5:0.1:5 99 | 100 | x = [x1; x2]; 101 | 102 | % solve the MPC problem 103 | [Uopt, fval, flag] = quadprog(H,L*x,Pc,qc+Sc*x); 104 | 105 | if flag >= 1 106 | plot(x1,x2,'b.'); 107 | end 108 | 109 | end 110 | end 111 | --------------------------------------------------------------------------------