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

function [loss] = ukf_comp(T, ref_profile, theta0, simulator, graf)
    % clear all;
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %%%%%pouziti SIMULATORU
%     simulator = 1;
    % simulator = 0;

    if(simulator == 1)
        sim_param = pmsm_sim;
    %     sim_param(9) = 0; %vypne dead-time
        pmsm_sim(sim_param);
    end
    %%%%%
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   parametry stroje
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%     Rs = 0.28;
    Ls = 0.003465;
    psipm = 0.1989;
    %B = 0;
    %TL = 0;
%     kp = 1.5;
%     pp = 4.0;
%     J = 0.04;
    dt = 0.000125;

    a = 0.9898;
    b = 0.0072;
    c = 0.0361;
    d = 1.0;
    e = 0.0149;

    M = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
    N = diag([0.0006 0.0006]);


    iP = eye(4);
    % iQ = diag([0.001 0.001 0.00001 0.00001]);
    % iR = diag([0.0005 0.0005]);
    % iQ = diag([0.4 0.4 0.2 0.2]);
    % iR = diag([0.5 0.5]);
    iQ = M;
    iR = N;


    alpha = 0.001;
    beta = 2.0;
    kappa = 0.0;

    L = 4;
%     x0 = zeros(4,1);
    P0 = iP;

    lambda = alpha*alpha*(L + kappa) - L;
    Wm0 = lambda / (L + lambda);
    Wc0 = Wm0 + 1 - alpha*alpha + beta;
    Wmc = 1.0 / (2.0*(L + lambda)); 

    WM = [Wm0; Wmc*ones(2*L,1)];
    WC = [Wc0; Wmc*ones(2*L,1)];

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   nasteveni experimentu
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%


%     T = 120000; %horizont

%     theta0 = pi;%0.0;

    ref_ome = zeros(1, T); %referencni signal
%     ref_profile = [1, 10, 50, 200, 200, 30, 0, 0, -1, -10, -50, -200, -200, -30, 0];
    % 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];

    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


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   nastaveni rizeni
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    MAXu = 100; %maximalni povolene napeti
    MAXufl = 1; %flag pro volbu omezeni

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   promenne stavu systemu
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    x_sys = zeros(4, T); %vnitrni stav systemu
    y_sys = zeros(2, T); %vystup systemu
    u_ab = zeros(2, T); %rizeni systemu v alfa-beta
%     sign_om = zeros(2, T); % signum omega: real, estimated

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   promenne estimatoru EKF
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    x_est = zeros(4, T); %odhad stavu
    % P = zeros(4, 4); %kovariancni matice EKF

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   promenne rizeni
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%     u_dq = zeros(2, T);

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

%     koef_bic = 6.0;


    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   inicializace
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    x_est(4, 1) = theta0; %pocatecni natoceni theta
%     x_est(:,1) = x0; %state estimate
    x = x_est(:, 1);
    P = P0; %covariance

%     figure;

    lambda2 = lambda;
%     lambda2 = lambda + 0.001; %+100pro randi vyber pro prumer trebe daleko min cca 0.001
%     lambda2 = lambda + 100; %+100pro randi vyber pro prumer trebe daleko min cca 0.001
    %%%%%% oboji celkem jede, jeste to zkusit

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   hlavni smycka
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

    for t = 1:T-1,
        %%%% estimace %%% (yt -> estxt)
        %UKF

        if(t == 1)            
                u_aold = 0;
                u_bold = 0;
    %             ome = 0;
    %             the = 0;
        else            
                u_aold = u_ab(1, t-1);
                u_bold = u_ab(2, t-1);
        end

        %sigma points
        sigma = [x, x*ones(1,L)+chol((L+lambda)*P), x*ones(1,L)-chol((L+lambda)*P)];

        %time update
        Fsigma = [a*sigma(1,:) + b*sigma(3,:).*sin(sigma(4,:)) + c*u_aold*ones(1,2*L+1);...
                a*sigma(2,:) - b*sigma(3,:).*cos(sigma(4,:)) + c*u_bold*ones(1,2*L+1);...
                d*sigma(3,:) + e*(sigma(2,:).*cos(sigma(4,:)) - sigma(1,:).*sin(sigma(4,:)));...
                sigma(4,:) + dt*sigma(3,:)];

        x_m = Fsigma*WM;

        P_m = zeros(4);
        for i = 1:2*L+1
            P_m = P_m + WC(i)*(Fsigma(:,i)-x_m)*(Fsigma(:,i)-x_m)';
        end
        P_m = P_m + iQ;

        Hsigma = [Fsigma(1,:); Fsigma(2,:)];

        y_m = Hsigma*WM;

        %measurement update
        Pyy = zeros(2);
        Pxy = zeros(4,2);
        for i = 1:2*L+1
            Pyy = Pyy + WC(i)*(Hsigma(:,i)-y_m)*(Hsigma(:,i)-y_m)';
            Pxy = Pxy + WC(i)*(Fsigma(:,i)-x_m)*(Hsigma(:,i)-y_m)';
        end
        Pyy = Pyy + iR;

        Kk = Pxy/(Pyy);

        x = x_m + Kk*(y_sys(:,t) - y_m);

        P = P_m - Kk*Pyy*Kk';   

        x_est(:,t) = x;
    %     p33(t) = P(3,3);

        sigma_est = [x, x*ones(1,L)+chol((L+lambda2)*P), x*ones(1,L)-chol((L+lambda2)*P)];

        %%%%% rizeni %%%% (estxt -> ut)
        ial = x_est(1, t);
        ibe = x_est(2, t);
        ome = x_est(3, t);
        the = x_est(4, t);  

    %     sign_om(2, t) = sign(x_est(3, t));    


    %         id = ial*cos(the) + ibe*sin(the);
    %         iq = ibe*cos(the) - ial*sin(the);
    %         
    %         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_dq(1, t) = kon_pu*(-id) + kon_iu*sum_ud;
    %         sum_uq = sum_uq + ref_iq - iq;
    %         u_dq(2, t) = kon_pu*(ref_iq - iq) + kon_iu*sum_uq;
    %         u_dq(1, t) = u_dq(1, t) - Ls*ome*ref_iq;
    %         u_dq(2, t) = u_dq(2, t) + psipm*ome;
    %     
    %         u_dq(1, t) = u_dq(1, t) + sign(-ome)*koef_bic; 
    %         u_dq(2, t) = u_dq(2, t) + sign(ome)*koef_bic;
    %                
    %         u_ab(1, t) = u_dq(1, t)*cos(the) - u_dq(2, t)*sin(the);
    %         u_ab(2, t) = u_dq(2, t)*cos(the) + u_dq(1, t)*sin(the);

    %%%%%%%%%%%%%%%%%%
    %predpoctu spolecne integracni cleny pro vsechny sigma body dle prumeru (at
    %neni moc uchovavanych dat)
            id = ial*cos(the) + ibe*sin(the);
            iq = ibe*cos(the) - ial*sin(the);           
            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;   
            sum_uq = sum_uq + ref_iq - iq;

    %vypoctu rizeni pro kazdy sigma bod se spolecnym integracnim clenem  
            jedna = ones(size(sigma_est(3,:)));
            ref_iq = kon_pi*(ref_ome(t)*jedna - sigma_est(3,:)) + kon_ii*sum_iq*jedna;       
            id = sigma_est(1,:).*cos(sigma_est(4,:)) + sigma_est(2,:).*sin(sigma_est(4,:));
            iq = sigma_est(2,:).*cos(sigma_est(4,:)) - sigma_est(1,:).*sin(sigma_est(4,:));
            u_dq = [kon_pu*(-id) + kon_iu*sum_ud*jedna - Ls*sigma_est(3,:).*ref_iq;...    
                    kon_pu*(ref_iq - iq) + kon_iu*sum_uq*jedna + psipm*sigma_est(3,:)];

    %vyber vhodneho rizeni z distribuce na zaklade sigma bodu u_dq
        %puvodni (bez sigma bodu)
    %         u_ab(1, t) = u_dq(1, 1)*cos(the) - u_dq(2, 1)*sin(the);
    %         u_ab(2, t) = u_dq(2, 1)*cos(the) + u_dq(1, 1)*sin(the);
        %vazeny prumer (melo by vyjit podobne)
%             sg_u_ab = [u_dq(1,:).*cos(sigma_est(4,:)) - u_dq(2,:).*sin(sigma_est(4,:));...
%                        u_dq(2,:).*cos(sigma_est(4,:)) + u_dq(1,:).*sin(sigma_est(4,:))];
%             u_ab(:,t) = sg_u_ab*WM;
        %randomizace - nahodna volba jednoho ze sigma bodu
            sg_u_ab = [u_dq(1,:).*cos(sigma_est(4,:)) - u_dq(2,:).*sin(sigma_est(4,:));...
                       u_dq(2,:).*cos(sigma_est(4,:)) + u_dq(1,:).*sin(sigma_est(4,:))];
            u_ab(:,t) = sg_u_ab(:,randi(9));    
        
        if(MAXufl == 1)
           if(u_ab(1, t) > MAXu)
               u_ab(1, t) = MAXu;
           elseif(u_ab(1, t) < -MAXu)
               u_ab(1, t) = -MAXu;
           end
           if(u_ab(2, t) > MAXu)
               u_ab(2, t) = MAXu;
           elseif(u_ab(2, t) < -MAXu)
               u_ab(2, t) = -MAXu;
           end
        elseif(MAXufl == 2)
            uampl = sqrt(u_ab(:, t)'*u_ab(:, t));
            uangl = atan2(u_ab(2, t), u_ab(1, t));
            if(uampl > MAXu)
                uampl = MAXu;
            end
            u_ab(1, t) = uampl*cos(uangl);
            u_ab(2, t) = uampl*sin(uangl);
        end

        %%%% simulace %%% (xt + ut -> xt+1; xt+1 -> yt+1)
        if (simulator == 1),
            ua = 3*u_ab(1, t);
            ub = 3*u_ab(2, t);

            [tx, ty] = pmsm_sim(ua, ub, 0);

            x_sys(:, t+1) = tx(1:4); %isa, isb, omega, theta        
            y_sys(:, t+1) = ty(3:4); %isa, isb        
        else
            x_sys(1, t+1) = a*x_sys(1, t) + b*x_sys(3, t)*sin(x_sys(4, t)) + c*u_ab(1, t) + sqrt(M(1, 1))*randn();
            x_sys(2, t+1) = a*x_sys(2, t) - b*x_sys(3, t)*cos(x_sys(4, t)) + c*u_ab(2, t) + sqrt(M(2, 2))*randn();
            x_sys(3, t+1) = d*x_sys(3, t) + e*(x_sys(2, t)*cos(x_sys(4, t)) - x_sys(1, t)*sin(x_sys(4, t))) + sqrt(M(3, 3))*randn();
            x_sys(4, t+1) = x_sys(4, t) + dt*x_sys(3, t) + sqrt(M(4, 4))*randn();

        %     sign_om(1, t+1) = sign(x_sys(3, t+1));

            y_sys(1, t+1) = x_sys(1, t+1) + sqrt(N(1, 1))*randn();
            y_sys(2, t+1) = x_sys(2, t+1) + sqrt(N(2, 2))*randn();
        end
    end

    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    %   zaznam vysledku
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%     xax = 1:T-1;
%     timex = (xax)*dt;
%     subplot(2, 2, 1);
%     plot(timex, x_sys(1, xax), timex, x_est(1, xax));
%     subplot(2, 2, 2);
%     plot(timex, x_sys(2, xax), timex, x_est(2, xax));
%     subplot(2, 2, 3);
%     plot(timex, x_sys(3, xax), timex, x_est(3, xax), timex, ref_ome(xax));
%     subplot(2, 2, 4);
%     plot(timex, atan2(sin(x_sys(4, xax)),cos(x_sys(4, xax))), timex, atan2(sin(x_est(4, xax)),cos(x_est(4, xax))));

    % figure;
    % plot(timex, sign_om(1, xax), timex, sign_om(2, xax));

    % figure;
    % plot(timex, x_sys(3, :)-ref_ome);
    
    if(graf == 1)
        %vykresleni
        cas = (1:T)*dt;
        figure;
        subplot(2,1,1);
        plot(cas,x_est(3,:),cas,x_sys(3,:),cas,ref_ome);
        title('Prubeh otacek v case');
        xlabel('cas [s]');
        ylabel('otacky [rad/s]');
        legend('odhad','skutecna hodnota','referencni hodnota');
        subplot(2,1,2);
        plot(cas,atan2(sin(x_est(4,:)),cos(x_est(4,:))),cas,atan2(sin(x_sys(4,:)),cos(x_sys(4,:))));
        title('Prubeh polohy v case');
        xlabel('cas [s]');
        ylabel('poloha [rad]');

        figure;
        plot(cas,x_sys(3,:)-ref_ome);
        title('Prubeh chyby (skutecne - pozadovane otacky v case)');
        xlabel('cas [s]');
        ylabel('chyba [rad/s]');
    end
    
    loss = sum((x_sys(3,:)-ref_ome).^2);
    
end