function pmsm_lqg
% rizeni pmsm motoru - jednoduchy lqg algoritmus

%nastaveni algortimu
K = 10; %casy
Kt = 100; %test casy

N = 100; %vzorky
It = 1; %iterace


%konstanty modelu
DELTAt = 0.000125;

cRs = 0.28;
cLs = 0.003465;
cPSIpm = 0.1989;
ckp = 1.5;
cp = 4.0;
cJ = 0.04;
cB = 0;

% a = 0.9898;
% b = 0.0072;
% c = 0.0361;
% d = 1;
% e = 0.0149; 

a = 1 - DELTAt*cRs/cLs;
b = DELTAt*cPSIpm/cLs;
c = DELTAt/cLs;
d = 1 - DELTAt*cB/cJ;
e = DELTAt*ckp*cp*cp*cPSIpm/cJ;

OMEGAt = 1.0015;

%penalizace vstupu a rizeni
v = 0.00001;
w = 1;

%matice modelu
A = [a 0 0 0;...
     0 a 0 0;...
     0 0 d 0;...
     0 0 DELTAt 1];
 
B = [c 0;...
            0 c;...
            0 0;...
            0 0];
 
% C = [1 0 0 0;...
%      0 1 0 0];
 
X = [0 0 0 0;...
     0 0 0 0;...
     0 0 w 0;...
     0 0 0 0];
 
Y = [v 0;...
     0 v];
 
%pocatecni nastaveni
Q = diag([0.0013, 0.0013, 5e-6, 1e-10]);
R = diag([0.0006, 0.0006]);

x0 = [0 0 1 pi/2];
P = diag([0.01, 0.01, 0.01, 0.01]);

%globalni promenne
u = zeros(2, Kt);
xs = zeros(4, Kt);
xn = zeros(4, Kt, N);

S = zeros(4, 4, K);
L = zeros(2, 4, Kt);

%zapinani a vypinani sumu, sumu v simulaci a generovani trajektorii s
%rozptylem
sum = 1;%0.01;
sumsim = 1;%0.01;
neznalost = 1;

tic

%hlavni iteracni smycka
for iterace = 1:It,
    %generovani stavu
    for n = 1:N,
        xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1);
        for k = 1:Kt-1,
           tu =  L(:, :, k)*(xn(:, k, n) - [0 0 OMEGAt 0]');
           xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, 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)*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn();
           xn(3, k+1, n) = d*xn(3, 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)*DELTAt + sumsim*sqrt(Q(4, 4))*randn();           
%            xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n)+OMEGAt)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn();  
%            xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n)+OMEGAt)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn();
%            xn(3, k+1, n) = -OMEGAt + d*(xn(3, k, n)+OMEGAt) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sum*sqrt(Q(3, 3))*randn();
%            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn();
        end
    end
    
    %prumerny stav
    xs = mean(xn, 3);
    
    %napocteni S a L
    for kt = 1:Kt-1,
%     receding horizon
     if((K-1+kt)<Kt)
        S(:, :, K) = X;
        for k = K-1+kt-1:-1:1+kt-1,
            A(3, 1) = -e*sin(xs(4, k));
            A(3, 2) = e*cos(xs(4, k));
            A(1, 3) = b*sin(xs(4, k));
            A(2, 3) = -b*cos(xs(4, k));
            A(1, 4) = b*(xs(3, k))*cos(xs(4, k));
            A(2, 4) = b*(xs(3, k))*sin(xs(4, k));
%             A(1, 4) = b*(xs(3, k)+OMEGAt)*cos(xs(4, k));
%             A(2, 4) = b*(xs(3, k)+OMEGAt)*sin(xs(4, k));
            A(3, 4) = -e*(xs(2, k)*sin(xs(4, k) + xs(1,k)*cos(xs(4, k))));
            S(:, :, k-kt+1) = A'*(S(:, :, k-kt+2) - S(:, :, k-kt+2)*B*inv(B'*S(:, :, k-kt+2)*B + Y)*B'*S(:, :, k-kt+2))*A + X;        
        end  
        L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
     else
        L(:, :, kt) = L(:, :, kt-1); %kopiruje poslednich K kroku z Kt kde to nejde na K predpocitat
     end
        
    end
end
    toc

%vysledky
    clf
    subplot(2, 3, 3);
    hold all
    plot(1:Kt, OMEGAt*ones(1,Kt));
% L(:,:,1)
    for n = 1:N,
        xn(:, 1, n) = x0' + neznalost*sqrtm(P)*randn(4,1);
        for k = 1:Kt-1,
           tu =  L(:, :, 1)*(xn(:, k, n) - [0 0 OMEGAt 0]');
           xn(1, k+1, n) = a*xn(1, k, n) + b*xn(3, 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)*cos(xn(4, k, n)) + c*tu(2) + sumsim*sqrt(Q(2, 2))*randn();
           xn(3, k+1, n) = d*xn(3, 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)*DELTAt + sumsim*sqrt(Q(4, 4))*randn();
%            xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n)+OMEGAt)*sin(xn(4, k, n)) + c*tu(1) + sum*sqrt(Q(1, 1))*randn();  
%            xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n)+OMEGAt)*cos(xn(4, k, n)) + c*tu(2) + sum*sqrt(Q(2, 2))*randn();
%            xn(3, k+1, n) = -OMEGAt + d*(xn(3, k, n)+OMEGAt) + e*(xn(2, k, n)*cos(xn(4, k, n)) - xn(1, k, n)*sin(xn(4, k, n))) + sum*sqrt(Q(3, 3))*randn();
%            xn(4, k+1, n) = xn(4, k, n) + (xn(3, k, n)+OMEGAt)*DELTAt + sum*sqrt(Q(4, 4))*randn();
           
           u(:, k) = tu;          
        end
        
%         xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, Kt);
        
        subplot(2, 3, 1);
        hold all
        plot(1:Kt, xn(1, :, n));
        title('i_{\alpha}');
        subplot(2, 3, 2);
        hold all
        plot(1:Kt, xn(2, :, n));
        title('i_{\beta}');
        subplot(2, 3, 3);
        hold all
        plot(1:Kt, xn(3, :, n));
        title('\omega');
        subplot(2, 3, 4);
        hold all
        plot(1:Kt, xn(4, :, n));
        title('\theta');
        subplot(2, 3, 5);
        hold all
        plot(1:Kt, u(1, :));
        title('u_{\alpha}');
        subplot(2, 3, 6);
        hold all
        plot(1:Kt, u(2, :));
        title('u_{\beta}');
    end


end