Changeset 913 for applications

Show
Ignore:
Timestamp:
04/21/10 16:50:13 (15 years ago)
Author:
smidl
Message:

upravy

Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/dual/IterativeLocal/pmsm_lqg.m

    r909 r913  
    3333e = DELTAt*ckp*cp*cp*cPSIpm/cJ; 
    3434 
    35 OMEGAt = 1.15;%1.0015; 
     35OMEGAt = 2.15;%1.0015; 
    3636 
    3737%penalizace vstupu a rizeni 
    38 v = 0.000001;%0.000001; 
     38v = 0.0001;%0.000001; 
    3939w = 1; 
    4040 
    … …  
    6969 
    7070x0 = [0 0 1.0-OMEGAt pi/2 OMEGAt]; 
    71 P = diag([0.01, 0.01, 0.01, 0.01, 0]); 
     71P = diag([0.01, 0.01, 0.01, 0.5, 0]); 
    7272 
    7373%globalni promenne 
    … …  
    8989    subplot(2, 3, 3); 
    9090    hold all 
    91     plot(1:Kt, OMEGAt*ones(1,Kt)); 
     91%    plot(1:Kt, OMEGAt*ones(1,Kt)); 
    9292     
    9393tic 
    … …  
    9797    L = zeros(2, 5, Kt+K); 
    9898    %iterace 
    99 %     for iterace = 1:It, 
    10099    x00 = x0' + neznalost*sqrt(P)*randn(5,1);    
    101100        %testovaci casy 
    102101        for kt = 1:Kt, 
    103102            %generovani stavu - jen pro horizont 
    104             xn(:, 1, n) = x00;             
    105             for k = 1:kt+K-1,                
    106                tu = L(:, :, k)*(xn(:, k, n));                
    107                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();   
    108                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(); 
    109                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(); 
    110                xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn();  
    111                xn(5, k+1, n) = xn(5, k, n); 
    112             end 
    113             %prumerny stav 
    114             xs = xn(:, :, n);%mean(xn, 3); 
    115              
    116             %receding horizon 
    117             S(:, :, K) = X; 
    118             for k = K:-1:2,                
    119                 A(3, 1) = -e*sin(xs(4, k+kt-1)); 
    120                 A(3, 2) = e*cos(xs(4, k+kt-1)); 
    121                 A(1, 3) = b*sin(xs(4, k+kt-1)); 
    122                 A(2, 3) = -b*cos(xs(4, k+kt-1)); 
    123                 A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1)); 
    124                 A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1));     
    125                 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)))); 
    126                 A(1, 5) = b*sin(xs(4, k+kt-1)); 
    127                 A(2, 5) = -b*cos(xs(4, k+kt-1)); 
    128                 S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X;                          
    129             end 
    130             L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; 
    131             %spocital kt-te rizeni a vsechna dalsi nahradi jim 
    132             for k = kt+1:kt+K-1, 
    133                 L(:, :, k) = L(:, :, kt); 
     103            for iterace = 1:1, 
     104                xn(:, 1, n) = x00; 
     105                for k = 1:kt+K-1, 
     106                    tu = L(:, :, k)*(xn(:, k, n)); 
     107                    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(); 
     108                    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(); 
     109                    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(); 
     110                    xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt;% + sumsim*sqrt(Q(4, 4))*randn(); 
     111                    xn(5, k+1, n) = xn(5, k, n); 
     112                end 
     113                %prumerny stav 
     114                xs = xn(:, :, n);%mean(xn, 3); 
     115 
     116                %receding horizon 
     117                S(:, :, K) = X; 
     118                for k = K:-1:2, 
     119                    A(3, 1) = -e*sin(xs(4, k+kt-1)); 
     120                    A(3, 2) = e*cos(xs(4, k+kt-1)); 
     121                    A(1, 3) = b*sin(xs(4, k+kt-1)); 
     122                    A(2, 3) = -b*cos(xs(4, k+kt-1)); 
     123                    A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1)); 
     124                    A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1)); 
     125                    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)))); 
     126                    A(1, 5) = b*sin(xs(4, k+kt-1)); 
     127                    A(2, 5) = -b*cos(xs(4, k+kt-1)); 
     128                    S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X; 
     129                    L(:, :, kt+k-2) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; 
     130                end 
     131%                L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A; 
     132                %spocital kt-te rizeni a vsechna dalsi nahradi jim 
     133%                 for k = kt+1:kt+K-1, 
     134%                     L(:, :, k) = L(:, :, kt); 
     135%                 end 
    134136            end 
    135137        end         
    … …  
    139141            xn(:, 1, n) = x00; 
    140142            for k = 1:Kt+K-1,                
    141                u(:, k) = L(:, :, k)*(xn(:, k, n));                
     143               u(:, k) = L(:, :, k)*(x_kalman(:, k, n));   
    142144               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();   
    143145               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(); 
    … …  
    145147               xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn();  
    146148               xn(5, k+1, n) = xn(5, k, n); 
     149                
     150               yn()=...; 
     151               x_kalman = ...; 
    147152            end 
    148153