├── Linear MPC ├── Untitled2.m ├── b_eq_calc.m ├── constant_term.m ├── lin_sys.m └── lin_sys_Luenberger_observer.m └── QP.docx /Linear MPC/Untitled2.m: -------------------------------------------------------------------------------- 1 | clc;clear;close all; 2 | A = [ -6 -1 1; 3 -5 3; 4 1 -2]; %%aa 3 | B = [ -2 -1 -5; 4 2 7; -2 -3 3]; %aa 4 | C = eye(2,3); 5 | D = zeros(2,3); 6 | poles = [.66 .52 0.8]; 7 | 8 | % A = [ -2]; %%aa 9 | % B = [ 3]; %aa 10 | % C = .1*eye(1); 11 | % D = zeros(1); 12 | % poles = [.66]; 13 | 14 | % C = eye(3,3); 15 | % D = zeros(3,3); 16 | sysCont = ss(A,B,C,D); 17 | %Discrete model 18 | Ts = .1; 19 | sysDiscr = c2d(sysCont,Ts,'zoh'); 20 | sys.a = sysDiscr.a;sys.b = sysDiscr.b;sys.c = sysDiscr.c;sys.d = sysDiscr.d;sys.Ts = Ts; 21 | 22 | 23 | 24 | L_gain = place(sys.a', sys.c', poles).'; 25 | 26 | [sys.n_x,sys.n_u] = size(sys.b); 27 | [sys.n_y,~] = size(sys.c); 28 | sys.ap = [sys.a,sys.b;zeros(sys.n_u,sys.n_x),eye(sys.n_u)]; 29 | sys.bp = [sys.b;eye(sys.n_u)]; 30 | sys.cp = [sys.c,zeros(sys.n_y,sys.n_u)]; 31 | 32 | sys.h = 10; 33 | sys.Q_x = 0.0*eye(sys.n_x);sys.R_u = 0.0*eye(sys.n_u);sys.R_du = 0.0*eye(sys.n_u);sys.Q_y = .4*eye(sys.n_y); 34 | % sys.Q_x = 0.1*eye(sys.n_x);sys.R_u = 0.2*eye(sys.n_u);sys.R_du = 0.3*eye(sys.n_u);sys.Q_y = .4*eye(sys.n_y); 35 | sys.Q_xp = blkdiag(sys.Q_x,sys.R_u); 36 | 37 | sys.lb_u = [-100; -110; -120]; 38 | sys.ub_u = [150; 140; 130]; 39 | sys.lb_x = -Inf(sys.n_x,1); 40 | sys.ub_x = +Inf(sys.n_x,1); 41 | sys.lb_y = -Inf(sys.n_y,1); 42 | sys.ub_y = +Inf(sys.n_y,1); 43 | x_0 = zeros(sys.n_x,1); 44 | u__1 = zeros(sys.n_u,1); 45 | xsp = 5*ones(sys.n_x,1); 46 | ysp = sys.c*xsp 47 | y_sp = [];x_sp = []; 48 | for i=1:sys.h 49 | y_sp = [y_sp,ysp]; 50 | x_sp = [x_sp,xsp]; 51 | end 52 | lb = [];ub = []; 53 | for i=1:sys.h 54 | lb = [lb;sys.lb_u-sys.ub_u;sys.lb_x-x_sp(:,i);sys.lb_u]; 55 | ub = [ub;sys.ub_u-sys.lb_u;sys.ub_x-x_sp(:,i);sys.ub_u]; 56 | end 57 | for i=1:sys.h 58 | lb = [lb;sys.lb_y - y_sp(:,i)]; 59 | ub = [ub;sys.ub_y - y_sp(:,i)]; 60 | end 61 | x_spp = [x_sp;zeros(sys.n_u,sys.h)]; 62 | [Aeq,H,f] = constant_term(sys); 63 | A = []; 64 | b = []; 65 | 66 | u__1 = zeros(sys.n_u,1); 67 | x1_updated(:,1) = zeros(sys.n_x,1); 68 | x_plant(:,1) = zeros(sys.n_x,1); 69 | y1(:,1) = sys.c*x1_updated(:,1); 70 | y_plant(:,1) = sys.c*x_plant(:,1); 71 | NTs = 150; 72 | umpc = zeros(size(H,1)); 73 | for i=1:NTs 74 | beq = b_eq_calc(sys,x_spp,x1_updated(:,i),u__1,y_sp); 75 | umpc = quadprog(H,f,A,b,Aeq,beq,lb,ub,umpc); %Run QP for optimal input 76 | u(:,i) = umpc(sys.n_u+sys.n_x+1:sys.n_u+sys.n_x+sys.n_u); %Set the first input as applied input 77 | u__1 = u(:,i); 78 | [y_plant(:,i+1),x_plant(:,i+1)] = lin_sys(x_plant(:,i),sys,u(:,i)); %Apply the input to the plant 79 | [y1(:,i+1),x1_updated(:,i+1)] = lin_sys_Luenberger_observer(x1_updated(:,i),sys,u(:,i),y_plant(:,i),L_gain);%Use plant output for state estimation 80 | end 81 | T = sys.Ts*(1:NTs+1); 82 | figure 83 | for i=1:sys.n_y 84 | subplot(sys.n_y,1,i) 85 | % plot(T,y_plant(i,:),[T(1),T(end)],[ysp(i),ysp(i)],'--r',T,y1(i,:)) 86 | plot(T,y_plant(i,:),[T(1),T(end)],[ysp(i),ysp(i)],'--r') 87 | legend(['y_',num2str(i)],['y_s_p_,_',num2str(i)],['y_o_b_s_v_',num2str(i)]) 88 | xlabel('Time');ylabel(['y_',num2str(i)]) 89 | grid on 90 | end 91 | figure 92 | for i=1:sys.n_u 93 | subplot(sys.n_u,1,i) 94 | plot(T(1:end-1),u(i,:),[T(1),T(end)],[sys.lb_u(i),sys.lb_u(i)],'--r',[T(1),T(end)],[sys.ub_u(i),sys.ub_u(i)],'--r') 95 | legend(['u_',num2str(i)],['lb_u_',num2str(i)],['ub_u_',num2str(i)]) 96 | xlabel('Time');ylabel(['u_',num2str(i)]) 97 | grid on 98 | end -------------------------------------------------------------------------------- /Linear MPC/b_eq_calc.m: -------------------------------------------------------------------------------- 1 | function b_eq = b_eq_calc(sys,x_spp,x_0,u__1,y_sp) 2 | b_eq = zeros(sys.h*(sys.n_x+sys.n_u+sys.n_y),1); 3 | b_eq(1:sys.n_x+sys.n_u) = sys.ap*[x_0;u__1]-x_spp(:,1); 4 | i = 1; 5 | b_eq(sys.h*(sys.n_x+sys.n_u)+(i-1)*sys.n_y+1:sys.h*(sys.n_x+sys.n_u)+i*sys.n_y) = +sys.cp*x_spp(:,i)-y_sp(:,i); 6 | for i=2:sys.h 7 | b_eq((i-1)*(sys.n_x+sys.n_u)+1:i*(sys.n_x+sys.n_u)) = sys.ap*x_spp(:,i-1)-x_spp(:,i); 8 | b_eq(sys.h*(sys.n_x+sys.n_u)+(i-1)*sys.n_y+1:sys.h*(sys.n_x+sys.n_u)+i*sys.n_y) = +sys.cp*x_spp(:,i)-y_sp(:,i); 9 | end 10 | end -------------------------------------------------------------------------------- /Linear MPC/constant_term.m: -------------------------------------------------------------------------------- 1 | function [A_eq,Q_X,f] = constant_term(sys) 2 | A_eq = zeros((sys.n_x+sys.n_u+sys.n_y)*sys.h,(sys.n_x+2*sys.n_u+sys.n_y)*sys.h); 3 | A_eq(1:sys.n_x+sys.n_u,1:sys.n_x+2*sys.n_u) = [-sys.bp,eye(sys.n_x+sys.n_u)]; 4 | Q_X_1 = blkdiag(sys.R_du,sys.Q_xp); 5 | Q_X_2 = sys.Q_y; 6 | A_eq((sys.n_x+sys.n_u)*sys.h+1:(sys.n_x+sys.n_u)*sys.h+sys.n_y,sys.n_u+1:sys.n_u+(sys.n_x+sys.n_u)) = -sys.cp; 7 | for i=2:sys.h 8 | A_eq((i-1)*(sys.n_x+sys.n_u)+1:(i)*(sys.n_x+sys.n_u),sys.n_u+(i-2)*(sys.n_x+2*sys.n_u)+1:sys.n_u+(i-1)*(sys.n_x+2*sys.n_u)+sys.n_x+sys.n_u) = [-sys.ap,-sys.bp,eye(sys.n_x+sys.n_u)]; 9 | Q_X_1 = blkdiag(Q_X_1,sys.R_du,sys.Q_xp); 10 | Q_X_2 = blkdiag(Q_X_2,sys.Q_y); 11 | A_eq((sys.n_x+sys.n_u)*sys.h+(i-1)*sys.n_y+1:(sys.n_x+sys.n_u)*sys.h+i*sys.n_y,sys.n_u+(i-1)*(sys.n_x+2*sys.n_u)+1:sys.n_u+(i-1)*(sys.n_x+2*sys.n_u)+sys.n_x+sys.n_u) = -sys.cp; 12 | end 13 | Q_X = blkdiag(Q_X_1,Q_X_2); 14 | A_eq((sys.n_x+sys.n_u)*sys.h+1:end,(sys.n_x+2*sys.n_u)*sys.h+1:end) = eye(sys.h*sys.n_y); 15 | f = zeros(size(Q_X,1),1); 16 | end -------------------------------------------------------------------------------- /Linear MPC/lin_sys.m: -------------------------------------------------------------------------------- 1 | function [y,x] = lin_sys(x0,sys,u) 2 | x = sys.a*x0 + sys.b*u; 3 | y = sys.c*x + sys.d*u; 4 | end -------------------------------------------------------------------------------- /Linear MPC/lin_sys_Luenberger_observer.m: -------------------------------------------------------------------------------- 1 | function [y1,x1_updated] = lin_sys_Luenberger_observer(x0,sys,u0,y_plant_0,K1) 2 | x1_predicted = sys.a*x0 + sys.b*u0; 3 | y_0 = sys.c*x0; 4 | e_0 = y_plant_0 - y_0; 5 | x1_updated = x1_predicted + K1*e_0; 6 | y1 = sys.c*x1_updated; 7 | end -------------------------------------------------------------------------------- /QP.docx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Kheradm/Linear-Model-Predictive-Control-MPC-with-quadratic-programming-QP-and-state-space-model/0d4f5316c88fdf1163601a1c993401c72ef9e548/QP.docx --------------------------------------------------------------------------------