Changeset 893 for applications/dual

Show
Ignore:
Timestamp:
04/06/10 22:05:22 (16 years ago)
Author:
vahalam
Message:
 
Files:
1 modified

Legend:

Unmodified
Added
Removed

  • applications/dual/IterativeLocal/pmsm_lqg.m

    r890 r893  
    33 
    44%nastaveni algortimu 
    5 K = 1000; %casy 
     5K = 10; %casy 
     6Kt = 100; %test casy 
    67 
    7 N = 50; %vzorky 
    8 It = 5; %iterace 
     8N = 100; %vzorky 
     9It = 1; %iterace 
    910 
    1011 
    … …  
    3536 
    3637%penalizace vstupu a rizeni 
    37 v = 0.0001; 
     38v = 0.00001; 
    3839w = 1; 
    3940 
    … …  
    4445     0 0 DELTAt 1]; 
    4546  
    46 B = DELTAt*[c 0;... 
     47B = [c 0;... 
    4748            0 c;... 
    4849            0 0;... 
    … …  
    6869 
    6970%globalni promenne 
    70 u = zeros(2, K); 
    71 xs = zeros(4, K); 
    72 xn = zeros(4, K, N); 
     71u = zeros(2, Kt); 
     72xs = zeros(4, Kt); 
     73xn = zeros(4, Kt, N); 
    7374 
    7475S = zeros(4, 4, K); 
    75 L = zeros(2, 4, K); 
     76L = zeros(2, 4, Kt); 
    7677 
    7778%zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s 
    7879%rozptylem 
    79 sum = 0;%1;%0.01; 
    80 sumsim = 0;%1;%0.01; 
     80sum = 1;%0.01; 
     81sumsim = 1;%0.01; 
    8182neznalost = 1; 
    8283 
    … …  
    8788    %generovani stavu 
    8889    for n = 1:N, 
    89         xn(:, 1, n) = x0' - [0 0 OMEGAt 0]' + neznalost*sqrtm(P)*randn(4,1); 
    90         for k = 1:K-1, 
    91            tu =  L(:, :, k)*(xn(:, k, n)); 
    92            xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, k, n)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn();   
    93            xn(2, k+1, n) = a*xn(2, k, n) - b*xn(3, k, n)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn(); 
    94            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))) + sum*sqrt(Q(3, 3))*randn(); 
    95            xn(4, k+1, n) = xn(4, k, n) + xn(3, k, n)*DELTAt + sum*sqrt(Q(4, 4))*randn(); 
     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(); 
    96101        end 
    97102    end 
    … …  
    101106     
    102107    %napocteni S a L 
    103     S(:, :, K) = X; 
    104     for k = K-1:-1:1, 
    105         A(3, 1) = -e*sin(xs(4, k)); 
    106         A(3, 2) = e*cos(xs(4, k)); 
    107         A(1, 3) = b*sin(xs(4, k)); 
    108         A(2, 3) = -b*cos(xs(4, k)); 
    109         A(1, 4) = b*xs(3, k)*cos(xs(4, k)); 
    110         A(2, 4) = b*xs(3, k)*sin(xs(4, k)); 
    111         A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k)))); 
    112         S(:, :, k) = A'*(S(:, :, k+1) - S(:, :, k+1)*B*inv(B'*S(:, :, k+1)*B + Y)*B'*S(:, :, k+1))*A + X; 
    113         L(:, :, k) = -inv(B'*S(:, :, k+1)*B + Y)*B'*S(:, :, k+1)*A; 
    114     end     
     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 
    115130end 
    116131    toc 
    … …  
    120135    subplot(2, 3, 3); 
    121136    hold all 
    122     plot(1:K, OMEGAt*ones(1,K)); 
    123  
     137    plot(1:Kt, OMEGAt*ones(1,Kt)); 
     138% L(:,:,1) 
    124139    for n = 1:N, 
    125         xn(:, 1, n) = x0' - [0 0 OMEGAt 0]' + neznalost*sqrtm(P)*randn(4,1); 
    126         for k = 1:K-1, 
    127            tu =  L(:, :, k)*(xn(:, k, 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]'); 
    128143           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();   
    129144           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(); 
    130145           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(); 
    131146           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(); 
    132151            
    133            u(:, k) = tu; 
     152           u(:, k) = tu;           
    134153        end 
    135154         
    136         xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, K); 
     155%         xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt); 
    137156         
    138157        subplot(2, 3, 1); 
    139158        hold all 
    140         plot(1:K, xn(1, :, n)); 
     159        plot(1:Kt, xn(1, :, n)); 
    141160        title('i_{\alpha}'); 
    142161        subplot(2, 3, 2); 
    143162        hold all 
    144         plot(1:K, xn(2, :, n)); 
     163        plot(1:Kt, xn(2, :, n)); 
    145164        title('i_{\beta}'); 
    146165        subplot(2, 3, 3); 
    147166        hold all 
    148         plot(1:K, xn(3, :, n)); 
     167        plot(1:Kt, xn(3, :, n)); 
    149168        title('\omega'); 
    150169        subplot(2, 3, 4); 
    151170        hold all 
    152         plot(1:K, xn(4, :, n)); 
     171        plot(1:Kt, xn(4, :, n)); 
    153172        title('\theta'); 
    154173        subplot(2, 3, 5); 
    155174        hold all 
    156         plot(1:K, u(1, :)); 
     175        plot(1:Kt, u(1, :)); 
    157176        title('u_{\alpha}'); 
    158177        subplot(2, 3, 6); 
    159178        hold all 
    160         plot(1:K, u(2, :)); 
     179        plot(1:Kt, u(2, :)); 
    161180        title('u_{\beta}'); 
    162181    end