%simulator PMSM pro PCRB
% zatim zaver:
%   v d-q vzdy roste neomezene
%   v al-be se omezuje a roste jen pri nulovych otackach
%   zda se, ze al-be not equal L vubec nepomuze
%   model d-q mereni al-be termer nepomuze
%   zadne bikriterialni vylepseni vicemene nemeni PCRB
%   znacne snizeni PCRB je nasledkem drzeni konst. nenul. id, ale otazka,
%       co kdyz budu drzet esimovane id...funguje i pro estimovane id a 
%       zlepsuje nejen PCRB ale i odhad kalmana; jen konstanta, zadny sin ani
%       obdelniky, jedine hodne mala frekvence, tj. skoro konstanta
%   maximova norma rozdilu DF pro al-be pri stejnych a ruznych
%       indukcnostech je stejneho radu (popr. o rad nizsi) nez samotna PCRB
%       pro theta, nicmene ale vetsi rozdil je pri vyssim omega, pri
%       nizkych otackach je rozdil maly a tedy tam, kde mel pomoct nepomuze
%       a matice jsou prakticky stejne
%      
%   zjistit otacky omega, ktere kdyz to drzi konst. tak je konst. i
%       chyba: pro kazde nenulove otacky pri N rozdeleni to nakonec dojde
%       ke konstantni chybe, ale muze byt neunosne vysoka, otazka tedy nema
%       smysl, po dostatecne dlouhem case je pak chyba vzdy konstantni
%
%   dalsi zpusob, jak ziskat lepsi mez je pocitat model sc5, ale stejne
%       nezastavuje rust chyby pri nulovych otackach - v realne aplikaci je
%       treba pridat korekce pri opusteni intervalu <-1;1>, jinak to casem
%       diverguje v dusledku diskretizace derivace; dale PCRB nabyvaji pro
%       sc "temer" nuly, kdyz odpovidajici skutecna hodnota sin(the) resp. 
%       cos(the) je extremni, tj. +-1, takze hodnoty +-1 dokazi urcit s
%       temer nulovou minimalni chybou, pro hodnoty 0 chyba (PCRB) odpovida
%       hodnote PCRB theta pro model se 4 stavovymi velicinami (horsi tedy
%       nejsem nikdy)
% TODO:
%           von Mises na theta
clear all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   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;

kpp = kp*pp*pp;
psi = psipm;

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

Lq = 1.0*Ls;
Ld = 0.9*Ls;

Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
R = diag([0.0006 0.0006]);


T = 120000; %horizont

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];
% ref_profile = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0];
% ref_profile = ones(1,16);

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



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

x_sys = zeros(4, T); %vnitrni stav systemu
u_ab = zeros(2, T); %rizeni systemu v alfa-beta

Q6 = zeros(5);
Q6(1:4,1:4) = Q;
Q6(5,5) = Q(4,4);

iJn = zeros(4,4,T);
iJn1 = zeros(4,4,T);
iJn2 = zeros(4,4,T);
iJn3 = zeros(4,4,T);
iJn4 = zeros(4,4,T);
iJn5 = zeros(4,4,T);
iJn6 = zeros(5,5,T);
iJn7 = zeros(5,5,T);

Jj = inv(Q);
Jj1 = inv(Q);
Jj2 = inv(Q);
Jj3 = inv(Q);
Jj4 = inv(Q);
Jj5 = inv(Q);
Jj6 = inv(Q6);
Jj7 = inv(Q6);


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

kon_pi = 30.0;
kon_ii = 0.0;
kon_pu = 20.0;
kon_iu = 0.0;

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

%alpha-beta, equal Ls
DF = zeros(4);
DF(1,1) = a;
DF(2,2) = a;
DF(3,3) = d;
DF(4,3) = dt;
DF(4,4) = 1.0;
DH = zeros(2,4);
DH(1,1) = 1.0;
DH(2,2) = 1.0;

DF1 = DF;

%d-q red, equal Ls
DF2 = zeros(4);
DF2(1,1) = a;
DF2(2,2) = a;
DF2(2,3) = -b;
DF2(3,2) = e;
DF2(3,3) = d;
DF2(4,3) = dt;
DF2(4,4) = 1.0;
DH2 = DH;
% 
%d-q full, equal Ls
DF3 = DF2;

%d-q, not equal Ld, Lq
DF4 = zeros(4);
DF4(1,1) = 1.0 - Rs*dt/Ld;
DF4(2,2) = 1.0 - Rs*dt/Lq;
DF4(3,3) = 1-B*dt/J;
DF4(4,3) = dt;
DF4(4,4) = 1.0;
    
%alpha-beta, not equal Ld, Lq
DF5 = zeros(4);

%alpha-beta, equal Ls sc theta
DF6 = zeros(5);
DF6(1,1) = a;
DF6(2,2) = a;
DF6(3,3) = d;
DF6(4,4) = 1.0;
DF6(5,5) = 1.0;
DH6 = zeros(2,5);
DH6(1,1) = 1.0;
DH6(2,2) = 1.0;

q1 = Q6(1,1);
q2 = Q6(2,2);
q3 = Q6(3,3);
q4 = Q6(4,4);
q5 = Q6(5,5);
opr_D11 = diag([e^2*q4/q3,...
                e^2*q5/q3,...
                b^2*q5/q2 + dt^2*q5/q4 + b^2*q4/q1 + dt^2*q4/q5,...
                e^2*q1/q3 + b^2*q3/q1 + dt^2*q3/q5,...
                e^2*q2/q3 + b^2*q3/q2 + dt^2*q3/q4]);
            
%%%%
opr_3_D11 = diag([dt^2*q3/q2, dt^2*q3/q1, dt^2*q1/q2 + dt^2*q2/q1, 0.0]);
opr_4_D11 = diag([Ld^2/Lq^2*dt^2*q3/q2 + dt^2*kp^2*pp^4*(Ld-Lq)^2*q2/J^2/q3,...
                  Lq^2/Ld^2*dt^2*q3/q1 + dt^2*kp^2*pp^4*(Ld-Lq)^2*q1/J^2/q3,...
                  Lq^2/Ld^2*dt^2*q2/q1 + Ld^2/Lq^2*dt^2*q1/q2, 0.0]);

              
% D11_a = zeros(4,4,T);
% D11_b = zeros(4,4,T);   

DF7 = DF6;
              
figure;
% nrm = zeros(1,T);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   hlavni smycka
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

for t = 1:T-1,
       
    
    %%%%% rizeni %%%% (estxt -> ut)
    ial = x_sys(1, t);
    ibe = x_sys(2, t);
    ome = x_sys(3, t);
    the = x_sys(4, t);  
    
    id = ial*cos(the) + ibe*sin(the);
    iq = ibe*cos(the) - ial*sin(the);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%
    
%     %alpha-beta, equal Ls
%     DF(1,3) = b*sin(the);
%     DF(1,4) = b*ome*cos(the);
%     DF(2,3) = -b*cos(the);
%     DF(2,4) = b*ome*sin(the);
%     DF(3,1) = -e*sin(the);
%     DF(3,2) = e*cos(the);
%     DF(3,4) = -e*(ial*cos(the)+ibe*sin(the));
%     
%     D11 = DF'*inv(Q)*DF;
%     D12 = -DF'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
%     
%     Jj = D22 - D21*inv(Jj + D11)*D12;
%         
%     iJn(:,:,t) = inv(Jj);
    
%     D11_a(:,:,t) = D11;
    
%     %alpha-beta, equal Ls - oprava!!!!!        
%     D11(1, 1) = a^2/q1 + e^2/q3*(1/2 - 1/2*exp(-2*q4)*cos(2*the));
%     D11(1, 2) = -e^2/q3*sin(the)*cos(the)*exp(-2*q4);
%     D11(1, 3) = (a*b/q1 - d*e/q3)*sin(the)*exp(-1/2*q4);
%     D11(1, 4) = e^2/q3*(ial*sin(the)*cos(the)*exp(-2*q4) + ibe*(1/2 - 1/2*exp(-2*q4)*cos(2*the)))...
%                 + a*b/q1*ome*cos(the)*exp(-1/2*q4);
%     
%     D11(2, 1) = D11(1, 2);
%     D11(2, 2) = a^2/q2 + e^2/q3*(1/2 + 1/2*exp(-2*q4)*cos(2*the));
%     D11(2, 3) = (d*e/q3 - a*b/q2)*cos(the)*exp(-1/2*q4);
%     D11(2, 4) = a*b/q2*ome*sin(the)*exp(-1/2*q4)...
%                 - e^2/q3*(ial*(1/2 + 1/2*exp(-2*q4)*cos(2*the)) + ibe*sin(the)*cos(the)*exp(-2*q4));
%     
%     D11(3, 1) = D11(1, 3);
%     D11(3, 2) = D11(2, 3);
%     D11(3, 3) = d^2/q3 + dt^2/q4 + b^2/q2*(1/2 + 1/2*exp(-2*q4)*cos(2*the))...
%                 + b^2/q1*(1/2 - 1/2*exp(-2*q4)*cos(2*the));
%     D11(3, 4) = dt/q4 - d*e/q3*(ial*cos(the)*exp(-1/2*q4) + ibe*sin(the)*exp(-1/2*q4))...
%                 + b^2/q1*ome*sin(the)*cos(the)*exp(-2*q4) - b^2/q2*ome*sin(the)*cos(the)*exp(-2*q4);
%     
%     D11(4, 1) = D11(1, 4);
%     D11(4, 2) = D11(2, 4);
%     D11(4, 3) = D11(3, 4);
%     D11(4, 4) = 1/q4 + e^2/q3*((q1 + ial^2)*(1/2 + 1/2*exp(-2*q4)*cos(2*the))...
%                 + 2*ial*ibe*sin(the)*cos(the)*exp(-2*q4)...
%                 + (q2 + ibe^2)*(1/2 - 1/2*exp(-2*q4)*cos(2*the)))...
%                 + b^2/q2*(q3 + ome^2)*(1/2 - 1/2*exp(-2*q4)*cos(2*the))...
%                 + b^2/q1*(q3 + ome^2)*(1/2 + 1/2*exp(-2*q4)*cos(2*the));
%             
%     DF1(1,1) = a;
%     DF1(1,2) = 0.0;
%     DF1(1,3) = b*sin(the)*exp(-1/2*q4);
%     DF1(1,4) = b*ome*cos(the)*exp(-1/2*q4);
%     
%     DF1(2,1) = 0.0;
%     DF1(2,2) = a;
%     DF1(2,3) = -b*cos(the)*exp(-1/2*q4);
%     DF1(2,4) = b*ome*sin(the)*exp(-1/2*q4);
%     
%     DF1(3,1) = -e*sin(the)*exp(-1/2*q4);
%     DF1(3,2) = e*cos(the)*exp(-1/2*q4);
%     DF1(3,3) = d;
%     DF1(3,4) = -e*(ial*cos(the)*exp(-1/2*q4) + ibe*sin(the)*exp(-1/2*q4));
%     
%     DF1(4,1) = 0.0;
%     DF1(4,2) = 0.0;
%     DF1(4,3) = dt;
%     DF1(4,4) = 1.0;
%     
%     D12 = -DF1'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
%     
%     Jj1 = D22 - D21*inv(Jj1 + D11)*D12;
%         
%     iJn1(:,:,t) = inv(Jj1);
    
%     D11_b(:,:,t) = D11;
    
    %d-q red, equal Ls
%     DH2(1,1) = cos(the);
%     DH2(1,2) = -sin(the);
%     DH2(1,4) = -id*sin(the)-iq*cos(the);
%     DH2(2,1) = sin(the);
%     DH2(2,2) = cos(the);
%     DH2(2,4) = id*cos(the)-iq*sin(the);
%     D11 = DF2'*inv(Q)*DF2;
%     D12 = -DF2'*inv(Q);
%     D21 = D12';
% %     D22 = inv(Q) + DH'*inv(R)*DH;
%     D22 = inv(Q) + DH2'*inv(R)*DH2;
%     
%     Jj2 = D22 - D21*inv(Jj2 + D11)*D12;
%         
%     iJn2(:,:,t) = inv(Jj2);
    
%     d-q full, equal Ls
%     DF3(1,2) = dt*ome;
%     DF3(1,3) = dt*iq;
%     DF3(2,1) = -dt*ome;
%     DF3(2,3) = -dt*id-b;
%     D11 = DF3'*inv(Q)*DF3 + opr_3_D11;
%     D12 = -DF3'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
% %     D22 = inv(Q) + DH2'*inv(R)*DH2;
%     
%     Jj3 = D22 - D21*inv(Jj3 + D11)*D12;
%         
%     iJn3(:,:,t) = inv(Jj3);
    
    %d-q, not equal Ld, Lq
%     DF4(1,2) = Lq*dt/Ld*ome;
%     DF4(1,3) = Lq*dt/Ld*iq;
%     DF4(2,1) = -Ld*dt/Lq*ome;
%     DF4(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
%     DF4(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
%     DF4(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);    
%     D11 = DF4'*inv(Q)*DF4 + opr_4_D11;
%     D12 = -DF4'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
% %     D22 = inv(Q) + DH2'*inv(R)*DH2;
%     
%     Jj4 = D22 - D21*inv(Jj4 + D11)*D12;
%         
%     iJn4(:,:,t) = inv(Jj4);
    
    %alpha-beta, not equal Ld, Lq
%     ia = ial;
%     ib = ibe;
%     psi = psipm;
%     kppj = kp*pp*pp/J;
%     DF5 = [[ (Lq - Rs*dt*sin(the)^2)/Lq - (dt*ome*sin(the)*Lq^2*cos(the) + Rs*dt*Lq*cos(the)^2)/(Ld*Lq) + (Ld*dt*ome*cos(the)*sin(the))/Lq,                                       (dt*(Ld - Lq)*(- Lq*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Ld*ome*sin(the)^2))/(Ld*Lq),    dt*cos(the)*(ia*sin(the) - ib*cos(the) + (Lq*(ib*cos(the) - ia*sin(the)))/Ld) + dt*sin(the)*(psi/Lq - ia*cos(the) - ib*sin(the) + (Ld*(ia*cos(the) + ib*sin(the)))/Lq),                                               (dt*(ome*psi*cos(the) + Rs*ib*cos(2*the) - Rs*ia*sin(2*the)))/Lq + (Ld*dt*(ia*ome*cos(2*the) + ib*ome*sin(2*the)))/Lq - (dt*(Lq^2*ia*ome*cos(2*the) + Lq^2*ib*ome*sin(2*the) + Lq*Rs*ib*cos(2*the) - Lq*Rs*ia*sin(2*the)))/(Ld*Lq)];...
%             [                                       (dt*(Ld - Lq)*(- Ld*ome*cos(the)^2 + Rs*cos(the)*sin(the) + Lq*ome*sin(the)^2))/(Ld*Lq), (Lq - Rs*dt*cos(the)^2)/Lq - (Lq*Rs*dt*sin(the)^2 - Lq^2*dt*ome*cos(the)*sin(the))/(Ld*Lq) - (Ld*dt*ome*cos(the)*sin(the))/Lq, (dt*(Lq*ia - psi*cos(the)))/Lq + (dt*((Lq^2*ia*cos(2*the))/2 - (Lq^2*ia)/2 + (Lq^2*ib*sin(2*the))/2))/(Ld*Lq) - (Ld*dt*(ia/2 + (ia*cos(2*the))/2 + (ib*sin(2*the))/2))/Lq, (dt*ome*psi*sin(the) - Rs*dt*ia*(2*sin(the)^2 - 1) + Rs*dt*ib*sin(2*the))/Lq + (Ld*(dt*ib*ome*(2*sin(the)^2 - 1) + dt*ia*ome*sin(2*the)))/Lq - (Lq*Rs*dt*ib*sin(2*the) + Lq^2*dt*ib*ome*(2*sin(the)^2 - 1) + Lq^2*dt*ia*ome*sin(2*the) - Lq*Rs*dt*ia*(2*sin(the)^2 - 1))/(Ld*Lq)];...
%             [     -dt*kppj*(psi*sin(the) - cos(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the)) + sin(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the))),      dt*kppj*(psi*cos(the) + cos(the)*(Ld - Lq)*(ia*cos(the) + ib*sin(the)) + sin(the)*(Ld - Lq)*(ib*cos(the) - ia*sin(the))),                                                                                                                                                                         1.0,                                                                                                                                                   -dt*kppj*(psi*(ia*cos(the) + ib*sin(the)) + (Ld - Lq)*(ia*cos(the) + ib*sin(the))^2 - (Ld - Lq)*(ib*cos(the) - ia*sin(the))^2)];...
%             [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
%     D11 = DF5'*inv(Q)*DF5;
%     D12 = -DF5'*inv(Q);
%     D21 = D12';
%     D22 = inv(Q) + DH'*inv(R)*DH;
%     
%     Jj5 = D22 - D21*inv(Jj5 + D11)*D12;
%         
%     iJn5(:,:,t) = inv(Jj5);
    
%     delma = abs(DF - DF5);
%     nrm(t) = max(delma(:));
    
%     alpha-beta, equal Ls sc theta
    DF6(1,3) = b*sin(the);
    DF6(1,4) = b*ome;
    DF6(2,3) = -b*cos(the);
    DF6(2,5) = -b*ome;
    DF6(3,1) = -e*sin(the);
    DF6(3,2) = e*cos(the);
    DF6(3,4) = -e*ial;
    DF6(3,5) = e*ibe;
    DF6(4,3) = dt*cos(the);
    DF6(4,5) = dt*ome;
    DF6(5,3) = -dt*sin(the);
    DF6(5,4) = -dt*ome;
        
    D11 = DF6'*inv(Q6)*DF6;
%     D11 = DF6'*inv(Q6)*DF6 + opr_D11;
    D12 = -DF6'*inv(Q6);
    D21 = D12';
    D22 = inv(Q6) + DH6'*inv(R)*DH6;
    
    Jj6 = D22 - D21*inv(Jj6 + D11)*D12;
        
    iJn6(:,:,t) = inv(Jj6);

    
%     %     alpha-beta, not equal Ld Lq sc theta, bad E  
%     st = sin(the);
%     ct = cos(the);
%     DF7(1, 1) = 1.0 + dt*(-Rs*(st^2/Lq + ct^2/Ld) + ome*st*ct*(Ld/Lq - Lq/Ld));
%     DF7(1, 2) = dt*(-ome + Rs*st*ct*(1/Lq - 1/Ld) + ome*(Ld*st^2/Lq + Lq*ct^2/Ld));
%     DF7(1, 3) = dt*psi*st/Lq + dt*ial*st*ct*(Ld/Lq - Lq/Ld) + dt*ibe*(-1 + Ld/Lq*st^2 + Lq/Ld*ct^2);
%     DF7(1, 4) = dt*psi*ome/Lq + dt*ial*(-Rs*2*st/Lq + ome*ct*(Ld/Lq - Lq/Ld)) + dt*ibe*(Rs*ct*(1/Lq - 1/Ld) + ome*2*Ld*st/Lq);
%     DF7(1, 5) = dt*ial*(-Rs*2*ct/Ld + ome*st*(Ld/Lq - Lq/Ld)) + dt*ibe*(Rs*st*(1/Lq - 1/Ld) + ome*2*Lq*ct/Ld);
% 
%     DF7(2, 1) = dt*(ome + Rs*st*ct*(1/Lq - 1/Ld) - ome*(Lq*st^2/Ld + Ld*ct^2/Lq));
%     DF7(2, 2) = 1 + dt*(-Rs*(st^2/Ld + ct^2/Lq) + ome*st*ct*(Lq/Ld - Ld/Lq));
%     DF7(2, 3) = -dt*psi*ct/Lq + dt*ial*(1 - Lq*st^2/Ld - Ld*ct^2/Lq) + dt*ibe*st*ct*(Lq/Ld - Ld/Lq);
%     DF7(2, 4) = dt*ial*(Rs*ct*(1/Lq - 1/Ld) - ome*2*Lq*st/Ld) + dt*ibe*(-Rs*2*st/Ld + ome*ct*(Lq/Ld - Ld/Lq));
%     DF7(2, 5) = -dt*psi*ome/Lq + dt*ial*(Rs*st*(1/Lq - 1/Ld) -ome*2*Ld*ct/Lq) + dt*ibe*(-Rs*2*ct/Lq + ome*st*(Lq/Ld - Ld/Lq));
% 
%     DF7(3, 1) = dt*kpp/J*((Ld - Lq)*(-2*ial*st*ct + ibe*ct^2 - ibe*st^2) - psi*st);
%     DF7(3, 2) = dt*kpp/J*((Ld - Lq)*(ial*ct^2 - ial*st^2 + 2*ibe*st*ct) + psi*ct);
%     DF7(3, 3) = 1;
%     DF7(3, 4) = dt*kpp/J*((Ld - Lq)*(-ial^2*ct - ial*ibe*2*st + ibe^2*ct) - psi*ial);
%     DF7(3, 5) = dt*kpp/J*((Ld - Lq)*(-ial^2*st + ial*ibe*2*ct + ibe^2*st) + psi*ibe);
%     
%     DF7(4,3) = dt*cos(the);
%     DF7(4,5) = dt*ome;
%     DF7(5,3) = -dt*sin(the);
%     DF7(5,4) = -dt*ome;
%         
% %     D11(1,1) = (dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)^2/q1 + (dt^2*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq))^2)/q2 + (dt^2*kpp^2*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2))^2)/(J^2*q3);
% %     D11(1,2) = (dt*(dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 + (dt*(dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq)))/q2 - (dt^2*kpp^2*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2))*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J^2*q3);
% %     D11(1,3) = (dt*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq))*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 - ((dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld)))/q1 - (dt*kpp*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J*q3);
% %     D11(1,4) = (dt*(dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq)))/q2 - ((dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq))/q1 + (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J^2*q3);
% %     D11(1,5) = (dt*(ome*((Ld*ct^2)/Lq + (Lq*st^2)/Ld) - ome + Rs*ct*st*(1/Ld - 1/Lq))*(dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq))/q2 - ((dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(dt*(Rs*(ct^2/Ld + st^2/Lq) - ct*ome*st*(Ld/Lq - Lq/Ld)) - 1))/q1 - (dt^2*kpp^2*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2))*(psi*st + (Ld - Lq)*(- ibe*ct^2 + 2*ial*ct*st + ibe*st^2)))/(J^2*q3);
% %     
% %     D11(2,1) = D11(1,2);
% %     D11(2,2) = (dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)^2/q2 + (dt^2*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq))^2)/q1 + (dt^2*kpp^2*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2))^2)/(J^2*q3);
% %     D11(2,3) = ((dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 - (dt*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld))*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 + (dt*kpp*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2)))/(J*q3);
% %     D11(2,4) = ((dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1))/q2 - (dt*(dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq)*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 - (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2)))/(J^2*q3);
% %     D11(2,5) = ((dt*(Rs*(ct^2/Lq + st^2/Ld) + ct*ome*st*(Ld/Lq - Lq/Ld)) - 1)*(dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq))/q2 - (dt*(dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(ome - ome*((Lq*ct^2)/Ld + (Ld*st^2)/Lq) + Rs*ct*st*(1/Ld - 1/Lq)))/q1 + (dt^2*kpp^2*(ct*psi + (Ld - Lq)*(ial*ct^2 + 2*ibe*ct*st - ial*st^2))*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2)))/(J^2*q3);
% %     
% %     D11(3,1) = D11(1,3);
% %     D11(3,2) = D11(2,3);
% %     D11(3,3) = (dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld))^2/q2 + (dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld))^2/q1 + 1/q3 + (ct^2*dt^2)/q4 + (dt^2*st^2)/q5;
% %     D11(3,4) = ((dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq)*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld)))/q1 + ((dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 + (ct*dt)/q4 + (dt^2*ome*st)/q5 - (dt*kpp*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2)))/(J*q3);
% %     D11(3,5) = ((dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(dt*ibe*((Lq*ct^2)/Ld + (Ld*st^2)/Lq - 1) + (dt*psi*st)/Lq + ct*dt*ial*st*(Ld/Lq - Lq/Ld)))/q1 - (dt*st)/q5 + ((dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq)*(dt*ial*((Ld*ct^2)/Lq + (Lq*st^2)/Ld - 1) + (ct*dt*psi)/Lq + ct*dt*ibe*st*(Ld/Lq - Lq/Ld)))/q2 + (ct*dt^2*ome)/q4 + (dt*kpp*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2)))/(J*q3);
% %     
% %     D11(4,1) = D11(1,4);
% %     D11(4,2) = D11(2,4);
% %     D11(4,3) = D11(3,4);
% %     D11(4,4) = (dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))^2/q2 + 1/q4 + (dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq)^2/q1 + (dt^2*ome^2)/q5 + (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))^2)/(J^2*q3);
% %     D11(4,5) = (dt*ome)/q4 - (dt*ome)/q5 + ((dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))*(dt*ial*(ct*ome*(Ld/Lq - Lq/Ld) - (2*Rs*st)/Lq) - dt*ibe*(Rs*ct*(1/Ld - 1/Lq) - (2*Ld*ome*st)/Lq) + (dt*ome*psi)/Lq))/q1 + ((dt*ibe*(ct*ome*(Ld/Lq - Lq/Ld) + (2*Rs*st)/Ld) + dt*ial*(Rs*ct*(1/Ld - 1/Lq) + (2*Lq*ome*st)/Ld))*(dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq))/q2 - (dt^2*kpp^2*(ial*psi + (Ld - Lq)*(ct*ial^2 + 2*st*ial*ibe - ct*ibe^2))*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2)))/(J^2*q3);
% %     
% %     D11(5,1) = D11(1,5);
% %     D11(5,2) = D11(2,5);
% %     D11(5,3) = D11(3,5);
% %     D11(5,4) = D11(4,5);
% %     D11(5,5) = (dt*ial*(ome*st*(Ld/Lq - Lq/Ld) - (2*Rs*ct)/Ld) - dt*ibe*(Rs*st*(1/Ld - 1/Lq) - (2*Lq*ct*ome)/Ld))^2/q1 + 1/q5 + (dt*ibe*(ome*st*(Ld/Lq - Lq/Ld) + (2*Rs*ct)/Lq) + dt*ial*(Rs*st*(1/Ld - 1/Lq) + (2*Ld*ct*ome)/Lq) + (dt*ome*psi)/Lq)^2/q2 + (dt^2*ome^2)/q4 + (dt^2*kpp^2*(ibe*psi + (Ld - Lq)*(- st*ial^2 + 2*ct*ial*ibe + st*ibe^2))^2)/(J^2*q3);   
%     
%     D11 = DF7'*inv(Q6)*DF7;
% 
%     D12 = -DF7'*inv(Q6);
%     D21 = D12';
%     D22 = inv(Q6) + DH6'*inv(R)*DH6;
%     
%     Jj7 = D22 - D21*inv(Jj7 + D11)*D12;
%         
%     iJn7(:,:,t) = inv(Jj7);
    
    %%%%%%%%%%%%%%%%%%%%%%%%%%%        
               
        
        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(2,t) = u_dq(2,t) + 10.0*cos(100*dt*t);   
        
%         ual = u_dq(1,t)*cos(the) - u_dq(2,t)*sin(the);
%         ube = u_dq(1,t)*sin(the) + u_dq(2,t)*cos(the);
% 
% %         ual = ual + 10.0*cos(100*dt*t);
% %         ube = ube + 10.0*sin(100*dt*t);
% 
% %         duab = sign(rand(2,1)-0.5)*10;
% %         ual = ual + duab(1);
% %         ube = ube + duab(2);
%         
%         ud = ual*cos(the) + ube*sin(the);
%         uq = ube*cos(the) - ual*sin(the);
    
%     u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
    
    %%%% simulace %%% (xt + ut -> xt+1; xt+1 -> yt+1)
    idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*u_dq(1,t);
    iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*u_dq(2,t);
    
    x_sys(1, t+1) = idpl*cos(the) - iqpl*sin(the);
    x_sys(2, t+1) = idpl*sin(the) + iqpl*cos(the);
    
    x_sys(3, t+1) = ref_ome(t);%(1.0-B*dt/J)*ome + kp*pp*pp*dt/J*((Ld-Lq)*id*iq + psipm*iq);
    x_sys(4, t+1) = the + dt*ome;
        
end

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

xax = 1:T-1;
timex = (xax)*dt;

% subplot(2, 2, 1);
% plot(timex, x_sys(1, xax));
% subplot(2, 2, 2);
% plot(timex, x_sys(2, xax));
% subplot(2, 2, 3);
% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
% subplot(2, 2, 4);
% plot(timex, atan2(sin(x_sys(4, xax)),cos(x_sys(4, xax))));

% subplot(2,1,1);
% plot(timex, x_sys(3, xax), timex, ref_ome(xax));
% subplot(2,1,2);
%  plot(timex,nrm(xax),'g')
% hold on;
% plot(timex, squeeze(iJn(4,4,xax)),timex, squeeze(iJn1(4,4,xax)),timex, squeeze(iJn6(4,4,xax)),timex, squeeze(iJn6(5,5,xax)));
plot(timex, squeeze(iJn6(4,4,xax)),timex, squeeze(iJn6(5,5,xax)),timex, squeeze(iJn7(4,4,xax)),timex, squeeze(iJn7(5,5,xax)));

% figure;
% plot(timex, x_sys(3, xax)-ref_ome(xax));