Index: applications/dual/vahala/kim/basic_main.m
===================================================================
--- applications/dual/vahala/kim/basic_main.m (revision 1435)
+++ applications/dual/vahala/kim/basic_main.m (revision 1436)
@@ -1,228 +1,283 @@
-% main - hlavni skript
-clear all;
-% oznaceni: s ... system
-%           k ... kalman (EKF)
-%           l ... rizeni (LQR)
-
-% KONSTANTY
-T = 40000; %horizont
-dt = 0.000125; %casovy krok
-
-% Rs = 0.28;
-% Ls = 0.003465;
-% psipm = 0.1989;
-% B = 0;    
-% kp = 1.5;
-% pp = 4.0;
-% J = 0.04;
-
-% Lq = 1.05*Ls;
-% Ld = 0.95*Ls;
-
-a = 0.9898;
-b = 0.0072;
-c = 0.0361;
-d = 1.0;
-e = 0.0149;
-
-Ls = 0.003465;
-psipm = 0.1989;
-
-% ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200;
-ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
-
-%kovariance EKF na stavu
-% Q_k = diag([0.001, 0.00001]);
-% R_k = diag([0.015, 0.015]);
-Q_k = diag([0.01, 0.0001]);
-R_k = diag([0.15, 0.15]);
-
-%hodnoty sumu v systemu
-nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]);
-nR = diag([0.0006, 0.0006]);
-
-iter_l = 10;% pocet iteraci ve vypoctu rizeni
-
-% B_l = zeros(3,2);
-% B_l = zeros(2,2);
-% B_l(1,1) = c;
-% B_l(2,2) = c;
-
-Q_l = diag([1 0 0]);
-% % Q_l = diag([0 0 1 0 0]);
-r = 0.01;
-R_l = diag([r, r]);
-
-% PROMENNE
-x_s = zeros(4,T); %stav
-y_s = zeros(2,T); %mereni
-x_k = zeros(2,T); %odhad stavu
-P_k = zeros(2); %kovariance stavu
-u_l = zeros(2,T); %rizeni
-% S_l = zeros(3); %jadro ztraty
-S_l = zeros(2);
-
-% POCATECNI HODNOTY
-noise = 1; %prepinac sumu
-% noise = 0;
-
-theta0 = 1.5;%1.7; %pocatecni poloha
-P0 = eye(2); %odhad pocatecni kovariance stavu (apriorni)
-% ST = zeros(3); %koncova ztrata
-ST = ones(3);
-
-
-% INICIALIZACE
-x_s(4,1) = theta0;
-% x_s(3,1) = 5;
-P_k = P0;
-S_l = ST;
-
-ref_ome = zeros(1, T);  
-    for k = 1:T,
-           index = floor(k*dt);
-           if(index>0)
-               lower = ref_profile(index);
-           else
-               lower = 0;
-           end
-           if(index<T*dt)
-               upper = ref_profile(index+1);
-           else
-               upper = 0;
-           end
-           ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
+function [loss] = basic_main(T, ref_profile, theta0, simulator, graf, inddq)
+    % basic main - hlavni skript
+    % clear all;
+    % oznaceni: s ... system
+    %           k ... kalman (EKF)
+    %           l ... rizeni (LQR)
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %%%%%pouziti SIMULATORU
+    % simulator = 1;
+    % simulator = 0;
+
+    if((simulator == 1)||(simulator == 10))
+        sim_param = pmsm_sim;
+    %     sim_param(9) = 0; %vypne dead-time
+        pmsm_sim(sim_param);
     end
-% ref_ome = 0*ones(1, T);
-
-% Derivace pro prvni EKF 
-ome = x_k(1,1);
-the = x_k(2,1);    
-ia = y_s(1,1);
-ib = y_s(2,1);
-A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0];
-C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)];
-
-
-ri = 0.0001;
-ai = (1-a*a)/c/c;
-Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2;
-Li = a*c*Si/(c*c*Si+ri);
-    
-A_l = [d,0,0;dt,1,dt;0,0,1];
-% B_l = zeros(2);
-% % A_l = [a 0 0 0 0; 0 a 0 0 0; 0 0 d 0 (d-1); 0 0 dt 1 dt; 0 0 0 0 1];
-% % B_l = zeros(5,2);
-% % B_l(1:2,1:2) = [c 0;0 c];
-
-%PI vektorove
-% kon_pi = 3.0;
-% kon_ii = 0.00375;
-% kon_pu = 20.0;
-% kon_iu = 0.05;
-% sum_iq = 0;
-% sum_ud = 0;
-% sum_uq = 0;
-
-
-
-% HLAVNI SMYCKA
-for t = 1:T-1,
-    % EKF      
-    Pp = A*P_k*A' + Q_k;
-    S = C*Pp*C' + R_k;
-    K = Pp*C'/S;
-    P_k = Pp - K*C*Pp;    
-    
-    xp = zeros(2,1);
-    xp(1) = d*x_k(1,t) + e*(y_s(2,t)*cos(x_k(2,t)) - y_s(1,t)*sin(x_k(2,t)));
-    xp(2) = x_k(2,t) + dt*x_k(1,t);    
-    yp = zeros(2,1);
-    yp(1) = a*y_s(1,t) + b*x_k(1,t)*sin(x_k(2,t)) + c*u_l(1,t);
-    yp(2) = a*y_s(2,t) - b*x_k(1,t)*cos(x_k(2,t)) + c*u_l(2,t);
-    
-    x_k(:,t+1) = xp + K*(y_s(:,t) - yp);
-    
-    %!!!
-%     tmp = x_k(:,t+1);
-%     x_k(:,t+1) = x_s(3:4,t);
-    
-    % Derivace    
-    ome = x_k(1,t+1);
-    the = x_k(2,t+1);    
-    ia = y_s(1,t);
-    ib = y_s(2,t);
+    %%%%%
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+    % KONSTANTY
+    % T = 120000; %horizont
+    dt = 0.000125; %casovy krok
+
+    % Rs = 0.28;
+    % Ls = 0.003465;
+    % psipm = 0.1989;
+    % B = 0;    
+    % kp = 1.5;
+    % pp = 4.0;
+    % J = 0.04;
+
+    % Lq = 1.05*Ls;
+    % Ld = 0.95*Ls;
+
+    a = 0.9898;
+    b = 0.0072;
+    c = 0.0361;
+    d = 1.0;
+    e = 0.0149;
+
+    Rs = 0.28;
+    Ls = 0.003465;
+    psi = 0.1989;
+    B = 0;    
+    kp = 1.5;
+    pp = 4.0;
+    J = 0.04;
+    Lq = 1.0*Ls;
+    Ld = 0.9*Ls;
+    kpp = kp*pp*pp;
+    kppj = kpp/J;
+
+%     Ls = 0.003465;
+%     psipm = 0.1989;
+
+    % ref_profile = [0, -1, 3, 6, 9, 6, 3, 0, 0, 0, 0, 0, 0,-3, -6, -3];%/9*200;
+    % ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
+    % ref_profile = [0, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 1, -1, 0];
+    % ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0];
+    % ref_profile = [20, 20, 20, 50, 50, 50, -10, -10, -10, 0, 0, 0, 20, 20, 20];
+
+    %kovariance EKF na stavu
+    % Q_k = diag([0.001, 0.00001]);
+    % R_k = diag([0.015, 0.015]);
+    Q_k = diag([0.01, 0.0001]);
+    R_k = diag([0.15, 0.15]);
+
+    %hodnoty sumu v systemu
+    nQ = diag([0.0013, 0.0013, 5.0e-6, 1.0e-10]);
+    nR = diag([0.0006, 0.0006]);
+
+    iter_l = 10;% pocet iteraci ve vypoctu rizeni
+
+    % B_l = zeros(3,2);
+    % B_l = zeros(2,2);
+    % B_l(1,1) = c;
+    % B_l(2,2) = c;
+
+    Q_l = diag([1 0 0]);
+    % % Q_l = diag([0 0 1 0 0]);
+    r = 0.01;
+    R_l = diag([r, r]);
+
+    % PROMENNE
+    x_s = zeros(4,T); %stav
+    y_s = zeros(2,T); %mereni
+    x_k = zeros(2,T); %odhad stavu
+%     P_k = zeros(2); %kovariance stavu
+    u_l = zeros(2,T); %rizeni
+    % S_l = zeros(3); %jadro ztraty
+%     S_l = zeros(2);
+
+    % POCATECNI HODNOTY
+    noise = 1; %prepinac sumu
+    % noise = 0;
+
+    % theta0 = 0; %pocatecni poloha odhadu (nejde pro stav kvuli simulatoru)
+    P0 = eye(2); %odhad pocatecni kovariance stavu (apriorni)
+    % ST = zeros(3); %koncova ztrata
+    ST = ones(3);
+
+
+    % INICIALIZACE
+    x_k(2,1) = theta0;
+    % x_s(3,1) = 5;
+    P_k = P0;
+    S_l = ST;
+
+    ref_ome = zeros(1, T);  
+        for k = 1:T,
+               index = floor(k*dt);
+               if(index>0)
+                   lower = ref_profile(index);
+               else
+                   lower = 0;
+               end
+               if(index<T*dt)
+                   upper = ref_profile(index+1);
+               else
+                   upper = 0;
+               end
+               ref_ome(k) = lower + (upper-lower)*dt*(k-index/dt);
+        end
+    % ref_ome = 0*ones(1, T);
+
+    % Derivace pro prvni EKF 
+    ome = x_k(1,1);
+    the = x_k(2,1);    
+    ia = y_s(1,1);
+    ib = y_s(2,1);
     A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0];
     C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)];
+
+
+    ri = 0.0001;
+    ai = (1-a*a)/c/c;
+    Si = (1 - ai*r + sqrt((ai*r-1)^2+4*r/c/c))/2;
+    Li = a*c*Si/(c*c*Si+ri);
+
+    A_l = [d,0,0;dt,1,dt;0,0,1];
+    % B_l = zeros(2);
+    % % A_l = [a 0 0 0 0; 0 a 0 0 0; 0 0 d 0 (d-1); 0 0 dt 1 dt; 0 0 0 0 1];
+    % % B_l = zeros(5,2);
+    % % B_l(1:2,1:2) = [c 0;0 c];
+
+    %PI vektorove
+    % kon_pi = 3.0;
+    % kon_ii = 0.00375;
+    % kon_pu = 20.0;
+    % kon_iu = 0.05;
+    % sum_iq = 0;
+    % sum_ud = 0;
+    % sum_uq = 0;
+
+
+
+    % HLAVNI SMYCKA
+    for t = 1:T-1,
+        % EKF      
+        Pp = A*P_k*A' + Q_k;
+        S = C*Pp*C' + R_k;
+        K = Pp*C'/S;
+        P_k = Pp - K*C*Pp;    
+
+        xp = zeros(2,1);
+        xp(1) = d*x_k(1,t) + e*(y_s(2,t)*cos(x_k(2,t)) - y_s(1,t)*sin(x_k(2,t)));
+        xp(2) = x_k(2,t) + dt*x_k(1,t);    
+        yp = zeros(2,1);
+        yp(1) = a*y_s(1,t) + b*x_k(1,t)*sin(x_k(2,t)) + c*u_l(1,t);
+        yp(2) = a*y_s(2,t) - b*x_k(1,t)*cos(x_k(2,t)) + c*u_l(2,t);
+
+        x_k(:,t+1) = xp + K*(y_s(:,t) - yp);
+
+        %!!!
+    %     tmp = x_k(:,t+1);
+    %     x_k(:,t+1) = x_s(3:4,t);
+
+        % Derivace    
+        ome = x_k(1,t+1);
+        the = x_k(2,t+1);    
+        ia = y_s(1,t);
+        ib = y_s(2,t);
+        if(inddq == 0)
+            %stejne indukcnosti
+            A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0];
+        else
+            %ruzne indukcnosti        
+            A = [d, -dt*kppj*(psi*(ia*cos(the) + ib*sin(the)) + (Ld - Lq)*(ia*cos(the) + ib*sin(the))^2 - (Ld - Lq)*(ib*cos(the) - ia*sin(the))^2); dt, 1.0];
+        end
+        C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)];
+
+    %     id = ia*cos(the) + ib*sin(the);
+    %     iq = ib*cos(the) - ia*sin(the);
+
+        % LQ
+    %     phi = zeros(2,1);
+    %     phi(1) = d*x_k(1,t+1) + e*(y_s(2,t)*cos(x_k(2,t+1)) - y_s(1,t)*sin(x_k(2,t+1)));
+    %     phi(2) = x_k(2,t+1) + dt*x_k(1,t+1);
+    %     y = x_k(:,t+1);
+    %     y(1) = y(1) - ref_ome(t);
+    %     A_l = zeros(3);
+    %     A_l(1:2,1:2) = A;
+    %     A_l = A;
+    %     A_l(1,2) = 0;
+    %     A_l(1:2,3) = phi - A*y;
+    %     A_l(3,3) = 1;
+        B_l = [-e*sin(the), e*cos(the); 0, 0; 0,0];
+        y = [(ome-ref_ome(t)); the; ref_ome(t)];
+    % %     y = [ia; ib; (ome-ref_ome(t)); the; ref_ome(t)];    
+    % %     A_l(1, 3) = b*sin(the);
+    % %     A_l(2, 3) = -b*cos(the);
+    % %     A_l(1, 5) = b*sin(the);
+    % %     A_l(2, 5) = -b*cos(the);
+    % %     A_l(3, 1) = -e*sin(the);
+    % %     A_l(3, 2) = e*cos(the);
+        for i = 1:iter_l
+           S_l = A_l'*(S_l - S_l*B_l/(B_l'*S_l*B_l + R_l)*B_l'*S_l)*A_l + Q_l;
+        end
+        L = (B_l'*S_l*B_l + R_l)\B_l'*S_l*A_l;    
+    %     yref = -L*y;%referencni proudy
+    %     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c - Li*y_s(:,t);
+
+    %     sum_iq = sum_iq + ref_ome(t) - ome;
+    %     ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq;
+    %     sum_ud = sum_ud - id;
+    %     u_d = kon_pu*(-id) + kon_iu*sum_ud;
+    %     sum_uq = sum_uq + ref_iq - iq;
+    %     u_q = kon_pu*(ref_iq - iq) + kon_iu*sum_uq;
+    %     u_d = u_d - Ls*ome*ref_iq;
+    %     u_q = u_q + psipm*ome;
+    %        
+    %     u_l(1, t+1) = u_d*cos(the) - u_q*sin(the);
+    %     u_l(2, t+1) = u_q*cos(the) + u_d*sin(the);
+    %     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c*[sin(the);-cos(the)] - Li*y_s(:,t);
+
+    %     u_l(:,t+1) = yref/c - Li*y_s(:,t);
+    %     u_l(:,t+1) = -L*[y;1];
+        u_l(:,t+1) = -L*y + b/c*ome*[-sin(the);cos(the)] - Li*y_s(:,t);
+        if u_l(1,t+1) > 100
+            u_l(1,t+1) = 100;
+        elseif u_l(1,t+1) < -100
+            u_l(1,t+1) = -100;
+        end
+        if u_l(2,t+1) > 100
+            u_l(2,t+1) = 100;
+        elseif u_l(2,t+1) < -100
+            u_l(2,t+1) = -100;
+        end
+    %     u_l(:,t+1) = 0;
+        % Vyvoj systemu       
+        [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator);
+
+
+        %!!!
+    %     x_k(:,t+1) = tmp;
+    end
+
+    if(graf == 1)
+        %vykresleni
+        cas = (1:T)*dt;
+        figure;
+        subplot(2,1,1);
+        plot(cas,x_k(1,:),cas,x_s(3,:),cas,ref_ome);
+        title('Prubeh otacek v case');
+        xlabel('cas [s]');
+        ylabel('otacky [rad/s]');
+        legend('odhad','skutecne','pozadovane');
+        subplot(2,1,2);
+        plot(cas,atan2(sin(x_k(2,:)),cos(x_k(2,:))),cas,atan2(sin(x_s(4,:)),cos(x_s(4,:))));
+        title('Prubeh polohy v case');
+        xlabel('cas [s]');
+        ylabel('poloha [rad]');
+
+        figure;
+        plot(cas,x_s(3,:)-ref_ome);
+        title('Prubeh chyby (skutecne - pozadovane otacky v case)');
+        xlabel('cas [s]');
+        ylabel('chyba [rad/s]');
+    end
     
-%     id = ia*cos(the) + ib*sin(the);
-%     iq = ib*cos(the) - ia*sin(the);
-    
-    % LQ
-%     phi = zeros(2,1);
-%     phi(1) = d*x_k(1,t+1) + e*(y_s(2,t)*cos(x_k(2,t+1)) - y_s(1,t)*sin(x_k(2,t+1)));
-%     phi(2) = x_k(2,t+1) + dt*x_k(1,t+1);
-%     y = x_k(:,t+1);
-%     y(1) = y(1) - ref_ome(t);
-%     A_l = zeros(3);
-%     A_l(1:2,1:2) = A;
-%     A_l = A;
-%     A_l(1,2) = 0;
-%     A_l(1:2,3) = phi - A*y;
-%     A_l(3,3) = 1;
-    B_l = [-e*sin(the), e*cos(the); 0, 0; 0,0];
-    y = [(ome-ref_ome(t)); the; ref_ome(t)];
-% %     y = [ia; ib; (ome-ref_ome(t)); the; ref_ome(t)];    
-% %     A_l(1, 3) = b*sin(the);
-% %     A_l(2, 3) = -b*cos(the);
-% %     A_l(1, 5) = b*sin(the);
-% %     A_l(2, 5) = -b*cos(the);
-% %     A_l(3, 1) = -e*sin(the);
-% %     A_l(3, 2) = e*cos(the);
-    for i = 1:iter_l
-       S_l = A_l'*(S_l - S_l*B_l/(B_l'*S_l*B_l + R_l)*B_l'*S_l)*A_l + Q_l;
-    end
-    L = (B_l'*S_l*B_l + R_l)\B_l'*S_l*A_l;    
-%     yref = -L*y;%referencni proudy
-%     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c - Li*y_s(:,t);
-    
-%     sum_iq = sum_iq + ref_ome(t) - ome;
-%     ref_iq = kon_pi*(ref_ome(t) - ome) + kon_ii*sum_iq;
-%     sum_ud = sum_ud - id;
-%     u_d = kon_pu*(-id) + kon_iu*sum_ud;
-%     sum_uq = sum_uq + ref_iq - iq;
-%     u_q = kon_pu*(ref_iq - iq) + kon_iu*sum_uq;
-%     u_d = u_d - Ls*ome*ref_iq;
-%     u_q = u_q + psipm*ome;
-%        
-%     u_l(1, t+1) = u_d*cos(the) - u_q*sin(the);
-%     u_l(2, t+1) = u_q*cos(the) + u_d*sin(the);
-%     u_l(:,t+1) = b/c*ome*[-sin(the);cos(the)] + yref/c*[sin(the);-cos(the)] - Li*y_s(:,t);
-
-%     u_l(:,t+1) = yref/c - Li*y_s(:,t);
-%     u_l(:,t+1) = -L*[y;1];
-    u_l(:,t+1) = -L*y + b/c*ome*[-sin(the);cos(the)] - Li*y_s(:,t);
-    if u_l(1,t+1) > 100
-        u_l(1,t+1) = 100;
-    elseif u_l(1,t+1) < -100
-        u_l(1,t+1) = -100;
-    end
-    if u_l(2,t+1) > 100
-        u_l(2,t+1) = 100;
-    elseif u_l(2,t+1) < -100
-        u_l(2,t+1) = -100;
-    end
-%     u_l(:,t+1) = 0;
-    % Vyvoj systemu
-    [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise);
-    
-    %!!!
-%     x_k(:,t+1) = tmp;
+    loss = sum((x_s(3,:)-ref_ome).^2);
 end
-
-figure;
-subplot(2,1,1);
-plot(1:T,x_k(1,:),1:T,x_s(3,:),1:T,ref_ome);
-subplot(2,1,2);
-plot(1:T,atan2(sin(x_k(2,:)),cos(x_k(2,:))),1:T,atan2(sin(x_s(4,:)),cos(x_s(4,:))));
-
