Changeset 898

Show
Ignore:
Timestamp:
04/09/10 11:58:01 (16 years ago)
Author:
vahalam
Message:
 
Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/dual/IterativeLocal/pmsm_lqg.m

    r893 r898  
    66Kt = 100; %test casy 
    77 
    8 N = 100; %vzorky 
     8N = 50; %vzorky 
    99It = 1; %iterace 
    1010 
    … …  
    3636 
    3737%penalizace vstupu a rizeni 
    38 v = 0.00001; 
     38v = 0.000001;%0.000001; 
    3939w = 1; 
    4040 
    4141%matice modelu 
    42 A = [a 0 0 0;... 
    43      0 a 0 0;... 
    44      0 0 d 0;... 
    45      0 0 DELTAt 1]; 
     42A = [a 0 0 0 0;... 
     43     0 a 0 0 0;... 
     44     0 0 d 0 (d-1);... 
     45     0 0 DELTAt 1 DELTAt;... 
     46     0 0 0 0 1]; 
    4647  
    4748B = [c 0;... 
    48             0 c;... 
    49             0 0;... 
    50             0 0]; 
     49     0 c;... 
     50     0 0;... 
     51     0 0;... 
     52     0 0]; 
    5153  
    5254% C = [1 0 0 0;... 
    5355%      0 1 0 0]; 
    5456  
    55 X = [0 0 0 0;... 
    56      0 0 0 0;... 
    57      0 0 w 0;... 
    58      0 0 0 0]; 
     57X = [0 0 0 0 0;... 
     58     0 0 0 0 0;... 
     59     0 0 w 0 0;... 
     60     0 0 0 0 0;... 
     61     0 0 0 0 0]; 
    5962  
    6063Y = [v 0;... 
    … …  
    6568R = diag([0.0006, 0.0006]); 
    6669 
    67 x0 = [0 0 1 pi/2]; 
    68 P = diag([0.01, 0.01, 0.01, 0.01]); 
     70x0 = [0 0 1-OMEGAt pi/2 OMEGAt]; 
     71P = diag([0.01, 0.01, 0.01, 0.01, 0]); 
    6972 
    7073%globalni promenne 
    71 u = zeros(2, Kt); 
    72 xs = zeros(4, Kt); 
    73 xn = zeros(4, Kt, N); 
    74  
    75 S = zeros(4, 4, K); 
    76 L = zeros(2, 4, Kt); 
     74u = zeros(2, Kt+K); 
     75xs = zeros(5, Kt+K); 
     76xn = zeros(5, Kt+K, N); 
     77 
     78S = zeros(5, 5, K); 
     79L = zeros(2, 5, Kt+K); 
    7780 
    7881%zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s 
    … …  
    8285neznalost = 1; 
    8386 
    84 tic 
    85  
    86 %hlavni iteracni smycka 
    87 for iterace = 1:It, 
    88     %generovani stavu 
    89     for n = 1:N, 
    90         xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); 
    91         for k = 1:Kt-1, 
    92            tu =  L(:, :, k)*(xn(:, k, n) - [0 0 OMEGAt 0]'); 
    93            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();   
    94            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(); 
    95            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(); 
    96            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn();            
    97 %            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();   
    98 %            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(); 
    99 %            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(); 
    100 %            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); 
    101         end 
    102     end 
    103      
    104     %prumerny stav 
    105     xs = mean(xn, 3); 
    106      
    107     %napocteni S a L 
    108     for kt = 1:Kt-1, 
    109 %     receding horizon 
    110      if((K-1+kt)<Kt) 
    111         S(:, :, K) = X; 
    112         for k = K-1+kt-1:-1:1+kt-1, 
    113             A(3, 1) = -e*sin(xs(4, k)); 
    114             A(3, 2) = e*cos(xs(4, k)); 
    115             A(1, 3) = b*sin(xs(4, k)); 
    116             A(2, 3) = -b*cos(xs(4, k)); 
    117             A(1, 4) = b*(xs(3, k))*cos(xs(4, k)); 
    118             A(2, 4) = b*(xs(3, k))*sin(xs(4, k)); 
    119 %             A(1, 4) = b*(xs(3, k)+OMEGAt)*cos(xs(4, k)); 
    120 %             A(2, 4) = b*(xs(3, k)+OMEGAt)*sin(xs(4, k)); 
    121             A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k)))); 
    122             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;         
    123         end   
    124         L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; 
    125      else 
    126         L(:, :, kt) = L(:, :, kt-1); %kopiruje poslednich K kroku z Kt kde to nejde na K predpocitat 
    127      end 
    128          
    129     end 
    130 end 
    131     toc 
    132  
    133 %vysledky 
     87 
     88 
     89% vycisti kreslici okno 
    13490    clf 
    13591    subplot(2, 3, 3); 
    13692    hold all 
    13793    plot(1:Kt, OMEGAt*ones(1,Kt)); 
    138 % L(:,:,1) 
    139     for n = 1:N, 
    140         xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); 
    141         for k = 1:Kt-1, 
    142            tu =  L(:, :, 1)*(xn(:, k, n) - [0 0 OMEGAt 0]'); 
    143            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();   
    144            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(); 
    145            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(); 
    146            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); 
    147 %            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();   
    148 %            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(); 
    149 %            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(); 
    150 %            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); 
    151             
    152            u(:, k) = tu;           
    153         end 
     94     
     95tic 
     96 
     97% vzorky stavu 
     98for n = 1:N, 
     99    L = zeros(2, 5, Kt+K); 
     100    %iterace 
     101%     for iterace = 1:It, 
     102    x00 = x0' + neznalost*sqrt(P)*randn(5,1);    
     103        %testovaci casy 
     104        for kt = 1:Kt, 
     105            %generovani stavu - jen pro horizont 
     106            xn(:, 1, n) = x00;             
     107            for k = 1:kt+K-1,                
     108               tu = L(:, :, k)*(xn(:, k, n));                
     109               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();   
     110               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(); 
     111               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(); 
     112               xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn();  
     113               xn(5, k+1, n) = xn(5, k, n); 
     114            end 
     115            %prumerny stav 
     116            xs = xn(:, :, n);%mean(xn, 3); 
     117             
     118            %receding horizon 
     119            S(:, :, K) = X; 
     120            for k = K:-1:2,                
     121                A(3, 1) = -e*sin(xs(4, k+kt-1)); 
     122                A(3, 2) = e*cos(xs(4, k+kt-1)); 
     123                A(1, 3) = b*sin(xs(4, k+kt-1)); 
     124                A(2, 3) = -b*cos(xs(4, k+kt-1)); 
     125                A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1)); 
     126                A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1));     
     127                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)))); 
     128                A(1, 5) = b*sin(xs(4, k+kt-1)); 
     129                A(2, 5) = -b*cos(xs(4, k+kt-1)); 
     130                S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X;                          
     131            end 
     132            L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; 
     133            %spocital kt-te rizeni a vsechna dalsi nahradi jim 
     134            for k = kt+1:kt+K-1, 
     135                L(:, :, k) = L(:, :, kt); 
     136            end 
     137        end         
    154138         
    155 %         xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt); 
    156          
     139%     end 
     140        %napocte trajektorii pro vykresleni s kompletnim rizenim 
     141            xn(:, 1, n) = x00; 
     142            for k = 1:Kt+K-1,                
     143               u(:, k) = L(:, :, k)*(xn(:, k, n));                
     144               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();   
     145               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(); 
     146               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(); 
     147               xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn();  
     148               xn(5, k+1, n) = xn(5, k, n); 
     149            end 
     150 
     151 
     152    %vykresleni 
    157153        subplot(2, 3, 1); 
    158154        hold all 
    159         plot(1:Kt, xn(1, :, n)); 
     155        plot(1:Kt, xn(1, 1:Kt, n)); 
    160156        title('i_{\alpha}'); 
    161157        subplot(2, 3, 2); 
    162158        hold all 
    163         plot(1:Kt, xn(2, :, n)); 
     159        plot(1:Kt, xn(2, 1:Kt, n)); 
    164160        title('i_{\beta}'); 
    165161        subplot(2, 3, 3); 
    166162        hold all 
    167         plot(1:Kt, xn(3, :, n)); 
     163        plot(1:Kt, xn(3, 1:Kt, n) + xn(5, 1:Kt, n)); 
    168164        title('\omega'); 
    169165        subplot(2, 3, 4); 
    170166        hold all 
    171         plot(1:Kt, xn(4, :, n)); 
     167        plot(1:Kt, xn(4, 1:Kt, n)); 
    172168        title('\theta'); 
    173169        subplot(2, 3, 5); 
    174170        hold all 
    175         plot(1:Kt, u(1, :)); 
     171        plot(1:Kt, u(1, 1:Kt)); 
    176172        title('u_{\alpha}'); 
    177173        subplot(2, 3, 6); 
    178174        hold all 
    179         plot(1:Kt, u(2, :)); 
    180         title('u_{\beta}'); 
    181     end 
    182  
    183  
     175        plot(1:Kt, u(2, 1:Kt)); 
     176        title('u_{\beta}');     
    184177end 
     178 
     179toc 
     180 
     181% %hlavni iteracni smycka 
     182% for iterace = 1:It, 
     183%     %generovani stavu 
     184%     for n = 1:N, 
     185%         xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); 
     186%         for k = 1:Kt-1, 
     187%            tu =  L(:, :, k)*(xn(:, k, n) - [0 0 OMEGAt 0]'); 
     188%            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();   
     189%            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(); 
     190%            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(); 
     191%            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn();            
     192% %            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();   
     193% %            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(); 
     194% %            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(); 
     195% %            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); 
     196%         end 
     197%     end 
     198%      
     199%     %prumerny stav 
     200%     xs = mean(xn, 3); 
     201%      
     202%     %napocteni S a L 
     203%     for kt = 1:Kt-1, 
     204% %     receding horizon 
     205%      if((K-1+kt)<Kt) 
     206%         S(:, :, K) = X; 
     207%         for k = K-1+kt-1:-1:1+kt-1, 
     208%             A(3, 1) = -e*sin(xs(4, k)); 
     209%             A(3, 2) = e*cos(xs(4, k)); 
     210%             A(1, 3) = b*sin(xs(4, k)); 
     211%             A(2, 3) = -b*cos(xs(4, k)); 
     212%             A(1, 4) = b*(xs(3, k))*cos(xs(4, k)); 
     213%             A(2, 4) = b*(xs(3, k))*sin(xs(4, k)); 
     214% %             A(1, 4) = b*(xs(3, k)+OMEGAt)*cos(xs(4, k)); 
     215% %             A(2, 4) = b*(xs(3, k)+OMEGAt)*sin(xs(4, k)); 
     216%             A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k)))); 
     217%             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;         
     218%         end   
     219%         L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; 
     220%      else 
     221%         L(:, :, kt) = L(:, :, kt-1); %kopiruje poslednich K kroku z Kt kde to nejde na K predpocitat 
     222%      end 
     223%          
     224%     end 
     225% end 
     226%     toc 
     227%  
     228% %vysledky 
     229%     clf 
     230%     subplot(2, 3, 3); 
     231%     hold all 
     232%     plot(1:Kt, OMEGAt*ones(1,Kt)); 
     233% % L(:,:,1) 
     234%     for n = 1:N, 
     235%         xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1); 
     236%         for k = 1:Kt-1, 
     237%            tu =  L(:, :, 1)*(xn(:, k, n) - [0 0 OMEGAt 0]'); 
     238%            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();   
     239%            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(); 
     240%            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(); 
     241%            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); 
     242% %            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();   
     243% %            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(); 
     244% %            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(); 
     245% %            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn(); 
     246%             
     247%            u(:, k) = tu;           
     248%         end 
     249%          
     250% %         xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt); 
     251%          
     252%         subplot(2, 3, 1); 
     253%         hold all 
     254%         plot(1:Kt, xn(1, :, n)); 
     255%         title('i_{\alpha}'); 
     256%         subplot(2, 3, 2); 
     257%         hold all 
     258%         plot(1:Kt, xn(2, :, n)); 
     259%         title('i_{\beta}'); 
     260%         subplot(2, 3, 3); 
     261%         hold all 
     262%         plot(1:Kt, xn(3, :, n)); 
     263%         title('\omega'); 
     264%         subplot(2, 3, 4); 
     265%         hold all 
     266%         plot(1:Kt, xn(4, :, n)); 
     267%         title('\theta'); 
     268%         subplot(2, 3, 5); 
     269%         hold all 
     270%         plot(1:Kt, u(1, :)); 
     271%         title('u_{\alpha}'); 
     272%         subplot(2, 3, 6); 
     273%         hold all 
     274%         plot(1:Kt, u(2, :)); 
     275%         title('u_{\beta}'); 
     276%     end 
     277 
     278 
     279end