function pmsm_lqg
% rizeni pmsm motoru - jednoduchy lqg algoritmus

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

N = 50; %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 = 2.15;%1.0015;

%penalizace vstupu a rizeni
v = 0.0001;%0.000001;
w = 1;

%matice modelu
A = [a 0 0 0 0;...
     0 a 0 0 0;...
     0 0 d 0 (d-1);...
     0 0 DELTAt 1 DELTAt;...
     0 0 0 0 1];
 
B = [c 0;...
     0 c;...
     0 0;...
     0 0;...
     0 0];
 
% C = [1 0 0 0;...
%      0 1 0 0];
 
X = [0 0 0 0 0;...
     0 0 0 0 0;...
     0 0 w 0 0;...
     0 0 0 0 0;...
     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.0-OMEGAt pi/2 OMEGAt];
P = diag([0.01, 0.01, 0.01, 0.5, 0]);

%globalni promenne
u = zeros(2, Kt+K);
xs = zeros(5, Kt+K);
xn = zeros(5, Kt+K, N);

S = zeros(5, 5, K);
L = zeros(2, 5, Kt+K);

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

% vycisti kreslici okno
    clf
    subplot(2, 3, 3);
    hold all
%    plot(1:Kt, OMEGAt*ones(1,Kt));
    
tic

% vzorky stavu
for n = 1:N,
    L = zeros(2, 5, Kt+K);
    %iterace
    x00 = x0' + neznalost*sqrt(P)*randn(5,1);   
        %testovaci casy
        for kt = 1:Kt,
            %generovani stavu - jen pro horizont
            for iterace = 1:1,
                xn(:, 1, n) = x00;
                for k = 1:kt+K-1,
                    tu = L(:, :, k)*(xn(:, k, n));
                    xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n) + xn(5, 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) + xn(5, 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) + (d-1)*xn(5, 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) + xn(5, k, n))*DELTAt;% + sumsim*sqrt(Q(4, 4))*randn();
                    xn(5, k+1, n) = xn(5, k, n);
                end
                %prumerny stav
                xs = xn(:, :, n);%mean(xn, 3);

                %receding horizon
                S(:, :, K) = X;
                for k = K:-1:2,
                    A(3, 1) = -e*sin(xs(4, k+kt-1));
                    A(3, 2) = e*cos(xs(4, k+kt-1));
                    A(1, 3) = b*sin(xs(4, k+kt-1));
                    A(2, 3) = -b*cos(xs(4, k+kt-1));
                    A(1, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*cos(xs(4, k+kt-1));
                    A(2, 4) = b*(xs(3, k+kt-1) + xs(5, k+kt-1))*sin(xs(4, k+kt-1));
                    A(3, 4) = -e*(xs(2, k+kt-1)*sin(xs(4, k+kt-1) + xs(1,k+kt-1)*cos(xs(4, k+kt-1))));
                    A(1, 5) = b*sin(xs(4, k+kt-1));
                    A(2, 5) = -b*cos(xs(4, k+kt-1));
                    S(:, :, k-1) = A'*(S(:, :, k) - S(:, :, k)*B*inv(B'*S(:, :, k)*B + Y)*B'*S(:, :, k))*A + X;
                    L(:, :, kt+k-2) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
                end
%                L(:, :, kt) = -inv(B'*S(:, :, 1)*B + Y)*B'*S(:, :, 1)*A;
                %spocital kt-te rizeni a vsechna dalsi nahradi jim
%                 for k = kt+1:kt+K-1,
%                     L(:, :, k) = L(:, :, kt);
%                 end
            end
        end        
        
%     end
        %napocte trajektorii pro vykresleni s kompletnim rizenim
            xn(:, 1, n) = x00;
            for k = 1:Kt+K-1,               
               u(:, k) = L(:, :, k)*(x_kalman(:, k, n));  
               xn(1, k+1, n) = a*xn(1, k, n) + b*(xn(3, k, n) + xn(5, k, n))*sin(xn(4, k, n)) + c*u(1, k) + sumsim*sqrt(Q(1, 1))*randn();  
               xn(2, k+1, n) = a*xn(2, k, n) - b*(xn(3, k, n) + xn(5, k, n))*cos(xn(4, k, n)) + c*u(2, k) + sumsim*sqrt(Q(2, 2))*randn();
               xn(3, k+1, n) = d*xn(3, k, n) + (d-1)*xn(5, 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) + xn(5, k, n))*DELTAt + sumsim*sqrt(Q(4, 4))*randn(); 
               xn(5, k+1, n) = xn(5, k, n);
               
               yn()=...;
               x_kalman = ...;
            end
           
        
    %vykresleni
        subplot(2, 3, 1);
        hold all
        plot(1:Kt, xn(1, 1:Kt, n));
        title('i_{\alpha}');
        subplot(2, 3, 2);
        hold all
        plot(1:Kt, xn(2, 1:Kt, n));
        title('i_{\beta}');
        subplot(2, 3, 3);
        hold all
        plot(1:Kt, xn(3, 1:Kt, n) + xn(5, 1:Kt, n));
        title('\omega');
        subplot(2, 3, 4);
        hold all
        plot(1:Kt, xn(4, 1:Kt, n));
        title('\theta');
        subplot(2, 3, 5);
        hold all
        plot(1:Kt, u(1, 1:Kt));
        title('u_{\alpha}');
        subplot(2, 3, 6);
        hold all
        plot(1:Kt, u(2, 1:Kt));
        title('u_{\beta}');    
end

toc

end