Changeset 913 for applications/dual

Timestamp:

04/21/10 16:50:13 (14 years ago)

Author:

smidl

Message:

upravy

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.15;%1.0015;
+OMEGAt = 2.15;%1.0015;
 
 %penalizace vstupu a rizeni
-v = 0.000001;%0.000001;
+v = 0.0001;%0.000001;
 w = 1;
 
@@ -69 +69 @@
 
 x0 = [0 0 1.0-OMEGAt pi/2 OMEGAt];
-P = diag([0.01, 0.01, 0.01, 0.01, 0]);
+P = diag([0.01, 0.01, 0.01, 0.5, 0]);
 
 %globalni promenne
@@ -89 +89 @@
     subplot(2, 3, 3);
     hold all
-    plot(1:Kt, OMEGAt*ones(1,Kt));
+%    plot(1:Kt, OMEGAt*ones(1,Kt));
     
 tic
@@ -97 +97 @@
     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);
+            for iterace = 1:1,
+                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;
+                    L(:, :, kt+k-2) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
+                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        
@@ -139 +141 @@
             xn(:, 1, n) = x00;
             for k = 1:Kt+K-1,               
-               u(:, k) = L(:, :, k)*(xn(:, k, n));               
+               u(:, k) = L(:, :, k)*(x_kalman(:, 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();
@@ -145 +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(); 
                xn(5, k+1, n) = xn(5, k, n);
+               
+               yn()=...;
+               x_kalman = ...;
             end