Changeset 905 for applications/dual/IterativeLocal

Timestamp:

04/14/10 11:24:04 (15 years ago)

Author:

vahalam

Message:

 

Files:

• applications/dual/IterativeLocal/pmsm_lqg.m (modified) (6 diffs)

applications/dual/IterativeLocal/pmsm_lqg.m

@@ -33 +33 @@
 e = DELTAt*ckp*cp*cp*cPSIpm/cJ;
 
-OMEGAt = 1.0015;
+OMEGAt = 1.15;%1.0015;
 
 %penalizace vstupu a rizeni
@@ -68 +68 @@
 R = diag([0.0006, 0.0006]);
 
-x0 = [0 0 1-OMEGAt pi/2 OMEGAt];
+x0 = [0 0 1.0-OMEGAt pi/2 OMEGAt];
 P = diag([0.01, 0.01, 0.01, 0.01, 0]);
 
@@ -81 +81 @@
 %zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s
 %rozptylem
-sum = 1;%0.01;
+% sum = 1;%0.01;
 sumsim = 1;%0.01;
 neznalost = 1;
 
-
+errnans = 0;
 
 % vycisti kreslici okno
@@ -141 +141 @@
             xn(:, 1, n) = x00;
             for k = 1:Kt+K-1,               
-               u(:, k) = L(:, :, k)*(xn(:, k, n));               
+               u(:, k) = L(:, :, k)*(xn(:, k, n)); %tady se vyuzije(k) / nevyuzije(1) receding horizon              
                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();
@@ -148 +148 @@
                xn(5, k+1, n) = xn(5, k, n);
             end
-
-
+            
+    %kontrola spatne L matice
+    lstore(:,:,n) = L(:,:,1);
+    if(isnan(sum(sum(L(:,:,1))))==1)
+        errnans = errnans + 1;
+    end
+    
     %vykresleni
         subplot(2, 3, 1);
@@ -179 +184 @@
 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
+if(errnans > 0)
+        lstore
+        disp('L is NaN')
+        errnans
+end
+
+