root/applications/dual/vahala/kim/main.m @ 1450

function [loss] = main(T, ref_profile, theta0, simulator, graf, inddq) 
    % 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;

    % 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, ktery vytvari hyperstav
    % 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]);

    %kovariance EKF na hyperstavu
    % Qh_k = diag([0.001, 0.00001, 0.00001, 0.00001, 0.00001]);
    % Rh_k = diag([0.015, 0.015]);
    Qh_k = diag([0.01, 0.0001, 0.1, 0.1, 0.1]);
    Rh_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(6,2);
    % B_l(1,1) = c;
    % B_l(2,2) = c;

    %           o t Po Pot Pt
    Q_l = diag([1 0 1 0 0 0]); % spravne z teoretickeho hlediska
%     Q_l = diag([1 0 1 0 1 0]);
    % Q_l = diag([1 0 0 0 0 0]);
    r = 0.0001;
    R_l = diag([r, r]);



    % PROMENNE
    x_s = zeros(4,T); %stav
    y_s = zeros(2,T); %mereni
    x_k = zeros(5,T); %odhad hyperstavu
%     P_k = zeros(5); %kovariance hyperstavu
    u_l = zeros(2,T); %rizeni
%     S_l = zeros(6); %jadro ztraty
%     pre_k = zeros(3,1); %predikce stavu


    % POCATECNI HODNOTY
    noise = 1; %prepinac sumu
    % noise = 0;

%     theta0 = 0; %pocatecni poloha odhadu (nejde pro stav kvuli simulatoru)
    Ps0 = eye(2); %odhad pocatecni kovariance stavu (apriorni)
    Pk0 = eye(5); %pocatecni kovariance hyperstavu
    ST = zeros(6); %koncova ztrata


    % INICIALIZACE
    x_k(2,1) = theta0;
    x_k(3,1) = Ps0(1,1);
    x_k(4,1) = Ps0(1,2);
    x_k(5,1) = Ps0(2,2);
    P_k = Pk0;
    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

    % Derivace pro prvni EKF 
    [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,1), y_s(:,1), x_k(:,1), Q_k, R_k, 0, inddq);

    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);

    Pia = 1;
    Pib = 1;
    qi = 0.1;
    ri = 0.05;
    y = [0;0];

    % HLAVNI SMYCKA
    for t = 1:T-1,
        % EKF   
        Pp = Pia*(a*a+b*b+b*b*cos(x_k(2,t))^2*(x_k(1,t)^2-1))+qi;
        Pia = Pp-Pp*Pp/(Pp+ri);
        y(1) = (1-Pp/(Pp+ri))*(a*y(1)+b*x_k(1,t)*sin(x_k(2,t))+c*u_l(1,t)) + Pp/(Pp+ri)*y_s(1,t);
        Pp = Pib*(a*a+b*b+b*b*sin(x_k(2,t))^2*(x_k(1,t)^2-1))+qi;
        Pib = Pp-Pp*Pp/(Pp+ri);
        y(2) = (1-Pp/(Pp+ri))*(a*y(2)-b*x_k(1,t)*cos(x_k(2,t))+c*u_l(2,t)) + Pp/(Pp+ri)*y_s(2,t);
        [x_k(:,t+1), P_k] = extKF(x_k(:,t), y, u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k);
    %     [x_k(:,t+1), P_k] = extKF(x_k(:,t), y_s(:,t), u_l(:,t), pre_k, A_k, C_k, P_k, Qh_k, Rh_k);

    %     Q_l(1,1) = 1/(1+exp(-2*x_k(1,t+1)+6))+0.1;
    %     Q_l(3,3) = x_k(5,t+1)^5*50;
    %     Q_l(5,5) = Q_l(3,3);

        % Derivace
        [A_k, C_k, pre_k, A_l] = assembDeriv(x_k(:,t+1), y_s(:,t), x_k(:,t+1), Q_k, R_k, ref_ome(t), inddq);

        % LQ
        B_l(1,1:2) = [-e*sin(x_k(2,t+1)), e*cos(x_k(2,t+1))];
    %     [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, S_l, Q_l, R_l, iter_l);
        [u_l(:,t+1), S_l] = ctrlLQ(x_k(:,t+1), ref_ome(t), A_l, B_l, ST, Q_l, R_l, iter_l);
        u_l(:,t+1) = b/c*x_k(1,t+1)*[-sin(x_k(2,t+1));cos(x_k(2,t+1))] + u_l(:,t+1) - 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

        % Vyvoj systemu
        [x_s(:,t+1), y_s(:,t+1)] = evolSys(x_s(:,t), u_l(:,t+1), nQ, nR, noise, simulator);    
    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