function pmsm_lqg
% rizeni pmsm motoru - jednoduchy lqg algoritmus

%nastaveni algortimu
K = 1000; %casy

N = 50; %vzorky
It = 5; %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.0001;
w = 1;

%matice modelu
A = [a 0 0 0;...
     0 a 0 0;...
     0 0 d 0;...
     0 0 DELTAt 1];
 
B = DELTAt*[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, K);
xs = zeros(4, K);
xn = zeros(4, K, N);

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

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

tic

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

%vysledky
    clf
    subplot(2, 3, 3);
    hold all
    plot(1:K, OMEGAt*ones(1,K));

    for n = 1:N,
        xn(:, 1, n) = x0' - [0 0 OMEGAt 0]' + neznalost*sqrtm(P)*randn(4,1);
        for k = 1:K-1,
           tu =  L(:, :, k)*(xn(:, k, n));
           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();
           
           u(:, k) = tu;
        end
        
        xn(3, :, n) = xn(3, :, n) + OMEGAt*ones(1, K);
        
        subplot(2, 3, 1);
        hold all
        plot(1:K, xn(1, :, n));
        title('i_{\alpha}');
        subplot(2, 3, 2);
        hold all
        plot(1:K, xn(2, :, n));
        title('i_{\beta}');
        subplot(2, 3, 3);
        hold all
        plot(1:K, xn(3, :, n));
        title('\omega');
        subplot(2, 3, 4);
        hold all
        plot(1:K, xn(4, :, n));
        title('\theta');
        subplot(2, 3, 5);
        hold all
        plot(1:K, u(1, :));
        title('u_{\alpha}');
        subplot(2, 3, 6);
        hold all
        plot(1:K, u(2, :));
        title('u_{\beta}');
    end


end