www.pudn.com > predictive.rar > flywheelmpc.m, change:2009-04-02,size:2616b


% 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')