How can i control 2 output system by 1 input
    조회 수: 4 (최근 30일)
  
       이전 댓글 표시
    
I am designing model predictive controller for single input multi output system. When i tried to control siso or mimo system i can get the desired solution. But When i tried to control simo system which is 2 output 1 input system one of output goes 0 other one follows reference signal. I want to 2 of output follows reference signal. I am sharing the code below.
clc
clear all
close all
Cf = 19000 ;
Cr = 33000 ;
Iz = 2875 ;
Lf = 1.2 ;
Lr = 1.6 ;
m = 1575 ;
Vx = 15 ;
n = 4 ;
A = [  0, 1, Vx, 0 ;
      0, (-(2 * Cf + 2 * Cr)/(m * Vx)), 0, (-Vx -(2 * Cf * Lf - 2 * Cr * Lr)/(m * Vx) ) ;
      0, 0, 0, 1;
      0, (- (2 * Lf * Cf - 2 * Lr * Cr)/ (Iz * Vx)), 0, (- (2 * Lf^2 * Cf + 2 * Lr^2 * Cr) /(Iz * Vx))];
B = [0; ((2 * Cf) / m); 0; (2 * Lf * Cf / Iz)];
C = [1,0,0,0;
     0,0,1,0] ;
D= [0;0] ;
ts = 0.01;
[Ad, Bd, Cd, Dd] = c2dm(A,B,C,D,ts);
[state_size, X ] = size(Ad);
[output_size, X ] = size(Cd);
[X , number_of_inputs] = size(Bd);
 Anew = zeros(state_size + output_size) ;
 Anew(1 : state_size, 1:state_size) = Ad ;
 Anew(state_size+1 : state_size+output_size, 1 : state_size ) = Cd*Ad ;
 Anew(1 : state_size, state_size+1 : state_size+output_size ) = 0;
 Anew(state_size+1 : state_size+output_size , state_size+1 : state_size+output_size ) = eye(output_size);
 Bnew = zeros((state_size + output_size),number_of_inputs);
 Bnew(1:state_size,1:number_of_inputs) = Bd;
 Bnew(state_size+1 : state_size+output_size,:) = Cd * Bd ;
 Cnew = zeros(output_size,(state_size+output_size));
 Cnew(1:output_size, state_size+1 : state_size+output_size) = eye(output_size) ; 
 Dnew = zeros(number_of_inputs ,output_size);
aug_state_size = size(Anew); % augmented state size
Np = 20; % Predict Horizon
Nc = 10;  % Nc*2 > Np olursa hata var 
N_sim = 2000 ; % # of sample
[Fi, Fj] = size(Cnew * Anew);
F = zeros(Np * Fi ,Fj);
phi = zeros(Np * Fi , Nc); %Phi = Np*Fi X Nc 
%%%% calculating F
m = 1 ;
j = 1;
pow = 1;
for i = 1 : 2 : Np * Fi 
   dummy = Cnew * Anew^pow ;
   while j <= aug_state_size(2)        
       for k = 0 : output_size - 1
            F(i+k, j) = dummy(k + 1,j);
       end
%         F(i, j) = dummy(1,j);
%         F(i + 1 , j) = dummy(2, j)
        j = j + 1;
   end      
   j = 1 ;
   pow = pow +1;
end
%%%% calculating phi
i = 1 ; 
k = 0 ;
m = 1 ;
for j = 1 : Nc * number_of_inputs
    while (i <= Np)
%         phi(i,j) = Cnew*Anew^(i-1 - k) * Bnew;
          dummy = Cnew * Anew^(i-1 - k) * Bnew;
          phi(m,j) = dummy(1);
          phi(m + 1, j) = dummy(2);
          i = i + 1;    
          m = m + 2;
    end 
   i = j + 1 ;
   m = j + i ;
   k = k + 1 ;
end
phiT_phi = phi' * phi ;
phiT_F = phi' * F  ;
% barRs =  ones(Np * Fi ,2);  
% phiTRs = phi' * barRs;
xm = [0;0;0;0]; %initial states
[new_state_size, unnecc] = size(Bnew);
Xf = zeros(new_state_size,1) ;  % Augmented states
phiTRs = (phi' * F);
phiTRs = phiTRs(:, state_size+1 : state_size + output_size); % last # of output of phi'*F columns 
r =  ones(output_size, N_sim) ;  % referenece signal
u = [0];
y = [0;0];
rw = 0.1;
% rw is used as a tunning parameter for the desired closed loop performance
for kk = 1 : N_sim 
     deltaU =  (phi' * phi + rw * eye(Nc * number_of_inputs) )^-1  * (phiTRs * r(:,kk)  - phi' * F * Xf) ;
     deltaUs(kk) = deltaU(1);
     u = u + deltaU(1) ;
%      u1(kk) = u ;
     xm_old = xm ;
     xm = Ad * xm + Bd * u ;
     y(:,kk) = Cd * xm ;
%      
     Xf(1:4) = xm - xm_old ;
     Xf(5:6) = y(1:2,kk);
end
k = 0 : (N_sim -1);
figure
plot(k,y(1,:))
ylabel(' lateral position ')
grid on
figure
plot(k,y(2,:))
ylabel(' yaw angle  ')
grid on
figure
plot(k,deltaUs(:))
ylabel(' steering angle ')
grid on
댓글 수: 0
답변 (0개)
참고 항목
카테고리
				Help Center 및 File Exchange에서 Introduction to Installation and Licensing에 대해 자세히 알아보기
			
	Community Treasure Hunt
Find the treasures in MATLAB Central and discover how the community can help you!
Start Hunting!
