root/applications/dual/vahala/kim/basic_main.m @ 1436

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
    %%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    % 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
    
    loss = sum((x_s(3,:)-ref_ome).^2);
end