flywheelmpc.m

一个很好的mpc例子

% Mpc Controller for Flywheel
%% By Chen Shauixun
%% Power Group

%%%%%%%%%%      PROBLEM DEFINITION:    %%%%%%%%%%

% Define time-constant of reference trajectory Tref:
clear all; 
clc;
Tref = 0.0008;
T_end = 1;
sd=0.003;

% Define sampling interval Ts (default Tref/10):
if Tref == 0,
  Ts = 1;
else
  Ts = Tref;
end

K1 = 2.90e6; K2 = 455.44;Kt = 3.028;
m = 4.09; J = 0.0158;
w = 1*[157;0];
% yplant=0; uplant=0; 
ydata=[]; udata=[]; wdata=[]; tdata=[];
yplant=[0;-0.0001]; uplant=[0;0]; wdata1=[];
xplant=[0;0;-0.0001;0;0;0];set=[];xdata = [];
ymdata=[];noises=[];

Ap = [0 1 0 0;0 0 0 0;0 0 0 1; 0 0 K1/m 0];
Bp = [0 0;Kt/J 0;0 0;0 K2/m];
Cp = [0 1 0 0;0 0 1 0];

%%%%%% Get the size of A,B,C %%%%%%%
% [n,n] = size(Ap);
% [n,m1] = size(Bp);
% [p,n] = size(Cp);

A1 = [Ap Bp;[0 0 0 0;0 0 0 0] [1 0;0 1]];
B1 = [Bp;Cp*Bp];
C1 = [Cp [0 0;0 0]];
% A = [Ap Bp;[0 0 0 0;0 0 0 0] [1 0;0 1]];
% B = [Bp;Cp*Bp];
% C = [Cp [0 0;0 0]];
%%%%%%% Ts=0.0008 %%%%%%%%%
G = ss(A1,B1,C1,0);
G1 =c2d(G,Tref);
[A,B,C,D] = ssdata(G1);

% Define control horizon :
N1 = 1;
N2 = 20; %10;
Nu = 1;
lamda = 0.5;
% k=0;
% unit1 = zeros(Nu*m1,1);unit1(1:2,1) = 1;
% unit2 = zeros()
for t=N1:N2
    wdata = [wdata; w(:,1)];
    wdata1 = [wdata1; 2*w(:,1)];
end

  yk = yplant(:,1);
  xk = xplant(:,1);
        
for t=0:Tref:T_end
     %k = K+1;
   noise = sd*randn;
   ykm = (1+noise)*yk;
   xk(2:3,1)=(1+noise)*xk(2:3,1);
    
    [Phi, G] = predictions(A,B,C,N1,N2,Nu);
    [K1,K2] = MPC2(Phi,G,lamda);
    
if t>0.5
    uk = K1*wdata1 + K2*xk;
    set = [set, 2*w(:,1)];
else
    uk = K1*wdata + K2*xk;
    set = [set, w(:,1)];
end
%     tdata = [tdata;k-1]; wdata = [wdata; w];
%     ydata = [ydata;yk]; udata = [udata; uk];    
    
    tdata = [tdata,t]; %set = [set, w(:,1)];
    ydata = [ydata,yk]; udata = [udata, uk(1:2,:)];
    xdata = [xdata,xk]; ymdata = [ymdata,ykm];
    noises = [noises,noise];

    
    
    xk = A*xk + B*uk(1:2,1);
    yk = C*xk;
      
end

% subplot(411),plot(tdata, ydata(1,:)), ylabel('Output'), xlabel('Sample')
% subplot(412),plot(tdata, ydata(2,:)), ylabel('Control'), xlabel('Sample')
% subplot(413),plot(tdata, xdata(5,:)),ylabel('input1'), xlabel('Sample')
% subplot(414),plot(tdata, xdata(6,:)),ylabel('input2'), xlabel('Sample')
subplot(311),plot(tdata, ydata(1,:),tdata,set(1,:),':r'), ylabel('Speed'), xlabel('Time')
subplot(312),plot(tdata, ydata(2,:),tdata,set(2,:),':r'), ylabel('displacement'), xlabel('Time')
subplot(313),plot(tdata, xdata(5,:)),ylabel('Input(Iq)'), xlabel('Time')