root/applications/dual/IterativeLocal/pmsm_lqg.m @ 900

function pmsm_lqg
% rizeni pmsm motoru - jednoduchy lqg algoritmus

%nastaveni algortimu
K = 10; %casy
Kt = 100; %test casy

N = 50; %vzorky
It = 1; %iterace


%konstanty modelu
DELTAt = 0.000125;

cRs = 0.28;
cLs = 0.003465;
cPSIpm = 0.1989;
ckp = 1.5;
cp = 4.0;
cJ = 0.04;
cB = 0;

% a = 0.9898;
% b = 0.0072;
% c = 0.0361;
% d = 1;
% e = 0.0149; 

a = 1 - DELTAt*cRs/cLs;
b = DELTAt*cPSIpm/cLs;
c = DELTAt/cLs;
d = 1 - DELTAt*cB/cJ;
e = DELTAt*ckp*cp*cp*cPSIpm/cJ;

OMEGAt = 1.0015;

%penalizace vstupu a rizeni
v = 0.000001;%0.000001;
w = 1;

%matice modelu
A = [a 0 0 0 0;...
     0 a 0 0 0;...
     0 0 d 0 (d-1);...
     0 0 DELTAt 1 DELTAt;...
     0 0 0 0 1];
 
B = [c 0;...
     0 c;...
     0 0;...
     0 0;...
     0 0];
 
% C = [1 0 0 0;...
%      0 1 0 0];
 
X = [0 0 0 0 0;...
     0 0 0 0 0;...
     0 0 w 0 0;...
     0 0 0 0 0;...
     0 0 0 0 0];
 
Y = [v 0;...
     0 v];
 
%pocatecni nastaveni
Q = diag([0.0013, 0.0013, 5e-6, 1e-10]);
R = diag([0.0006, 0.0006]);

x0 = [0 0 1-OMEGAt pi/2 OMEGAt];
P = diag([0.01, 0.01, 0.01, 0.01, 0]);

%globalni promenne
u = zeros(2, Kt+K);
xs = zeros(5, Kt+K);
xn = zeros(5, Kt+K, N);

S = zeros(5, 5, K);
L = zeros(2, 5, Kt+K);

%zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s
%rozptylem
sum = 1;%0.01;
sumsim = 1;%0.01;
neznalost = 1;



% vycisti kreslici okno
    clf
    subplot(2, 3, 3);
    hold all
    plot(1:Kt, OMEGAt*ones(1,Kt));
    
tic

% vzorky stavu
for n = 1:N,
    L = zeros(2, 5, Kt+K);
    %iterace
%     for iterace = 1:It,
    x00 = x0' + neznalost*sqrt(P)*randn(5,1);   
        %testovaci casy
        for kt = 1:Kt,
            %generovani stavu - jen pro horizont
            xn(:, 1, n) = x00;            
            for k = 1:kt+K-1,               
               tu = L(:, :, k)*(xn(:, k, n));               
               xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n) + xn(5, k, n))*sin(xn(4, k, n)) + c*tu(1) + sumsim*sqrt(Q(1, 1))*randn();  
               xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n) + xn(5, k, n))*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn();
               xn(3, k+1, n) = d*xn(3, k, n) + (d-1)*xn(5, k, n) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sumsim*sqrt(Q(3, 3))*randn();
               xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); 
               xn(5, k+1, n) = xn(5, k, n);
            end
            %prumerny stav
            xs = xn(:, :, n);%mean(xn, 3);
            
            %receding horizon
            S(:, :, K) = X;
            for k = K:-1:2,               
                A(3, 1) = -e*sin(xs(4, k+kt-1));
                A(3, 2) = e*cos(xs(4, k+kt-1));
                A(1, 3) = b*sin(xs(4, k+kt-1));
                A(2, 3) = -b*cos(xs(4, k+kt-1));
                A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1));
                A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1));    
                A(3, 4) = -e*(xs(2, k+kt-1)*sin(xs(4, k+kt-1) + xs(1,k+kt-1)*cos(xs(4, k+kt-1))));
                A(1, 5) = b*sin(xs(4, k+kt-1));
                A(2, 5) = -b*cos(xs(4, k+kt-1));
                S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X;                         
            end
            L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
            %spocital kt-te rizeni a vsechna dalsi nahradi jim
            for k = kt+1:kt+K-1,
                L(:, :, k) = L(:, :, kt);
            end
        end        
        
%     end
        %napocte trajektorii pro vykresleni s kompletnim rizenim
            xn(:, 1, n) = x00;
            for k = 1:Kt+K-1,               
               u(:, k) = L(:, :, k)*(xn(:, k, n));               
               xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n) + xn(5, k, n))*sin(xn(4, k, n)) + c*u(1, k) + sumsim*sqrt(Q(1, 1))*randn();  
               xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n) + xn(5, k, n))*cos(xn(4, k, n)) + c*u(2, k) + sumsim*sqrt(Q(2, 2))*randn();
               xn(3, k+1, n) = d*xn(3, k, n) + (d-1)*xn(5, k, n) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sumsim*sqrt(Q(3, 3))*randn();
               xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); 
               xn(5, k+1, n) = xn(5, k, n);
            end


    %vykresleni
        subplot(2, 3, 1);
        hold all
        plot(1:Kt, xn(1, 1:Kt, n));
        title('i_{\alpha}');
        subplot(2, 3, 2);
        hold all
        plot(1:Kt, xn(2, 1:Kt, n));
        title('i_{\beta}');
        subplot(2, 3, 3);
        hold all
        plot(1:Kt, xn(3, 1:Kt, n) + xn(5, 1:Kt, n));
        title('\omega');
        subplot(2, 3, 4);
        hold all
        plot(1:Kt, xn(4, 1:Kt, n));
        title('\theta');
        subplot(2, 3, 5);
        hold all
        plot(1:Kt, u(1, 1:Kt));
        title('u_{\alpha}');
        subplot(2, 3, 6);
        hold all
        plot(1:Kt, u(2, 1:Kt));
        title('u_{\beta}');    
end

toc

% %hlavni iteracni smycka
% for iterace = 1:It,
%     %generovani stavu
%     for n = 1:N,
%         xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1);
%         for k = 1:Kt-1,
%            tu =  L(:, :, k)*(xn(:, k, n) - [0 0 OMEGAt 0]');
%            xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, k, n)*sin(xn(4, k, n)) + c*tu(1) + sumsim*sqrt(Q(1, 1))*randn();  
%            xn(2, k+1, n) = a*xn(2, k, n) - b*xn(3, k, n)*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn();
%            xn(3, k+1, n) = d*xn(3, k, n) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sumsim*sqrt(Q(3, 3))*randn();
%            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn();           
% %            xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n)+OMEGAt)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn();  
% %            xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n)+OMEGAt)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn();
% %            xn(3, k+1, n) = -OMEGAt + d*(xn(3, k, n)+OMEGAt) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sum*sqrt(Q(3, 3))*randn();
% %            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn();
%         end
%     end
%     
%     %prumerny stav
%     xs = mean(xn, 3);
%     
%     %napocteni S a L
%     for kt = 1:Kt-1,
% %     receding horizon
%      if((K-1+kt)<Kt)
%         S(:, :, K) = X;
%         for k = K-1+kt-1:-1:1+kt-1,
%             A(3, 1) = -e*sin(xs(4, k));
%             A(3, 2) = e*cos(xs(4, k));
%             A(1, 3) = b*sin(xs(4, k));
%             A(2, 3) = -b*cos(xs(4, k));
%             A(1, 4) = b*(xs(3, k))*cos(xs(4, k));
%             A(2, 4) = b*(xs(3, k))*sin(xs(4, k));
% %             A(1, 4) = b*(xs(3, k)+OMEGAt)*cos(xs(4, k));
% %             A(2, 4) = b*(xs(3, k)+OMEGAt)*sin(xs(4, k));
%             A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k))));
%             S(:, :, k-kt+1) = A'*(S(:, :, k-kt+2) - S(:, :, k-kt+2)*B*inv(B'*S(:, :, k-kt+2)*B + Y)*B'*S(:, :, k-kt+2))*A + X;        
%         end  
%         L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
%      else
%         L(:, :, kt) = L(:, :, kt-1); %kopiruje poslednich K kroku z Kt kde to nejde na K predpocitat
%      end
%         
%     end
% end
%     toc
% 
% %vysledky
%     clf
%     subplot(2, 3, 3);
%     hold all
%     plot(1:Kt, OMEGAt*ones(1,Kt));
% % L(:,:,1)
%     for n = 1:N,
%         xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1);
%         for k = 1:Kt-1,
%            tu =  L(:, :, 1)*(xn(:, k, n) - [0 0 OMEGAt 0]');
%            xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, k, n)*sin(xn(4, k, n)) + c*tu(1) + sumsim*sqrt(Q(1, 1))*randn();  
%            xn(2, k+1, n) = a*xn(2, k, n) - b*xn(3, k, n)*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn();
%            xn(3, k+1, n) = d*xn(3, k, n) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sumsim*sqrt(Q(3, 3))*randn();
%            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn();
% %            xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n)+OMEGAt)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn();  
% %            xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n)+OMEGAt)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn();
% %            xn(3, k+1, n) = -OMEGAt + d*(xn(3, k, n)+OMEGAt) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sum*sqrt(Q(3, 3))*randn();
% %            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn();
%            
%            u(:, k) = tu;          
%         end
%         
% %         xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt);
%         
%         subplot(2, 3, 1);
%         hold all
%         plot(1:Kt, xn(1, :, n));
%         title('i_{\alpha}');
%         subplot(2, 3, 2);
%         hold all
%         plot(1:Kt, xn(2, :, n));
%         title('i_{\beta}');
%         subplot(2, 3, 3);
%         hold all
%         plot(1:Kt, xn(3, :, n));
%         title('\omega');
%         subplot(2, 3, 4);
%         hold all
%         plot(1:Kt, xn(4, :, n));
%         title('\theta');
%         subplot(2, 3, 5);
%         hold all
%         plot(1:Kt, u(1, :));
%         title('u_{\alpha}');
%         subplot(2, 3, 6);
%         hold all
%         plot(1:Kt, u(2, :));
%         title('u_{\beta}');
%     end


end