Index: /applications/dual/vahala/pcrb/pcrb.m
===================================================================
--- /applications/dual/vahala/pcrb/pcrb.m (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb.m (revision 1416)
@@ -0,0 +1,542 @@
+%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));
Index: /applications/dual/vahala/pcrb/pcrb2.lyx
===================================================================
--- /applications/dual/vahala/pcrb/pcrb2.lyx (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb2.lyx (revision 1416)
@@ -0,0 +1,2071 @@
+#LyX 2.0 created this file. For more info see http://www.lyx.org/
+\lyxformat 413
+\begin_document
+\begin_header
+\textclass article
+\use_default_options true
+\maintain_unincluded_children false
+\language english
+\language_package default
+\inputencoding auto
+\fontencoding global
+\font_roman default
+\font_sans default
+\font_typewriter default
+\font_default_family default
+\use_non_tex_fonts false
+\font_sc false
+\font_osf false
+\font_sf_scale 100
+\font_tt_scale 100
+
+\graphics default
+\default_output_format default
+\output_sync 0
+\bibtex_command default
+\index_command default
+\paperfontsize default
+\use_hyperref false
+\papersize default
+\use_geometry false
+\use_amsmath 1
+\use_esint 1
+\use_mhchem 1
+\use_mathdots 1
+\cite_engine basic
+\use_bibtopic false
+\use_indices false
+\paperorientation portrait
+\suppress_date false
+\use_refstyle 1
+\index Index
+\shortcut idx
+\color #008000
+\end_index
+\secnumdepth 3
+\tocdepth 3
+\paragraph_separation indent
+\paragraph_indentation default
+\quotes_language english
+\papercolumns 1
+\papersides 1
+\paperpagestyle default
+\tracking_changes false
+\output_changes false
+\html_math_output 0
+\html_css_as_file 0
+\html_be_strict false
+\end_header
+
+\begin_body
+
+\begin_layout Title
+Výpočet PCRB
+\end_layout
+
+\begin_layout Subsection*
+PCRB obecně
+\end_layout
+
+\begin_layout Standard
+Výpočet PCRB (Posterior Cramer-Rao Bound) dle [Posterior Cramer-Rao Bounds
+ for Discrete-Time Nonlinear Filtering, 1998, Tichavský P.
+ et al.] jako:
+\begin_inset Formula 
+\[
+P\triangleq\mathtt{E}\left\{ \left[g(x)-\theta\right]\left[g(x)-\theta\right]^{T}\right\} \geq J^{-1}
+\]
+
+\end_inset
+
+kde 
+\begin_inset Formula $x$
+\end_inset
+
+ reprezentuje vektor měřených dat, 
+\begin_inset Formula $\theta$
+\end_inset
+
+ je vektorový estimovaný náhodný parametr a 
+\begin_inset Formula $g(x)$
+\end_inset
+
+ je funkce 
+\begin_inset Formula $x$
+\end_inset
+
+, která je odhadem 
+\begin_inset Formula $\theta$
+\end_inset
+
+.
+ 
+\begin_inset Formula $J$
+\end_inset
+
+ je (Fisherova) informační matice
+\begin_inset Formula 
+\[
+J_{ij}=\mathtt{E}\left[-\frac{\partial^{2}\log p_{x,\theta}(X,\Theta)}{\partial\Theta_{i}\partial\Theta_{j}}\right]
+\]
+
+\end_inset
+
+kde 
+\family roman
+\series medium
+\shape up
+\size normal
+\emph off
+\bar no
+\strikeout off
+\uuline off
+\uwave off
+\noun off
+\color none
+
+\begin_inset Formula $p_{x,\theta}(X,\Theta)$
+\end_inset
+
+ je sdružená hustota pravděpodobnosti dvojice 
+\begin_inset Formula $(x,\theta)$
+\end_inset
+
+.
+\end_layout
+
+\begin_layout Subsection*
+PCRB nelineární filtrace
+\end_layout
+
+\begin_layout Standard
+Spodní mez pro nelineární filtrační problém systému
+\begin_inset Formula 
+\begin{eqnarray*}
+x_{n+1} & = & f_{n}(x_{n},w_{n})\\
+z_{n} & = & h_{n}(x_{n},v_{n})
+\end{eqnarray*}
+
+\end_inset
+
+kde 
+\begin_inset Formula $x_{n}$
+\end_inset
+
+ je stav systému v čase 
+\begin_inset Formula $n$
+\end_inset
+
+, 
+\begin_inset Formula $z_{n}$
+\end_inset
+
+ je pozorování v čase 
+\begin_inset Formula $n$
+\end_inset
+
+, 
+\begin_inset Formula $w$
+\end_inset
+
+ a 
+\begin_inset Formula $v$
+\end_inset
+
+ jsou vzájemně nezávislé bílé procesy a 
+\begin_inset Formula $f_{n}$
+\end_inset
+
+ a 
+\begin_inset Formula $h_{n}$
+\end_inset
+
+ jsou obecně nelineární funkce.
+ Pak je možné počítat rekurzivně posloupnost matic 
+\begin_inset Formula $J_{n}$
+\end_inset
+
+ jako:
+\begin_inset Formula 
+\begin{equation}
+J_{n+1}=D_{n}^{22}-D_{n}^{21}\left(J_{n}+D_{n}^{11}\right)^{-1}D_{n}^{12}\label{eq: rekurze J}
+\end{equation}
+
+\end_inset
+
+kde se matice 
+\begin_inset Formula $D_{n}$
+\end_inset
+
+ počítají jako
+\begin_inset Formula 
+\begin{eqnarray}
+D_{n}^{11} & = & \mathtt{E}\left\{ -\triangle_{x_{n}}^{x_{n}}\log p(x_{n+1}\mid x_{n})\right\} \nonumber \\
+D_{n}^{12} & = & \mathtt{E}\left\{ -\triangle_{x_{n}}^{x_{n+1}}\log p(x_{n+1}\mid x_{n})\right\} \label{eq:matice Dn}\\
+D_{n}^{21} & = & \mathtt{E}\left\{ -\triangle_{x_{n+1}}^{x_{n}}\log p(x_{n+1}\mid x_{n})\right\} =\left(D_{n}^{12}\right)^{T}\nonumber \\
+D_{n}^{22} & = & \mathtt{E}\left\{ -\triangle_{x_{n+1}}^{x_{n+1}}\log p(x_{n+1}\mid x_{n})\right\} +\mathtt{E}\left\{ -\triangle_{x_{n+1}}^{x_{n+1}}\log p(z_{n+1}\mid x_{n+1})\right\} \nonumber 
+\end{eqnarray}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsection*
+PCRB Gaussovské
+\end_layout
+
+\begin_layout Standard
+Pro aditivní Gaussovský šum s nulovou střední hodnotou a invertovatelnými
+ kovariančními maticemi 
+\begin_inset Formula $Q_{n}$
+\end_inset
+
+ a 
+\begin_inset Formula $R_{n}$
+\end_inset
+
+ platí následující vztahy pro výpočet matic 
+\begin_inset Formula $D_{n}$
+\end_inset
+
+ jako speciální případ (
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq:matice Dn"
+
+\end_inset
+
+):
+\begin_inset Formula 
+\begin{eqnarray}
+D_{n}^{11} & = & \mathtt{E}\left\{ \left[\nabla_{x_{n}}f_{n}^{T}(x_{n})\right]Q_{n}^{-1}\left[\nabla_{x_{n}}f_{n}^{T}(x_{n})\right]^{T}\right\} \nonumber \\
+D_{n}^{12} & = & -\mathtt{E}\left\{ \nabla_{x_{n}}f_{n}^{T}(x_{n})\right\} Q_{n}^{-1}\label{eq:matice Dn gauss}\\
+D_{n}^{22} & = & Q_{n}^{-1}+\mathtt{E}\left\{ \left[\nabla_{x_{n+1}}h_{n+1}^{T}(x_{n+1})\right]R_{n+1}^{-1}\left[\nabla_{x_{n+1}}h_{n+1}^{T}(x_{n+1})\right]^{T}\right\} \nonumber 
+\end{eqnarray}
+
+\end_inset
+
+V případě linearity funkcí 
+\begin_inset Formula $f_{n}$
+\end_inset
+
+ a 
+\begin_inset Formula $h_{n}$
+\end_inset
+
+ pak rekurzivní výpočet matice 
+\begin_inset Formula $J_{n}$
+\end_inset
+
+ (
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq: rekurze J"
+
+\end_inset
+
+) spolu s dosazením výše uvedených matic 
+\begin_inset Formula $D_{n}$
+\end_inset
+
+ (
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq:matice Dn gauss"
+
+\end_inset
+
+) odpovídá výpočtu aposteriorní kovarianční matice 
+\begin_inset Formula $P_{n}$
+\end_inset
+
+ Kalmanova filtru při označení 
+\begin_inset Formula $\left(P_{n}\right)^{-1}=J_{n}$
+\end_inset
+
+.
+ Uvažovaný systém (PMSM) je však nelineární, je tedy užíváno rozšířeného
+ Kalmanova filtru (EKF), ve kterém se do napočtených matic derivací dosazují
+ odhady stavu.
+ Oproti tomu ve výše uvedených rovnicích (
+\begin_inset CommandInset ref
+LatexCommand ref
+reference "eq:matice Dn gauss"
+
+\end_inset
+
+) je počítána střední hodnota derivací (respektive jejich kvadrátů).
+ Jak významný je rozdíl dvou takto různě provedených výpočtů bude předmětem
+ testů v následujících experimentech.
+ Správný výpočet střední hodnoty derivace bude označován jako 
+\series bold
+E
+\series default
+ a dosazení očekávané hodnoty do napočtené derivace pak jako 
+\series bold
+nE
+\series default
+.
+ Dále je třeba poznamenat, že správné výpočty střední hodnoty (E) je třeba
+ provést analyticky (symbolicky), což je i pro Gaussovská rozdělení velmi
+ výpočetně náročná úloha a proto byla provedena za předpokladu nezávislosti
+ a časové invariance, tedy kovarianční matice 
+\begin_inset Formula $Q_{n}$
+\end_inset
+
+ je diagonální a konstantní v čase, 
+\begin_inset Formula $Q_{n}=Q=\mathrm{diag}([q_{i}]_{i=1}^{\mathrm{dim}(x)})$
+\end_inset
+
+.
+ Matice 
+\begin_inset Formula $R_{n}$
+\end_inset
+
+ je také použita konstantní a diagonální, ovšem těchto předpokladů není
+ třeba, protože použité zobrazení 
+\begin_inset Formula $h_{n}$
+\end_inset
+
+ je lineární a nezávislé na čase a jeho derivací je konstantní matice.
+\end_layout
+
+\begin_layout Subsection*
+Užité modely
+\end_layout
+
+\begin_layout Standard
+Obecně byly použity čtyři typy modelů v souřadném systému 
+\begin_inset Formula $\alpha\beta$
+\end_inset
+
+.
+ Souřadný systém 
+\begin_inset Formula $dq$
+\end_inset
+
+ totiž nemá smysl používat, jelikož mez stále roste, což lze jednak usuzovat
+ na základě tvaru ronvic, ale bylo ověřeno i experimentálně.
+ Tyto modely se liší tím, jestli je uvažován 
+\emph on
+plný 
+\emph default
+nebo 
+\emph on
+redukovaný
+\emph default
+ stav systému.
+ Dále pak jestli byl uvažován model motoru (PMSM) se stejnými (
+\series bold
+Ls
+\series default
+) nebo různými (
+\series bold
+Ldq
+\series default
+) indukčnostmi v osách 
+\begin_inset Formula $d$
+\end_inset
+
+ a 
+\begin_inset Formula $q$
+\end_inset
+
+.
+ Budou následovat matice derivací 
+\begin_inset Formula $A_{n}=\left[\nabla_{x_{n}}f_{n}^{T}(x_{n})\right]^{T}$
+\end_inset
+
+ zobrazení 
+\begin_inset Formula $f_{n}$
+\end_inset
+
+ a matice 
+\begin_inset Formula $C_{n+1}=\left[\nabla_{x_{n+1}}h_{n+1}^{T}(x_{n+1})\right]^{T}$
+\end_inset
+
+ zobrazení 
+\begin_inset Formula $h_{n+1}$
+\end_inset
+
+ dle jednotlivých stavových veličin.
+ Tyto matice však budou uvedeny pouze pro případ stejných indukčností.
+\begin_inset Formula 
+\[
+A_{full}^{Ls}=\left[\begin{array}{cccc}
+a & 0 & b\sin\vartheta & b\omega\cos\vartheta\\
+0 & a & -b\cos\vartheta & b\omega\sin\vartheta\\
+-e\sin\vartheta & e\cos\vartheta & d & -e\left(i_{\alpha}\cos\vartheta+i_{\beta}\sin\vartheta\right)\\
+0 & 0 & \Delta t & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\begin_inset Formula 
+\[
+C_{full}^{Ls}=\left[\begin{array}{cccc}
+1 & 0 & 0 & 0\\
+0 & 1 & 0 & 0
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\begin_inset Formula 
+\[
+A_{red}^{Ls}=\left[\begin{array}{cc}
+d & -e\left(i_{\alpha}\cos\vartheta+i_{\beta}\sin\vartheta\right)\\
+\Delta t & 1
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\begin_inset Formula 
+\[
+C_{red}^{Ls}=\left[\begin{array}{cc}
+b\sin\vartheta & b\omega\cos\vartheta\\
+-b\cos\vartheta & b\omega\sin\vartheta
+\end{array}\right]
+\]
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Pro přehlednost je souhrn použitých modelů uveden v následující tabulce:
+\end_layout
+
+\begin_layout Standard
+\align center
+\begin_inset Tabular
+<lyxtabular version="3" rows="3" columns="3">
+<features tabularvalignment="middle">
+<column alignment="center" valignment="top" width="0">
+<column alignment="center" valignment="top" width="0">
+<column alignment="center" valignment="top" width="0">
+<row>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Formula $\alpha-\beta$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Formula $L_{s}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Formula $L_{dq}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+full
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+1
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+2
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+red
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+3
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+4
+\end_layout
+
+\end_inset
+</cell>
+</row>
+</lyxtabular>
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Dále pak budou jednotlivé modely oznáčovány jejich číslem z tabulky.
+\end_layout
+
+\begin_layout Subsection*
+Užitá řízení
+\end_layout
+
+\begin_layout Standard
+Použitá řízení shrnuje následující seznam, dále budou označována svým číslem
+ položky:
+\end_layout
+
+\begin_layout Enumerate
+\begin_inset Formula $\omega=\overline{\omega}$
+\end_inset
+
+, 
+\begin_inset Formula $\vartheta=\int\omega$
+\end_inset
+
+, 
+\begin_inset Formula $i_{\alpha}=i_{\beta}=0$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI
+\end_layout
+
+\begin_layout Enumerate
+PI + injektáž sin do 
+\begin_inset Formula $d-q$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + injektáž obdélníků do 
+\begin_inset Formula $d-q$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + injektáž konstanty do 
+\begin_inset Formula $d$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + náhodná chyba na 
+\begin_inset Formula $\overline{\omega}$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + injektáž sin do 
+\begin_inset Formula $\alpha-\beta$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + injektáž obdélníků do 
+\begin_inset Formula $\alpha-\beta$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + bikriteriální metoda se 
+\begin_inset Formula $\mathrm{sign}\omega$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Enumerate
+PI + bikriteriální metoda náhodný výběr 5 možností
+\end_layout
+
+\begin_layout Subsection*
+Kovarianční matice
+\end_layout
+
+\begin_layout Standard
+Testování proběhlo s následujícími kovariančními maticemi:
+\end_layout
+
+\begin_layout Labeling
+\labelwidthstring 00.00.0000
+šumové 
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left(\left[\begin{array}{cccc}
+0.0013 & 0.0013 & 5.0e-6 & 1.0e-10\end{array}\right]\right)\\
+R & = & \mathrm{diag\left(\left[\begin{array}{cc}
+0.0006 & 0.0006\end{array}\right]\right)}
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Labeling
+\labelwidthstring 00.00.0000
+ekf 
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left(\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 0.001\end{array}\right]\right)\\
+R & = & \mathrm{diag\left(\left[\begin{array}{cc}
+0.05 & 0.05\end{array}\right]\right)}
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Labeling
+\labelwidthstring 00.00.0000
+t1 
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left(\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 0.1\end{array}\right]\right)\\
+R & = & \mathrm{diag\left(\left[\begin{array}{cc}
+0.05 & 0.05\end{array}\right]\right)}
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Labeling
+\labelwidthstring 00.00.0000
+t2 
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left(\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 1.0\end{array}\right]\right)\\
+R & = & \mathrm{diag\left(\left[\begin{array}{cc}
+0.05 & 0.05\end{array}\right]\right)}
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Labeling
+\labelwidthstring 00.00.0000
+t3 
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left(\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 10.0\end{array}\right]\right)\\
+R & = & \mathrm{diag\left(\left[\begin{array}{cc}
+0.5 & 0.5\end{array}\right]\right)}
+\end{eqnarray*}
+
+\end_inset
+
+V případě redukovaných modelů se první dva diagonální prvky 
+\begin_inset Formula $Q$
+\end_inset
+
+ nevyužijí.
+\end_layout
+
+\begin_layout Subsection*
+Experimenty
+\end_layout
+
+\begin_layout Standard
+Následující experimenty byly prováděny s ohledem na následující poznatky:
+\end_layout
+
+\begin_layout Itemize
+variance na proudech se ve všech případech ukazovaly jako malé (stále kolem
+ 
+\begin_inset Formula $0.0367$
+\end_inset
+
+) a dále již pak nebyly testovány
+\end_layout
+
+\begin_layout Itemize
+redukované modely díky výpočtu střední hodnoty přes 
+\begin_inset Formula $x_{n+1}$
+\end_inset
+
+ vykazují lepší vlastnosti PCR meze, která již v nulových otáčkách neroste
+ stále, ale zastavuje se na určité maximální hodnotě
+\end_layout
+
+\begin_layout Itemize
+modely pro stejné (Ls) a různé (Ldq) indukčnosti dávají obvykle téměř shodné
+ výsledky, injektáží periodického signálu (sinus, obdélníky) lze vylepšít
+ (snížit) mez pro různé indukčnosti (Ldq), toto 
+\begin_inset Quotes eld
+\end_inset
+
+zlepšení
+\begin_inset Quotes erd
+\end_inset
+
+ pak závisí na frekvenci injektovaného signálu, obecně vyšší frekvence dává
+ lepší výsledek
+\end_layout
+
+\begin_layout Itemize
+nastavení kovariančních matic 
+\begin_inset Formula $Q$
+\end_inset
+
+ a 
+\begin_inset Formula $R$
+\end_inset
+
+ nemá vliv na tvar křivek znázorňujících PCRB, ovlivňuje však značně jejich
+ hodnoty v absolutním měřítku
+\end_layout
+
+\begin_layout Itemize
+hodnoty počáteční kovariance 
+\begin_inset Formula $P_{0}$
+\end_inset
+
+ se projevují pouze na počátku a jejich vliv s rostoucím časem asymptoticky
+ vymizí
+\end_layout
+
+\begin_layout Itemize
+existuje vliv na volbě referenční hodnoty 
+\begin_inset Formula $\overline{\omega}$
+\end_inset
+
+, tj.
+ hodnoty kterou sledují skutečné otáčky 
+\begin_inset Formula $\omega$
+\end_inset
+
+; tento vliv je zřejmý, ale je zde ještě další charakterový vliv, při uvažování
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+ některá řízení fungují dobře, tj.
+ omezují PCRB, jiná však ne a fungují pouze u, alespoň částečně, nenulových
+ referenčních hodnot; toto lze předběžně interpretovat tak, že některá řízení
+ fungují dobře pořád a některá, jen když se nejdříve něco 
+\begin_inset Quotes eld
+\end_inset
+
+dozvědí
+\begin_inset Quotes erd
+\end_inset
+
+, tj.
+ již mají nějakou informaci
+\end_layout
+
+\begin_layout Itemize
+dále je třeba zkoumat především vliv jednotlivých řízení, a tedy i amplitud
+ a případně frekvencí injektovaných signálů
+\end_layout
+
+\begin_layout Subsubsection*
+Závislost PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ na použité amplitudě přídavného budícího signálu pro PI řízení s konstantní
+ injektáží do osy 
+\emph on
+d
+\end_layout
+
+\begin_layout Itemize
+Testované řízení: 
+\begin_inset Quotes eld
+\end_inset
+
+5 -- PI + injection (const.
+ -> ud)
+\begin_inset Quotes erd
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Použitý model: 
+\begin_inset Quotes eld
+\end_inset
+
+alpha-beta Ls
+\begin_inset Quotes erd
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Injektovaný signál:
+\end_layout
+
+\begin_deeper
+\begin_layout Itemize
+amplituda: různá, konstantní -- 
+\emph on
+předmět experimentu
+\end_layout
+
+\begin_layout Itemize
+frekvence: 
+\begin_inset Formula $\omega_{inj}\equiv0$
+\end_inset
+
+, tj.
+ neperiodický konstantní signál
+\end_layout
+
+\end_deeper
+\begin_layout Itemize
+Kovarianční matice systému: typ 
+\emph on
+ekf
+\emph default
+
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 0.001\end{array}\right]\\
+R & = & \mathrm{diag}\left[\begin{array}{cc}
+0.05 & 0.05\end{array}\right]
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Časový horizont: standartní 120000 vzorků, tj.
+ 15s
+\end_layout
+
+\begin_layout Itemize
+Referenční signál: nulový 
+\begin_inset Formula $\overline{\omega}=0$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Počáteční kovariance: dva testované případy 
+\begin_inset Formula $100\mathrm{Eye}$
+\end_inset
+
+ a 
+\begin_inset Formula $1\mathrm{Eye}$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Výsledné hodnoty PCRB polohy 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ v závislosti na amplitudě injektáže
+\begin_inset Foot
+status collapsed
+
+\begin_layout Plain Layout
+pro případ bez injektáže, tj.
+ 
+\begin_inset Formula $amp\equiv0$
+\end_inset
+
+, variance stále lineárně roste
+\end_layout
+
+\end_inset
+
+ zachycují grafy (Figure 1 a 2).
+\end_layout
+
+\begin_layout Standard
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Graphics
+	filename /home/michal/Plocha/DP/grafy/ampdepctr5hsvpoklze100.eps
+	scale 60
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Hodnoty PCRB polohy 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ v závislosti na amplitudě injektovaného konstantního signálu (viz legenda)
+ pro počáteční kovarianci 
+\begin_inset Formula $100\mathrm{Eye}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Graphics
+	filename /home/michal/Plocha/DP/grafy/ampdepctr5hsvrustz1.eps
+	scale 60
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Hodnoty PCRB polohy 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ v závislosti na amplitudě injektovaného konstantního signálu (viz legenda)
+ pro počáteční kovarianci 
+\begin_inset Formula $1\mathrm{Eye}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Subsubsection*
+Porovnání PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ pro jednotlivá řízení dle užitého referenčního signálu
+\end_layout
+
+\begin_layout Itemize
+Testovaná řízení: 1 -- 10
+\end_layout
+
+\begin_layout Itemize
+Použitý model: 
+\begin_inset Quotes eld
+\end_inset
+
+alpha-beta Ls
+\begin_inset Quotes erd
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Injektovaný signál:
+\end_layout
+
+\begin_deeper
+\begin_layout Itemize
+amplituda: 
+\begin_inset Formula $amp=10.0$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+frekvence: 
+\begin_inset Formula $\omega_{inj}=5000$
+\end_inset
+
+, (
+\emph on
+pozor
+\emph default
+ -- vysoká hodnota)
+\end_layout
+
+\end_deeper
+\begin_layout Itemize
+Kovarianční matice systému: typ 
+\emph on
+ekf
+\emph default
+
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 0.001\end{array}\right]\\
+R & = & \mathrm{diag}\left[\begin{array}{cc}
+0.05 & 0.05\end{array}\right]
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Časový horizont: standartní 120000 vzorků, tj.
+ 15s
+\end_layout
+
+\begin_layout Itemize
+Referenční signál: 
+\end_layout
+
+\begin_deeper
+\begin_layout Itemize
+nulový 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+ 
+\end_layout
+
+\begin_layout Itemize
+profil 
+\begin_inset Formula $\overline{\omega}_{profile}=\left[\begin{array}{cccccccccccccccc}
+0 & -1 & 3 & 6 & 9 & 6 & 3 & 0 & 0 & 0 & 0 & 0 & 0 & -3 & -6 & -3\end{array}\right]$
+\end_inset
+
+
+\end_layout
+
+\end_deeper
+\begin_layout Itemize
+Počáteční kovariance: 
+\begin_inset Formula $1\mathrm{Eye}$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Výsledné hodnoty PCRB polohy 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ v závislosti na užitém referenčním signálu zachycují grafy (Figure 3 a
+ 4).
+\end_layout
+
+\begin_layout Standard
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Graphics
+	filename grafy/ref0comctrl.eps
+	scale 60
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Porovnání hodnot PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ pro jednotlivá užitá řízení při referenčním signálu 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+.
+ Přitom křívky pro 1,2 a 9 splývají pod barvou 9; dále 3 splývá se 7 pod
+ barvou 7 a 4 splývá s 8 pod barvou 8; křivka pro 5 pak splývá s 6 pod barvou
+ 6.
+ 
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Graphics
+	filename grafy/reflowcomctrl1je2a34je78.eps
+	scale 60
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Porovnání hodnot PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ pro jednotlivá užitá řízení při referenčním signálu 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+.
+ Přitom křívky pro 1 a 2 splývají pod barvou 2; dále 3 splývá se 7 pod barvou
+ 7 a 4 splývá s 8 pod barvou 8; křivka pro 5 pak splývá s 6 pod barvou 6.
+ 
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Na dvou předchozích grafech (Figure 3 a 4) již může být pozorován jev zmiňovaný
+ v úvodních poznámkách k experimentům, tj.
+ o vlivu referenční hodnoty.
+ Konkrétně se jedná o řízení č.
+ 9.
+ Pro názornost bude uvažovaný jev ještě zobrazen na grafech (Figure 5) srovnávaj
+ící řízení č.
+ 2 (PI) a č.
+ 9 (PI + bikriteriální 
+\begin_inset Formula $\mathrm{sign}\omega$
+\end_inset
+
+).
+ 
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Tabular
+<lyxtabular version="3" rows="2" columns="2">
+<features tabularvalignment="middle">
+<column alignment="center" valignment="top" width="0">
+<column alignment="center" valignment="top" width="0">
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/comp29ome_2.eps
+	scale 40
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/comp29ome_9.eps
+	scale 40
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+a) Řízení č.
+ 2 (PI)
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+b) řízení č.
+ 9 (PI + bikriteriální 
+\begin_inset Formula $\mathrm{sign}\omega$
+\end_inset
+
+)
+\end_layout
+
+\end_inset
+</cell>
+</row>
+</lyxtabular>
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Srovnání PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ pro řízení č.
+ 2 a č.
+ 9 při referenčních signálech 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+ a 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+ 
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+
+\end_layout
+
+\end_inset
+
+ Tento jev lze vysvětlit tak, že řízení č.
+ 9 (PI + bikriteriální 
+\begin_inset Formula $\mathrm{sign}\omega$
+\end_inset
+
+) při nulovém referenčním signálu, a tedy stále nepozorovatelnému stavu
+ 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+, nezvládne nijak pozitivně ovlivnit PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+.
+ V případě, že ale stav 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ nějaký čas pozoruje, a tedy o něm dostane nějakou informaci, zvládne pak
+ pozitině ovlivnit, tj.
+ snížit, PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+.
+ Na grafu (Figure 5b) lze i pozorovat jakousi mez, kdy je 
+\begin_inset Quotes eld
+\end_inset
+
+získána
+\begin_inset Quotes erd
+\end_inset
+
+ schopnost omezovat PCRB a v tu chvíli hodnota rychle klesá a dále již nepřerůst
+á jistou mez.
+\end_layout
+
+\begin_layout Subsubsection*
+Porovnání PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ pro jednotlivá řízení a užité modely
+\end_layout
+
+\begin_layout Itemize
+Testovaná řízení: 1 -- 10
+\end_layout
+
+\begin_layout Itemize
+Použitý model: plný i redukovaný alpha-beta se stejnými (Ls) i různými (Ldq)
+ indukčnostimi
+\end_layout
+
+\begin_layout Itemize
+Injektovaný signál:
+\end_layout
+
+\begin_deeper
+\begin_layout Itemize
+amplituda: 
+\begin_inset Formula $amp=10.0$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+frekvence: 
+\begin_inset Formula $\omega_{inj}=1000$
+\end_inset
+
+
+\end_layout
+
+\end_deeper
+\begin_layout Itemize
+Kovarianční matice systému: typ 
+\emph on
+ekf
+\emph default
+
+\begin_inset Formula 
+\begin{eqnarray*}
+Q & = & \mathrm{diag}\left[\begin{array}{cccc}
+0.1 & 0.1 & 0.1 & 0.001\end{array}\right]\\
+R & = & \mathrm{diag}\left[\begin{array}{cc}
+0.05 & 0.05\end{array}\right]
+\end{eqnarray*}
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Itemize
+Časový horizont: standartní 120000 vzorků, tj.
+ 15s
+\end_layout
+
+\begin_layout Itemize
+Referenční signál: 
+\end_layout
+
+\begin_deeper
+\begin_layout Itemize
+nulový 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+ 
+\end_layout
+
+\begin_layout Itemize
+profil 
+\begin_inset Formula $\overline{\omega}_{profile}=\left[\begin{array}{cccccccccccccccc}
+0 & -1 & 3 & 6 & 9 & 6 & 3 & 0 & 0 & 0 & 0 & 0 & 0 & -3 & -6 & -3\end{array}\right]$
+\end_inset
+
+
+\end_layout
+
+\end_deeper
+\begin_layout Itemize
+Počáteční kovariance: 
+\begin_inset Formula $1\mathrm{Eye}$
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+Výsledné hodnoty PCRB polohy 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ v závislosti na užitém referenčním signálu a použitém řízení zachycují
+ grafy (Figure 6 a 7).
+ Některé výsledky jsou prakticky totožné, proto z podobných řízení bude
+ zobrazen pouze vybraný zástupce.
+ 
+\end_layout
+
+\begin_layout Standard
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Tabular
+<lyxtabular version="3" rows="8" columns="2">
+<features tabularvalignment="middle">
+<column alignment="center" valignment="top" width="0">
+<column alignment="center" valignment="top" width="0">
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl2o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+a) Řízení č.
+ 1 a 2, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+b) Řízení č.
+ 1 a 2, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl7.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl7o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+c) Řízení č.
+ 3 a 7, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+d) Řízení č.
+ 3 a 7, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl8.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl8o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+e) Řízení č.
+ 4 a 8, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+f) Řízení č.
+ 4 a 8, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl5.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl5o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+g) Řízení č.
+ 5, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+h) Řízení č.
+ 5, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+</lyxtabular>
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Hodnoty PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ dle volby řízení, referenčního signálu a použitého modelu.
+ Ve všech zde uvedených grafech (a-h) splývají křivky pro modely 3 a 4,
+ v případech a) a b) splývají i 1 a 2, a pro g) a h) pak splývá téměř vše
+ (1-4).
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Standard
+\begin_inset Float figure
+wide false
+sideways false
+status open
+
+\begin_layout Plain Layout
+\align center
+\begin_inset Tabular
+<lyxtabular version="3" rows="6" columns="2">
+<features tabularvalignment="middle">
+<column alignment="center" valignment="top" width="0">
+<column alignment="center" valignment="top" width="0">
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl6.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl6o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+i) Řízení č.
+ 6, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+j) Řízení č.
+ 6, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl9.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl9o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+k) Řízení č.
+ 9, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+l) Řízení č.
+ 9, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl10.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+\begin_inset Graphics
+	filename grafy/ctrl10o2.eps
+	scale 30
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+<row>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+m) Řízení č.
+ 10, ref.
+ 
+\begin_inset Formula $\overline{\omega}\equiv0$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+<cell alignment="center" valignment="top" topline="true" bottomline="true" leftline="true" rightline="true" usebox="none">
+\begin_inset Text
+
+\begin_layout Plain Layout
+n) Řízení č.
+ 10, ref.
+ 
+\begin_inset Formula $\overline{\omega}_{profile}$
+\end_inset
+
+
+\end_layout
+
+\end_inset
+</cell>
+</row>
+</lyxtabular>
+
+\end_inset
+
+
+\end_layout
+
+\begin_layout Plain Layout
+\begin_inset Caption
+
+\begin_layout Plain Layout
+Hodnoty PCRB 
+\begin_inset Formula $\vartheta$
+\end_inset
+
+ dle volby řízení, referenčního signálu a použitého modelu.
+ V grafech k) až n) splývají křivky pro modely 3 a 4, pro k) a l) pak splývá
+ dále 1 a 2, v i) a j) pak téměř splývají 2 a 4.
+ 
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\end_inset
+
+
+\end_layout
+
+\end_body
+\end_document
Index: /applications/dual/vahala/pcrb/pcrb_e.m
===================================================================
--- /applications/dual/vahala/pcrb/pcrb_e.m (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb_e.m (revision 1416)
@@ -0,0 +1,327 @@
+%simulator PMSM pro PCRB s lepsi ocekavanou hodnotou E
+% zatim zaver:
+%   
+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;
+
+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];
+
+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
+
+iJn = zeros(4,4,T);
+iJn2 = zeros(4,4,T);
+iJn3 = zeros(4,4,T);
+iJn4 = zeros(4,4,T);
+iJn5 = zeros(4,4,T);
+Jj = inv(Q);
+Jj2 = inv(Q);
+Jj3 = inv(Q);
+Jj4 = inv(Q);
+Jj5 = inv(Q);
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   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;
+
+%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);
+
+% figure;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   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);
+    
+    %d-q red, equal Ls ... DF-OK
+%     DH2(1,1) = cos(the);
+%     DH2(1,2) = -sin(the);
+%     DH2(1,4) = -id*ome*sin(the)-iq*ome*cos(the);
+%     DH2(2,1) = sin(the);
+%     DH2(2,2) = cos(the);
+%     DH2(2,4) = id*ome*cos(the)-iq*ome*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; !!!!!!!!!!!!!!!!!BAD
+    iQ = inv(Q);
+    q11 = iQ(1,1);
+    q12 = iQ(1,2); 
+    q13  = iQ(1,3);
+    q14  = iQ(1,4);
+    q21  = iQ(2,1);
+    q22  = iQ(2,2);
+    q23  = iQ(2,3);
+    q24  = iQ(2,4);
+    q31  = iQ(3,1);
+    q32  = iQ(3,2);
+    q33  = iQ(3,3);
+    q34  = iQ(3,4);
+    q41  = iQ(4,1);
+    q42  = iQ(4,2);
+    q43  = iQ(4,3);
+    q44 = iQ(4,4);
+    om = ome;
+    D11 = [[a*(a*iQ(1,1) - dt*om*iQ(2,1)) - dt*om*a*iQ(1,2) + dt^2*Q(3,3)*iQ(2,2), a*(a*iQ(1,2) - dt*om*iQ(2,2)) + e*(a*iQ(1,3) - dt*om*iQ(2,3)) + dt*om*a*iQ(1,1) - dt^2*Q(3,3)*iQ(2,1), d*(a*iQ(1,3) - dt*om*iQ(2,3)) - (a*iQ(1,2) - dt*om*iQ(2,2))*b - a*iQ(1,2)*dt*id - dt^2*Q(1,3)*iQ(2,2) + dt*(a*q14 - dt*om*q24) + dt*iq*a*q11 - dt^2*Q(2,3)*q21, a*q14 - dt*om*q24];...
+            [ a*(a*q21 + e*q31 + dt*om*q11) - dt*om*(a*q22 + e*q32) - dt^2*Q(3,3)*q12, a*(a*q22 + e*q32 + dt*om*q12) + e*(a*q23 + e*q33 + dt*om*q13) + dt*om*(a*q21 + e*q31) + dt^2*Q(3,3)*q11, d*(a*q23 + e*q33 + dt*om*q13) + dt*(a*q24 + e*q34 + dt*om*q14) - b*(a*q22 + e*q32 + dt*om*q12) - dt*id*(a*q22 + e*q32) - dt^2*Q(1,3)*q12 + dt*iq*(a*q21 + e*q31) + dt^2*Q(2,3)*q11,a*q24 + e*q34 + dt*om*q14];...
+            [ a*(d*q31 + dt*q41 - q21*(b + dt*id) + dt*iq*q11) - dt*om*(d*q32 + dt*q42 - q22*b) + dt*q22*(dt*Q(1,3)) - dt^2*Q(2,3)*q12, a*(d*q32 + dt*q42 - q22*(b + dt*id) + dt*iq*q12) + e*(d*q33 + dt*q43 - q23*(b + dt*id) + dt*iq*q13) + dt*om*(d*q31 + dt*q41 - q21*b) - q21*dt^2*Q(1,3) + dt^2*Q(2,3)*q11, d*(d*q33 + dt*q43 - q23*(b + dt*id) + dt*iq*q13) - (b + dt*id)*(d*q32 + dt*q42 - q22*b) + b*(q22*dt*id - dt*iq*q12) + dt*(q22*dt*Q(1,1) - dt*Q(1,2)*q12) + dt*(d*q34 + dt*q44 - q24*(b + dt*id) + dt*iq*q14) + dt*iq*(d*q31 + dt*q41 - q21*b) + dt*(- q21*dt*Q(1,2) + dt*Q(2,2)*q11), d*q34 + dt*q44 - q24*(b + dt*id) + dt*iq*q14];...
+            [ a*q41 - dt*om*q42,  a*q42 + e*q43 + dt*om*q41,  d*q43 + dt*q44 - q42*(b + dt*id) + dt*iq*q41,   q44]];
+ 
+    D12 = -DF3'*iQ;
+    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;
+%     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);
+    
+    
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%        
+               
+        
+        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);
+hold on;
+plot(timex, squeeze(iJn3(4,4,xax)),'r');%,timex, squeeze(iJn5(4,4,xax)),timex, squeeze(iJn4(4,4,xax)));
+
+% figure;
+% plot(timex, x_sys(3, xax)-ref_ome(xax));
Index: /applications/dual/vahala/pcrb/fpcrb.m
===================================================================
--- /applications/dual/vahala/pcrb/fpcrb.m (revision 1416)
+++ /applications/dual/vahala/pcrb/fpcrb.m (revision 1416)
@@ -0,0 +1,717 @@
+function [var_ome, var_th, var_th2] = fpcrb(model, control, amp, injom, Q, R, T, ref_profile)
+    %function for computing PCRB of PMSM model
+       
+
+    %   machine parameters
+    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;
+
+    Lq = 1.0*Ls;
+    Ld = 0.9*Ls;
+
+    %aliases
+    kpp = kp*pp*pp;
+    kppj = kpp/J;
+    psi = psipm;
+
+    a = 0.9898;
+    b = 0.0072;
+    c = 0.0361;
+    d = 1.0;
+    e = 0.0149;    
+
+    %reference signal
+    ref_ome = zeros(1, T);  
+    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
+
+
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %   system variables
+
+    %system state and control
+    x_sys = zeros(4, T);     
+    u_dq = zeros(2, T);
+
+    %covariance alternatives
+    Q4 = Q;
+    Q5 = zeros(5);
+    Q5(1:4,1:4) = Q;
+    Q5(5,5) = Q(4,4);
+    q1 = Q4(1,1);
+    q2 = Q4(2,2);
+    q3 = Q4(3,3);
+    q4 = Q4(4,4);
+    q5 = Q5(5,5);
+    iQ4 = inv(Q4);
+    iQ5 = inv(Q5);
+    iR = inv(R);
+    e05q4 = exp(-1/2*q4);
+    e2q4 = exp(-2*q4);
+    e92q4 = exp(-9/2*q4);
+    e4q4 = exp(4*q4);
+    e8q4 = exp(-8*q4);
+    e6q4 = exp(6*q4);
+
+    %PCRB J matrices
+    if(model(1) == 1)
+        iJn1 = zeros(2,T);
+        Jj1 = inv(Q4);
+    end
+    if(model(2) == 1)
+        iJn2 = zeros(2,T);
+        Jj2 = inv(Q4);
+    end
+    if(model(3) == 1)
+        iJn3 = zeros(3,T);
+        Jj3 = inv(Q5);
+    end
+    if(model(4) == 1)
+        iJn4 = zeros(2,T);
+        Jj4 = inv(Q4);
+    end
+    if(model(5) == 1)
+        iJn5 = zeros(2,T);
+        Jj5 = inv(Q4);
+    end
+    if(model(6) == 1)
+        iJn6 = zeros(3,T);
+        Jj6 = inv(Q5);
+    end
+    if(model(7) == 1)
+        iJn7 = zeros(2,T);
+        Jj7 = inv(Q4);
+    end
+    if(model(8) == 1)
+        iJn8 = zeros(2,T);
+        Jj8 = inv(Q4);
+    end
+    if(model(9) == 1)
+        iJn9 = zeros(3,T);
+        Jj9 = inv(Q5);
+    end
+    if(model(10) == 1)
+        iJn10 = zeros(2,T);
+        Jj10 = inv(Q4);
+    end
+    if(model(11) == 1)
+        iJn11 = zeros(2,T);
+        Jj11 = inv(Q4);
+    end
+    if(model(12) == 1)
+        iJn12 = zeros(3,T);
+        Jj12 = inv(Q5);   
+    end
+    
+
+    %PI control
+    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;
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %   init
+
+    % measurement function derivatives
+    C4 = zeros(2,4);
+    C4(1,1) = 1.0;
+    C4(2,2) = 1.0;
+
+    C5 = zeros(2,5);
+    C5(1,1) = 1.0;
+    C5(2,2) = 1.0;
+
+    % system function derivatives
+        %alpha-beta, equal Ls
+    A14 = zeros(4);
+    A14(1,1) = a;
+    A14(2,2) = a;
+    A14(3,3) = d;
+    A14(4,3) = dt;
+    A14(4,4) = 1.0;
+    A4 = zeros(4);
+        %d-q, equal Ls
+    A25 = zeros(4);
+    A25(1,1) = a;
+    A25(2,2) = a;
+    A25(2,3) = -b;
+    A25(3,2) = e;
+    A25(3,3) = d;
+    A25(4,3) = dt;
+    A25(4,4) = 1.0;
+        %5sc, equal Ls
+    A36 = zeros(5);
+    A36(1,1) = a;
+    A36(2,2) = a;
+    A36(3,3) = d;
+    A36(4,4) = 1.0;
+    A36(5,5) = 1.0;
+        %alpha-beta, Ldq
+    A70 = zeros(4);
+    % A70(4,4) = 1.0;
+    % A70(5,5) = 1.0;
+        %d-q, Ldq
+    A81 = zeros(4);
+    A81(1,1) = 1.0 - Rs*dt/Ld;
+    A81(2,2) = 1.0 - Rs*dt/Lq;
+    A81(3,3) = 1-B*dt/J;
+    A81(4,3) = dt;
+    A81(4,4) = 1.0;
+        %5sc, Ldq
+    A92 = zeros(5);
+    A92(4,4) = 1.0;
+    A92(5,5) = 1.0;
+
+
+    %expectation corrections
+        %cor. 2 -> 5
+    D11_cor5 = diag([dt^2*q3/q2, dt^2*q3/q1, dt^2*q1/q2 + dt^2*q2/q1, 0.0]);
+        %cor. 3 -> 6
+    D11_cor6 = 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]);
+        %cor. 8 -> 11
+    D11_cor11 = 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]);
+
+    % new figure              
+    figure;
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %   main loop
+
+    for t = 1:T-1,
+        %aliases
+        ial = x_sys(1, t);
+        ibe = x_sys(2, t);
+        ome = x_sys(3, t);
+        the = x_sys(4, t); 
+
+        ia = ial;
+        ib = ibe;
+
+        st = sin(the);
+        ct = cos(the);
+
+        id = ial*cos(the) + ibe*sin(the);
+        iq = ibe*cos(the) - ial*sin(the);
+
+        hia	= ial;
+        hib	= ibe;
+        hom	= ome;
+        sht	= sin(the);
+        cht	= cos(the);
+        sh2t = sin(2*the);
+        ch2t = cos(2*the);
+        sh3t = sin(3*the);
+        ch3t = cos(3*the);
+        ch4t = cos(4*the);    
+
+        %PCRB models
+        if(model(1) == 1) %al-be/nE/Ls
+            A14(1,3) = b*sin(the);
+            A14(1,4) = b*ome*cos(the);
+            A14(2,3) = -b*cos(the);
+            A14(2,4) = b*ome*sin(the);
+            A14(3,1) = -e*sin(the);
+            A14(3,2) = e*cos(the);
+            A14(3,4) = -e*(ial*cos(the)+ibe*sin(the));
+
+            D11 = A14'*iQ4*A14;
+            D12 = -A14'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj1 = D22 - D21/(Jj1 + D11)*D12;
+            tmp = inv(Jj1);        
+            iJn1(1,t) = tmp(3,3);
+            iJn1(2,t) = tmp(4,4);
+        end
+        if(model(2) == 1) %d-q/nE/Ls
+            A25(1,2) = dt*ome;
+            A25(1,3) = dt*iq;
+            A25(2,1) = -dt*ome;
+            A25(2,3) = -dt*id-b;
+
+            D11 = A25'*iQ4*A25;
+            D12 = -A25'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;    
+
+            Jj2 = D22 - D21/(Jj2 + D11)*D12;           
+            tmp = inv(Jj2);        
+            iJn2(1,t) = tmp(3,3);
+            iJn2(2,t) = tmp(4,4);
+        end
+        if(model(3) == 1) %5sc/nE/Ls
+            A36(1,3) = b*sin(the);
+            A36(1,4) = b*ome;
+            A36(2,3) = -b*cos(the);
+            A36(2,5) = -b*ome;
+            A36(3,1) = -e*sin(the);
+            A36(3,2) = e*cos(the);
+            A36(3,4) = -e*ial;
+            A36(3,5) = e*ibe;
+            A36(4,3) = dt*cos(the);
+            A36(4,5) = dt*ome;
+            A36(5,3) = -dt*sin(the);
+            A36(5,4) = -dt*ome;
+
+            D11 = A36'*iQ5*A36;    
+            D12 = -A36'*iQ5;
+            D21 = D12';
+            D22 = iQ5 + C5'*iR*C5;
+
+            Jj3 = D22 - D21/(Jj3 + D11)*D12;
+            tmp = inv(Jj3);        
+            iJn3(1,t) = tmp(3,3);
+            iJn3(2,t) = tmp(4,4);
+            iJn3(3,t) = tmp(5,5);
+        end
+        if(model(4) == 1) %al-be/E/Ls
+            D11 = zeros(4);
+            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));
+
+            A4(1,1) = a;
+            A4(1,2) = 0.0;
+            A4(1,3) = b*sin(the)*exp(-1/2*q4);
+            A4(1,4) = b*ome*cos(the)*exp(-1/2*q4);
+            A4(2,1) = 0.0;
+            A4(2,2) = a;
+            A4(2,3) = -b*cos(the)*exp(-1/2*q4);
+            A4(2,4) = b*ome*sin(the)*exp(-1/2*q4);
+            A4(3,1) = -e*sin(the)*exp(-1/2*q4);
+            A4(3,2) = e*cos(the)*exp(-1/2*q4);
+            A4(3,3) = d;
+            A4(3,4) = -e*(ial*cos(the)*exp(-1/2*q4) + ibe*sin(the)*exp(-1/2*q4));
+            A4(4,1) = 0.0;
+            A4(4,2) = 0.0;
+            A4(4,3) = dt;
+            A4(4,4) = 1.0;
+
+            D12 = -A4'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj4 = D22 - D21/(Jj4 + D11)*D12;
+            tmp = inv(Jj4);        
+            iJn4(1,t) = tmp(3,3);
+            iJn4(2,t) = tmp(4,4);
+        end
+        if(model(5) == 1) %d-q/E/Ls
+            A25(1,2) = dt*ome;
+            A25(1,3) = dt*iq;
+            A25(2,1) = -dt*ome;
+            A25(2,3) = -dt*id-b;
+
+            D11 = A25'*iQ4*A25 + D11_cor5;
+            D12 = -A25'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;    
+
+            Jj5 = D22 - D21/(Jj5 + D11)*D12;
+            tmp = inv(Jj5);        
+            iJn5(1,t) = tmp(3,3);
+            iJn5(2,t) = tmp(4,4);
+        end
+        if(model(6) == 1) %5sc/E/Ls
+            A36(1,3) = b*sin(the);
+            A36(1,4) = b*ome;
+            A36(2,3) = -b*cos(the);
+            A36(2,5) = -b*ome;
+            A36(3,1) = -e*sin(the);
+            A36(3,2) = e*cos(the);
+            A36(3,4) = -e*ial;
+            A36(3,5) = e*ibe;
+            A36(4,3) = dt*cos(the);
+            A36(4,5) = dt*ome;
+            A36(5,3) = -dt*sin(the);
+            A36(5,4) = -dt*ome;
+
+            D11 = A36'*iQ5*A36 + D11_cor6;    
+            D12 = -A36'*iQ5;
+            D21 = D12';
+            D22 = iQ5 + C5'*iR*C5;
+
+            Jj6 = D22 - D21/(Jj6 + D11)*D12;
+            tmp = inv(Jj6);        
+            iJn6(1,t) = tmp(3,3);
+            iJn6(2,t) = tmp(4,4);
+            iJn6(3,t) = tmp(5,5);
+        end
+        if(model(7) == 1) %al-be/nE/Ldq        
+            A70 = [[ (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 = A70'*iQ4*A70;
+            D12 = -A70'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj7 = D22 - D21/(Jj7 + D11)*D12;
+            tmp = inv(Jj7);        
+            iJn7(1,t) = tmp(3,3);
+            iJn7(2,t) = tmp(4,4);        
+        end
+        if(model(8) == 1) %d-q/nE/Ldq
+            A81(1,2) = Lq*dt/Ld*ome;
+            A81(1,3) = Lq*dt/Ld*iq;
+            A81(2,1) = -Ld*dt/Lq*ome;
+            A81(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
+            A81(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
+            A81(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);    
+
+            D11 = A81'*iQ4*A81;
+            D12 = -A81'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj8 = D22 - D21/(Jj8 + D11)*D12;
+            tmp = inv(Jj8);        
+            iJn8(1,t) = tmp(3,3);
+            iJn8(2,t) = tmp(4,4);
+        end
+        if(model(9) == 1) %5sc/nE/Ldq        
+            A92(1, 1) = 1.0 + dt*(-Rs*(st^2/Lq + ct^2/Ld) + ome*st*ct*(Ld/Lq - Lq/Ld));
+            A92(1, 2) = dt*(-ome + Rs*st*ct*(1/Lq - 1/Ld) + ome*(Ld*st^2/Lq + Lq*ct^2/Ld));
+            A92(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);
+            A92(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);
+            A92(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);
+
+            A92(2, 1) = dt*(ome + Rs*st*ct*(1/Lq - 1/Ld) - ome*(Lq*st^2/Ld + Ld*ct^2/Lq));
+            A92(2, 2) = 1 + dt*(-Rs*(st^2/Ld + ct^2/Lq) + ome*st*ct*(Lq/Ld - Ld/Lq));
+            A92(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);
+            A92(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));
+            A92(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));
+
+            A92(3, 1) = dt*kpp/J*((Ld - Lq)*(-2*ial*st*ct + ibe*ct^2 - ibe*st^2) - psi*st);
+            A92(3, 2) = dt*kpp/J*((Ld - Lq)*(ial*ct^2 - ial*st^2 + 2*ibe*st*ct) + psi*ct);
+            A92(3, 3) = 1;
+            A92(3, 4) = dt*kpp/J*((Ld - Lq)*(-ial^2*ct - ial*ibe*2*st + ibe^2*ct) - psi*ial);
+            A92(3, 5) = dt*kpp/J*((Ld - Lq)*(-ial^2*st + ial*ibe*2*ct + ibe^2*st) + psi*ibe);
+
+            A92(4,3) = dt*cos(the);
+            A92(4,5) = dt*ome;
+            A92(5,3) = -dt*sin(the);
+            A92(5,4) = -dt*ome;
+
+            D11 = A92'*iQ5*A92;
+            D12 = -A92'*iQ5;
+            D21 = D12';
+            D22 = iQ5 + C5'*iR*C5;
+
+            Jj9 = D22 - D21/(Jj9 + D11)*D12;
+            tmp = inv(Jj9);        
+            iJn9(1,t) = tmp(3,3);
+            iJn9(2,t) = tmp(4,4);
+            iJn9(3,t) = tmp(5,5);
+        end
+        if(model(10) == 1) %al-be/E/Ldq
+            A70 = [[ (Lq - Rs*dt*1/2*(1 - e2q4*ch2t))/Lq - (dt*hom*sht*e05q4*Lq^2*cht*e05q4 + Rs*dt*Lq*1/2*(1 + e2q4*ch2t))/(Ld*Lq) + (Ld*dt*hom*sht*cht*e2q4)/Lq,                                       (dt*(Ld - Lq)*(- Lq*hom*1/2*(1 + e2q4*ch2t) + Rs*sht*cht*e2q4 + Ld*hom*1/2*(1 - e2q4*ch2t)))/(Ld*Lq),    dt*cht*e05q4*(hia*sht*e05q4 - hib*cht*e05q4 + (Lq*(hib*cht*e05q4 - hia*sht*e05q4))/Ld) + dt*sht*e05q4*(psi/Lq - hia*cht*e05q4 - hib*sht*e05q4 + (Ld*(hia*cht*e05q4 + hib*sht*e05q4))/Lq),                                               (dt*(hom*psi*cht*e05q4 + Rs*hib*ch2t*e2q4 - Rs*hia*sh2t*e2q4))/Lq + (Ld*dt*(hia*hom*ch2t*e2q4 + hib*hom*sh2t*e2q4))/Lq - (dt*(Lq^2*hia*hom*ch2t*e2q4 + Lq^2*hib*hom*sh2t*e2q4 + Lq*Rs*hib*ch2t*e2q4 - Lq*Rs*hia*sh2t*e2q4))/(Ld*Lq)];...
+                    [                                       (dt*(Ld - Lq)*(- Ld*hom*1/2*(1 + e2q4*ch2t) + Rs*sht*cht*e2q4 + Lq*hom*1/2*(1 - e2q4*ch2t)))/(Ld*Lq), (Lq - Rs*dt*1/2*(1 + e2q4*ch2t))/Lq - (Lq*Rs*dt*1/2*(1 - e2q4*ch2t) - Lq^2*dt*hom*sht*cht*e2q4)/(Ld*Lq) - (Ld*dt*hom*sht*cht*e2q4)/Lq, (dt*(Lq*hia - psi*cht*e05q4))/Lq + (dt*((Lq^2*hia*ch2t*e2q4)/2 - (Lq^2*hia)/2 + (Lq^2*hib*sh2t*e2q4)/2))/(Ld*Lq) - (Ld*dt*(hia/2 + (hia*ch2t*e2q4)/2 + (hib*sh2t*e2q4)/2))/Lq, (dt*hom*psi*sht*e05q4 - Rs*dt*hia*(2*1/2*(1 - e2q4*ch2t) - 1) + Rs*dt*hib*sh2t*e2q4)/Lq + (Ld*(dt*hib*hom*(2*1/2*(1 - e2q4*ch2t) - 1) + dt*hia*hom*sh2t*e2q4))/Lq - (Lq*Rs*dt*hib*sh2t*e2q4 + Lq^2*dt*hib*hom*(2*1/2*(1 - e2q4*ch2t) - 1) + Lq^2*dt*hia*hom*sh2t*e2q4 - Lq*Rs*dt*hia*(2*1/2*(1 - e2q4*ch2t) - 1))/(Ld*Lq)];...
+                    [     -dt*kppj*(psi*sht*e05q4 - cht*e05q4*(Ld - Lq)*(hib*cht*e05q4 - hia*sht*e05q4) + sht*e05q4*(Ld - Lq)*(hia*cht*e05q4 + hib*sht*e05q4)),      dt*kppj*(psi*cht*e05q4 + cht*e05q4*(Ld - Lq)*(hia*cht*e05q4 + hib*sht*e05q4) + sht*e05q4*(Ld - Lq)*(hib*cht*e05q4 - hia*sht*e05q4)),                                                                                                                                                                         1.0,                                                                                                                                                   -dt*kppj*(psi*(hia*cht*e05q4 + hib*sht*e05q4) + (Ld - Lq)*(hia*cht*e05q4 + hib*sht*e05q4)^2 - (Ld - Lq)*(hib*cht*e05q4 - hia*sht*e05q4)^2)];...
+                    [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
+
+            D11 = zeros(4);
+            D11(1,1) = 1/q1 + (dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 + (dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q2 - (2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 + (4*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Rs^2*dt^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (2*Rs*dt*1/2*(1 + e2q4*ch2t))/(Ld*q1) + (dt^2*kppj^2*psi^2*1/2*(1 - e2q4*ch2t))/q3 - (2*Rs*dt*1/2*(1 - e2q4*ch2t))/(Lq*q1) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) + (2*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Ld*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (2*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*dt*hom*sht*cht*e2q4)/(Lq*q1) - (2*Lq*dt*hom*sht*cht*e2q4)/(Ld*q1) + (4*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (2*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (4*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (2*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (8*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3;
+            D11(1,2) = (dt*hom*1/2*(1 + e2q4*ch2t))/q2 - (dt*hom*1/2*(1 + e2q4*ch2t))/q1 - (dt*hom*1/2*(1 - e2q4*ch2t))/q1 + (dt*hom*1/2*(1 - e2q4*ch2t))/q2 - (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Lq*dt*hom*1/2*(1 + e2q4*ch2t))/(Ld*q1) - (Ld*dt*hom*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Ld*dt*hom*1/2*(1 - e2q4*ch2t))/(Lq*q1) - (Lq*dt*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (dt^2*kppj^2*psi^2*sht*cht*e2q4)/q3 - (Rs*dt*sht*cht*e2q4)/(Ld*q1) - (Rs*dt*sht*cht*e2q4)/(Ld*q2) + (Rs*dt*sht*cht*e2q4)/(Lq*q1) + (Rs*dt*sht*cht*e2q4)/(Lq*q2) + (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) - (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) - (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q2) + (Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (Lq^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (Ld^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (2*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (2*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (Ld^2*dt^2*hia*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*hia*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (Lq*Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (Ld*Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) - (3*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (3*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (3*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (3*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (Ld*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (Lq*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (4*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (4*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (4*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*Lq*dt^2*hia*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*hia*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (6*Ld^2*dt^2*hia*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (6*Lq^2*dt^2*hia*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (12*Ld*Lq*dt^2*hia*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3;
+            D11(1,3) = (3*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*q2) - (dt*hib*1/2*(1 - e2q4*ch2t))/q1 - (dt*hib*1/2*(1 + e2q4*ch2t))/q1 - (dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) + (3*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*q2) - (dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) - (dt*kppj*psi*sht*e05q4)/q3 + (dt*psi*sht*e05q4)/(Lq*q1) + (Lq*dt*hib*1/2*(1 + e2q4*ch2t))/(Ld*q1) + (Ld*dt*hib*1/2*(1 - e2q4*ch2t))/(Lq*q1) - (2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/q1 + (dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/q2 + (Rs*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) - (dt^2*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq*q2) - (dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Ld*dt*hib*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Lq*dt*hib*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Ld*dt*hib*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Lq*dt*hib*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Ld*dt^2*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q2) - (Ld*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) - (3*Lq*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*Ld*q2) + (Ld*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq*q2) - (Lq*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld*q2) - (3*Rs*dt^2*hia*sht*cht*e2q4)/(2*Ld*q2) + (3*Rs*dt^2*hia*sht*cht*e2q4)/(2*Lq*q2) + (Ld^2*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) + (Ld^2*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq^2*q2) + (Lq^2*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) + (Lq^2*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld^2*q2) - (2*Ld*dt*hia*kppj*sht*cht*e2q4)/q3 + (2*Lq*dt*hia*kppj*sht*cht*e2q4)/q3 + (Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Ld*q2) + (Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Ld*q2) - (Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) - (Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Lq*q2) + (Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Lq*q2) - (Rs*dt^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) - (dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) + (dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq*q2) + (Ld*dt*hia*sht*cht*e2q4)/(Lq*q1) - (Lq*dt*hia*sht*cht*e2q4)/(Ld*q1) + (Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (Lq*Rs*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (3*Ld*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*Lq*q2) - (Lq*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) - (Ld*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq*q2) + (Lq*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld*q2) - (Ld*Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (Lq*Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs*dt^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) + (Rs*dt^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q2) - (Lq^2*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld^2*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq^2*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (Ld^2*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (Ld*Rs*dt^2*hia*sht*cht*e2q4)/(2*Lq^2*q2) + (Lq*Rs*dt^2*hia*sht*cht*e2q4)/(2*Ld^2*q2) + (Lq^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (Lq^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(2*Ld^2*q2) - (Ld^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(2*Lq^2*q2) - (Ld*Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Lq*Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld*Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Lq^2*q2) - (Ld*Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Lq^2*q2) + (Lq*Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Ld^2*q2) - (Lq*Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Ld^2*q2) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (Ld*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1);
+            D11(1,4) = (dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/q2 - (dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/q2 - (2*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 + (Rs^2*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*q2) + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq*q2) - (Rs*dt*hib*1/2*(1 + e2q4*ch2t))/(Ld*q1) + (Rs*dt*hib*1/2*(1 + e2q4*ch2t))/(Lq*q1) + (dt^2*hib*kppj^2*psi^2*1/2*(1 - e2q4*ch2t))/q3 + (Rs*dt*hib*1/2*(1 - e2q4*ch2t))/(Ld*q1) - (Rs*dt*hib*1/2*(1 - e2q4*ch2t))/(Lq*q1) + (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 - (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 + (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (4*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 + (2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (dt*hom*psi*cht*e05q4)/(Lq*q1) - (Rs*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (Rs*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (2*Rs*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(Lq*q2) - (Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (2*Rs*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (4*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Rs*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (2*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (2*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Lq*dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (2*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Lq*q2) + (Lq*dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (2*Ld*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (2*Lq*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Rs^2*dt^2*hia*sht*cht*e2q4)/(Ld^2*q2) + (Rs^2*dt^2*hia*sht*cht*e2q4)/(Lq^2*q2) - (Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (2*Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (Ld*dt*hia*hom*1/2*(1 + e2q4*ch2t))/(Lq*q1) - (Lq*dt*hia*hom*1/2*(1 + e2q4*ch2t))/(Ld*q1) - (Rs^2*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) - (Ld*dt*hia*hom*1/2*(1 - e2q4*ch2t))/(Lq*q1) + (Lq*dt*hia*hom*1/2*(1 - e2q4*ch2t))/(Ld*q1) - (Rs^2*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) - (2*Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (dt^2*hia*kppj^2*psi^2*sht*cht*e2q4)/q3 + (Ld^2*dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) + (2*Rs*dt*hia*sht*cht*e2q4)/(Ld*q1) - (2*Rs*dt*hia*sht*cht*e2q4)/(Lq*q1) - (Lq^2*dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) + (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq*q2) + (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (10*Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (10*Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*dt*hib*hom*sht*cht*e2q4)/(Lq*q1) - (2*Lq*dt*hib*hom*sht*cht*e2q4)/(Ld*q1) - (2*Rs^2*dt^2*hia*sht*cht*e2q4)/(Ld*Lq*q2) + (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (2*Ld*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) - (2*Lq*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (2*Lq*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (3*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (3*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (6*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) + (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) - (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) + (Lq*Rs*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) + (Ld*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (2*Lq*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (3*Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (2*Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (3*Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) + (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) + (4*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (Rs*dt^2*hib*hom*sht*cht*e2q4)/(Ld*q2) + (Rs*dt^2*hib*hom*sht*cht*e2q4)/(Lq*q2) + (4*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (Lq^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (Lq^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (Ld^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) + (2*Lq^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (2*Ld^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (2*Ld*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (2*Lq*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*Lq*q1) + (6*Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (6*Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (4*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) + (3*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (6*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (3*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (4*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (20*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (3*Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (3*Lq*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (4*Lq*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (7*Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (7*Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (3*Ld*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (3*Lq*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (4*Ld*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (2*Lq*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) - (12*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (12*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (Ld*Rs*dt^2*hib*hom*sht*cht*e2q4)/(Lq^2*q2) - (Lq*Rs*dt^2*hib*hom*sht*cht*e2q4)/(Ld^2*q2);
+
+            D11(2,1) = D11(1,2);
+            D11(2,2) = 1/q2 + (dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 + (4*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 - (2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (Rs^2*dt^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (dt^2*kppj^2*psi^2*1/2*(1 + e2q4*ch2t))/q3 - (2*Rs*dt*1/2*(1 + e2q4*ch2t))/(Lq*q2) - (2*Rs*dt*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (2*Ld*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Ld*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (2*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) + (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld*dt*hom*sht*cht*e2q4)/(Lq*q2) + (2*Lq*dt*hom*sht*cht*e2q4)/(Ld*q2) - (2*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (4*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (4*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (4*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (8*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3;
+            D11(2,3) = (dt*hia)/q2 + (dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt*kppj*psi*cht*e05q4)/q3 - (dt*psi*cht*e05q4)/(Lq*q2) - (Ld*dt*hia)/(2*Lq*q2) - (Lq*dt*hia)/(2*Ld*q2) - (Ld*dt*hia*1/2*(1 + e2q4*ch2t))/(2*Lq*q2) + (Lq*dt*hia*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) + (Ld*dt*hia*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) - (Lq*dt*hia*1/2*(1 - e2q4*ch2t))/(2*Ld*q2) + (4*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/q1 - (2*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/q2 + (Rs*dt^2*hia*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) - (Rs*dt^2*hia*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld*q2) - (Rs*dt^2*hia*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Rs*dt^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q2) - (Rs*dt^2*hia*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (Rs*dt^2*hia*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) - (Rs*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq*q2) - (dt^2*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq*q1) - (dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Ld*dt*hia*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Lq*dt*hia*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Ld*dt*hia*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Lq*dt*hia*kppj*1/2*(1 - e2q4*ch2t))/q3 - (2*Ld*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Ld*dt^2*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) + (Lq^2*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (2*Ld*dt*hib*kppj*sht*cht*e2q4)/q3 - (2*Lq*dt*hib*kppj*sht*cht*e2q4)/q3 + (Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Rs*dt^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) - (dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q2) - (dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq*q1) - (Ld*dt*hib*sht*cht*e2q4)/(Lq*q2) + (Lq*dt*hib*sht*cht*e2q4)/(Ld*q2) - (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Ld*q2) - (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Lq*q2) + (Ld*Rs*dt^2*hia*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) + (Ld*Rs*dt^2*hia*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq^2*q2) + (Lq*Rs*dt^2*hia*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) + (Lq*Rs*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld^2*q2) - (2*Lq*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Ld*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Lq^2*q2) - (Lq*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Ld^2*q2) - (Rs*dt^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q1) + (Rs*dt^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) - (Lq^2*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld^2*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Ld*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (2*Lq*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (Lq^2*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Ld^2*q2) + (Lq^2*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Ld^2*q2) - (Ld^2*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Lq^2*q2) + (Ld^2*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Lq^2*q2) - (Ld*dt^2*hia*hom*sht*cht*e2q4)/(Lq*q2) + (Lq*dt^2*hia*hom*sht*cht*e2q4)/(Ld*q2) + (Lq^2*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (Ld*Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld*Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (Lq*Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Ld*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Ld*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Lq*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (Lq*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Lq^2*dt^2*hia*hom*sht*cht*e2q4)/(2*Ld^2*q2) + (Ld^2*dt^2*hia*hom*sht*cht*e2q4)/(2*Lq^2*q2) + (Ld*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2);
+            D11(2,4) = (dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 - (Rs*dt*hia)/(Ld*q2) + (Rs*dt*hia)/(Lq*q2) + (Rs^2*dt^2*hia*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (2*Rs^2*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (dt*hom*psi*sht*e05q4)/(Lq*q2) + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q1) - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq*q1) - (dt^2*hia*kppj^2*psi^2*1/2*(1 + e2q4*ch2t))/q3 + (2*Rs*dt*hia*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (2*Rs*dt*hia*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (4*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (Ld*dt*hib*hom)/(Lq*q2) + (Lq*dt*hib*hom)/(Ld*q2) - (Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 + (4*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 - (2*dt^2*hib*(hom^2 + q3)*sht*cht*e2q4)/q2 - (Rs^2*dt^2*hia*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) + (2*Rs*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*hib*hom*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (Rs*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Lq*dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld*dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Lq*dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (2*Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (2*Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Rs^2*dt^2*hia*1/2*(1 + e2q4*ch2t))/(Ld*Lq*q2) + (2*Ld*dt*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Lq*dt*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (Rs^2*dt^2*hia*1/2*(1 - e2q4*ch2t))/(Ld*Lq*q2) + (2*Rs^2*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q2) + (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (dt^2*hib*kppj^2*psi^2*sht*cht*e2q4)/q3 - (Lq^2*dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (2*Rs*dt*hib*sht*cht*e2q4)/(Ld*q2) + (2*Rs*dt*hib*sht*cht*e2q4)/(Lq*q2) - (Ld^2*dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq*q1) + (Lq^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Lq^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (2*Ld^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (10*Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (10*Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*dt*hia*hom*sht*cht*e2q4)/(Lq*q2) - (2*Lq*dt*hia*hom*sht*cht*e2q4)/(Ld*q2) + (4*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Ld*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Lq*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Lq*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (Lq^2*dt^2*hib*(hom^2 + q3)*sht*cht*e2q4)/(Ld^2*q2) + (Ld^2*dt^2*hib*(hom^2 + q3)*sht*cht*e2q4)/(Lq^2*q2) - (3*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (4*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (3*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) + (2*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) + (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) - (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (Lq*Rs*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*hib*hom*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) - (Ld*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (Lq*Rs*dt^2*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) + (2*Lq*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (2*Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (3*Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (2*Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (3*Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) - (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) - (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q2) + (Rs*dt^2*hia*hom*sht*cht*e2q4)/(Ld*q2) + (Rs*dt^2*hia*hom*sht*cht*e2q4)/(Lq*q2) + (Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (6*Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (6*Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Rs*dt^2*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*Lq*q2) - (2*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (4*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) - (Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (5*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (2*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (2*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (5*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) + (Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) - (4*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) + (20*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (3*Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (3*Lq*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (4*Lq*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (7*Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (7*Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (3*Ld*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (3*Lq*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (4*Ld*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Lq*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) - (12*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (12*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (Ld*Rs*dt^2*hia*hom*sht*cht*e2q4)/(Lq^2*q2) - (Lq*Rs*dt^2*hia*hom*sht*cht*e2q4)/(Ld^2*q2);
+
+            D11(3,1) = D11(1,3);
+            D11(3,2) = D11(2,3);
+            D11(3,3) = 1/q3 + dt^2/q4 + (3*dt^2*(hia^2 + q1))/(2*q2) + (dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 - (dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) + (dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 - (dt^2*(hia^2 + q1)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) + (Lq^2*dt^2*(hia^2 + q1))/(4*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1))/(4*Lq^2*q2) - (2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/q1 + (4*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/q1 + (dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/q2 - (2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/q2 + (dt^2*psi^2*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) - (Ld*dt^2*(hia^2 + q1))/(Lq*q2) - (Lq*dt^2*(hia^2 + q1))/(Ld*q2) + (dt^2*psi^2*1/2*(1 - e2q4*ch2t))/(Lq^2*q1) + (Lq^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Lq^2*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) + (Lq^2*dt^2*(hia^2 + q1)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(4*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(4*Lq^2*q2) + (dt^2*hia*psi*cht*e05q4)/(Ld*q2) - (2*dt^2*hia*psi*cht*e05q4)/(Lq*q2) + (Ld^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Lq^2*q2) + (Lq^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(4*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(4*Lq^2*q2) - (dt^2*hia*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q2) - (2*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq*q1) - (2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 - (2*Lq*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (Ld*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Lq*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (2*Ld*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Ld*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (Lq*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (2*Ld*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) - (2*Ld*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (2*Lq*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (Lq^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (Lq^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(2*Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(2*Lq^2*q2) + (Lq^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) + (2*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) + (dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (2*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q2) - (2*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq*q1) + (Ld*dt^2*hia*psi*cht*e05q4)/(Lq^2*q2) + (Ld*dt^2*hia*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q2) - (2*Lq^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (Lq^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q2) - (Ld^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (Ld^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (2*Ld*dt^2*hia*hib*sht*cht*e2q4)/(Lq*q2) + (2*Lq*dt^2*hia*hib*sht*cht*e2q4)/(Ld*q2) - (2*Ld*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Ld*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Lq*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Lq*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Lq^2*dt^2*hia*hib*sht*cht*e2q4)/(Ld^2*q2) + (Ld^2*dt^2*hia*hib*sht*cht*e2q4)/(Lq^2*q2) + (2*Ld*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) - (Ld*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) + (2*Ld*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2);
+            D11(3,4) = dt/q4 - (3*Rs*dt^2*(hia^2 + q1))/(2*Ld*q2) + (3*Rs*dt^2*(hia^2 + q1))/(2*Lq*q2) - (Rs*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (5*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Ld*q2) + (Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (2*Rs*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (7*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) + (Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (Ld*Rs*dt^2*(hia^2 + q1))/(2*Lq^2*q2) + (Lq*Rs*dt^2*(hia^2 + q1))/(2*Ld^2*q2) + (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 - (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 + (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (4*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (Ld*dt*(hia^2 + q1)*kppj*1/2*(1 + e2q4*ch2t))/q3 + (Ld*dt*(hib^2 + q2)*kppj*1/2*(1 + e2q4*ch2t))/q3 + (Lq*dt*(hia^2 + q1)*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Lq*dt*(hib^2 + q2)*kppj*1/2*(1 + e2q4*ch2t))/q3 + (Ld*dt*(hia^2 + q1)*kppj*1/2*(1 - e2q4*ch2t))/q3 - (Ld*dt*(hib^2 + q2)*kppj*1/2*(1 - e2q4*ch2t))/q3 - (Lq*dt*(hia^2 + q1)*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Lq*dt*(hib^2 + q2)*kppj*1/2*(1 - e2q4*ch2t))/q3 + (dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 - (dt^2*hia*hib*hom*1/2*(1 + e2q4*ch2t))/q2 + (dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/q2 - (2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 - (dt*hia*kppj*psi*cht*e05q4)/q3 - (dt*hib*kppj*psi*sht*e05q4)/q3 - (2*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/q2 + (2*Rs*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) - (Rs*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Lq*q2) - (Rs*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) + (dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q1) - (dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq*q1) - (Ld*dt^2*hia*hib*hom)/(Lq*q2) + (Lq*dt^2*hia*hib*hom)/(Ld*q2) + (dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*q1) - (dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(2*Ld*q2) + (2*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (2*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (2*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq*q2) + (2*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (Lq*Rs*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) - (Lq*Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Ld^2*q2) - (Ld*Rs*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (3*Ld*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Lq^2*q2) - (Lq*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) - (Ld*Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (Lq*Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) - (Lq^2*dt^2*hia*hib*hom)/(2*Ld^2*q2) + (Ld^2*dt^2*hia*hib*hom)/(2*Lq^2*q2) + (dt^2*hom*psi^2*sht*cht*e2q4)/(Lq^2*q1) - (dt^2*hom*psi^2*sht*cht*e2q4)/(Lq^2*q2) - (Rs*dt^2*hia*psi*cht*e05q4)/(Lq^2*q2) - (6*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/q1 + (6*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/q2 - (dt^2*hib*hom*psi*cht*e05q4)/(Ld*q2) - (dt^2*hia*hom*psi*sht*e05q4)/(2*Ld*q2) + (dt^2*hia*hom*psi*sht*e05q4)/(Lq*q2) - (2*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) - (2*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) + (5*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(2*Ld*q2) + (3*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq*q1) + (Ld*dt^2*hib*hom*psi*cht*e05q4)/(Lq^2*q2) - (Ld*dt^2*hia*hom*psi*sht*e05q4)/(2*Lq^2*q2) - (2*Ld*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Ld*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Lq*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Lq*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (Lq^2*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq^2*q2) + (Lq^2*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Lq^2*q2) + (Rs*dt^2*hia*psi*cht*e05q4)/(Ld*Lq*q2) - (Ld*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Lq*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Ld*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Lq*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Ld*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Lq*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (Ld*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) + (Ld*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(2*Lq^2*q2) - (2*Rs*dt^2*hia*hib*sht*cht*e2q4)/(Ld*q2) + (4*Rs*dt^2*hia*hib*sht*cht*e2q4)/(Lq*q2) - (2*Ld*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Lq*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (Lq*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (2*Ld*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Lq*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (2*Lq^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (2*Lq^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (2*Ld^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (Rs*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*Lq*q1) - (Lq^2*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Lq^2*dt^2*hia*hib*hom*1/2*(1 + e2q4*ch2t))/(2*Ld^2*q2) + (Ld^2*dt^2*hia*hib*hom*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) - (Ld^2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq^2*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) - (3*Ld^2*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(2*Lq^2*q2) + (Lq^2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (4*Ld*dt*hia*hib*kppj*sht*cht*e2q4)/q3 + (4*Lq*dt*hia*hib*kppj*sht*cht*e2q4)/q3 - (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (5*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (3*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (2*Ld*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq*q2) - (2*Lq*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld*q2) + (5*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) + (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) - (3*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) - (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) + (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (2*Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) + (3*Lq^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (3*Ld^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (3*Lq^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (3*Ld^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (3*Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (3*Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (3*Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (3*Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q2) + (2*Ld*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) + (3*Ld*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) - (5*Ld*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(2*Lq^2*q2) - (3*Ld*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) + (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q1) - (Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) - (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) + (2*Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q2) - (2*Ld*Rs*dt^2*hia*hib*sht*cht*e2q4)/(Lq^2*q2);
+
+            D11(4,1) = D11(1,4);
+            D11(4,2) = D11(2,4);
+            D11(4,3) = D11(3,4);
+            D11(4,4) = 1/q4 - (2*dt^2*(hib^2 + q2)*(hom^2 + q3))/q2 + (Rs^2*dt^2*(hia^2 + q1))/(Ld^2*q2) + (Rs^2*dt^2*(hia^2 + q1))/(Lq^2*q2) - (2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 - (2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (8*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/q2 - (8*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 + (4*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 - (8*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 - (8*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (4*Rs^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (4*Rs^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (dt^2*(hom^2 + q3)*psi^2*1/2*(1 + e2q4*ch2t))/(Lq^2*q1) + (Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (dt^2*(hom^2 + q3)*psi^2*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (dt^2*(hia^2 + q1)*kppj^2*psi^2*1/2*(1 + e2q4*ch2t))/q3 + (dt^2*(hib^2 + q2)*kppj^2*psi^2*1/2*(1 - e2q4*ch2t))/q3 - (2*Rs^2*dt^2*(hia^2 + q1))/(Ld*Lq*q2) + (Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Lq^2*q2) - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) + (8*Rs^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Ld*Lq*q2) - (8*Rs^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q2) + (Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (4*Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (4*Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (2*dt^2*hib*(hom^2 + q3)*psi*sht*e05q4)/(Ld*q2) - (2*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Rs*dt^2*hia*hib*hom)/(Ld*q2) + (2*Rs*dt^2*hia*hib*hom)/(Lq*q2) - (2*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (8*dt^2*hia*hib*(hom^2 + q3)*sht*cht*e2q4)/q2 - (2*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Ld*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q1) + (2*Ld*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (4*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (4*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (8*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 - (8*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (16*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) + (4*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*dt^2*hia*hib*kppj^2*psi^2*sht*cht*e2q4)/q3 + (2*Rs*dt^2*hia*hom*psi*sht*e05q4)/(Lq^2*q2) + (4*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) - (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (8*Rs^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) + (4*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (8*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) - (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (8*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (8*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) + (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (8*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (8*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (2*Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (2*Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (4*Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (2*Ld*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (2*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) - (4*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) - (4*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (2*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (2*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q1) + (20*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (20*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (8*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (8*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) - (2*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (8*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) + (8*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (2*Ld*dt^2*hib*(hom^2 + q3)*psi*sht*e05q4)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hia*hib*hom)/(Lq^2*q2) - (2*Lq*Rs*dt^2*hia*hib*hom)/(Ld^2*q2) - (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q2) + (4*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (8*Ld^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (8*Ld^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (8*Ld^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (8*Ld^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (8*Lq^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (8*Lq^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (8*Lq^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (8*Lq^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q1) + (4*Ld*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q2) + (4*Rs^2*dt^2*hia*hib*sht*cht*e2q4)/(Ld^2*q2) + (4*Rs^2*dt^2*hia*hib*sht*cht*e2q4)/(Lq^2*q2) - (2*Ld*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (2*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (4*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld*q2) + (4*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Ld*q2) - (4*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq*q2) + (4*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Lq*q2) - (2*Ld*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (2*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (4*Lq^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (4*Lq^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (4*Ld^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (4*Ld^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) + (8*Lq^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (8*Ld^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*Lq*q1) + (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*Lq*q2) + (4*Ld*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq^2*q2) + (4*Lq*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld^2*q2) - (4*Ld*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Lq^2*q2) - (4*Lq*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Ld^2*q2) - (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) - (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (4*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (40*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (8*Rs^2*dt^2*hia*hib*sht*cht*e2q4)/(Ld*Lq*q2) + (4*Ld*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (4*Ld*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) + (4*Lq*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) - (4*Lq*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (4*Ld*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (4*Ld*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (4*Lq*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (4*Lq*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (8*Ld*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (8*Lq*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (8*Ld*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (8*Lq*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (16*Ld*Lq*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (16*Ld*Lq*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (16*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (16*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (4*Lq^2*dt^2*hia*hib*(hom^2 + q3)*sht*cht*e2q4)/(Ld^2*q2) - (4*Ld^2*dt^2*hia*hib*(hom^2 + q3)*sht*cht*e2q4)/(Lq^2*q2) + (12*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (8*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (12*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (8*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (2*Ld*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (4*Ld*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) + (4*Ld*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) + (2*Ld*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (2*Ld*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (8*Ld*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (8*Lq*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (8*Ld*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (8*Lq*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (10*Ld*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (10*Ld*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (10*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (10*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (2*Rs*dt^2*hia*hom*psi*sht*e05q4)/(Ld*Lq*q2) - (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) + (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) + (16*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) + (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q1) - (4*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) - (12*Ld*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (12*Lq*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (8*Ld*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (8*Lq*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2); 
+
+
+            D12 = -A70'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj10 = D22 - D21/(Jj10 + D11)*D12;
+            tmp = inv(Jj10);        
+            iJn10(1,t) = tmp(3,3);
+            iJn10(2,t) = tmp(4,4);
+        end
+        if(model(11) == 1) %d-q/E/Ldq
+            A81(1,2) = Lq*dt/Ld*ome;
+            A81(1,3) = Lq*dt/Ld*iq;
+            A81(2,1) = -Ld*dt/Lq*ome;
+            A81(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
+            A81(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
+            A81(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);    
+
+            D11 = A81'*iQ4*A81 + D11_cor11;
+            D12 = -A81'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj11 = D22 - D21/(Jj11 + D11)*D12;
+            tmp = inv(Jj11);        
+            iJn11(1,t) = tmp(3,3);
+            iJn11(2,t) = tmp(4,4);
+        end
+        if(model(12) == 1) %5sc/E/Ldq
+            A92(1, 1) = 1.0 + dt*(-Rs*(1/2*(1 - e2q4*ch2t)/Lq + 1/2*(1 + e2q4*ch2t)/Ld) + hom*sht*cht*e2q4*(Ld/Lq - Lq/Ld));
+            A92(1, 2) = dt*(-hom + Rs*sht*cht*e2q4*(1/Lq - 1/Ld) + hom*(Ld*1/2*(1 - e2q4*ch2t)/Lq + Lq*1/2*(1 + e2q4*ch2t)/Ld));
+            A92(1, 3) = dt*psi*sht*e05q4/Lq + dt*hia*sht*cht*e2q4*(Ld/Lq - Lq/Ld) + dt*hib*(-1 + Ld/Lq*1/2*(1 - e2q4*ch2t) + Lq/Ld*1/2*(1 + e2q4*ch2t));
+            A92(1, 4) = dt*psi*hom/Lq + dt*hia*(-Rs*2*sht*e05q4/Lq + hom*cht*e05q4*(Ld/Lq - Lq/Ld)) + dt*hib*(Rs*cht*e05q4*(1/Lq - 1/Ld) + hom*2*Ld*sht*e05q4/Lq);
+            A92(1, 5) = dt*hia*(-Rs*2*cht*e05q4/Ld + hom*sht*e05q4*(Ld/Lq - Lq/Ld)) + dt*hib*(Rs*sht*e05q4*(1/Lq - 1/Ld) + hom*2*Lq*cht*e05q4/Ld);
+
+            A92(2, 1) = dt*(hom + Rs*sht*cht*e2q4*(1/Lq - 1/Ld) - hom*(Lq*1/2*(1 - e2q4*ch2t)/Ld + Ld*1/2*(1 + e2q4*ch2t)/Lq));
+            A92(2, 2) = 1 + dt*(-Rs*(1/2*(1 - e2q4*ch2t)/Ld + 1/2*(1 + e2q4*ch2t)/Lq) + hom*sht*cht*e2q4*(Lq/Ld - Ld/Lq));
+            A92(2, 3) = -dt*psi*cht*e05q4/Lq + dt*hia*(1 - Lq*1/2*(1 - e2q4*ch2t)/Ld - Ld*1/2*(1 + e2q4*ch2t)/Lq) + dt*hib*sht*cht*e2q4*(Lq/Ld - Ld/Lq);
+            A92(2, 4) = dt*hia*(Rs*cht*e05q4*(1/Lq - 1/Ld) - hom*2*Lq*sht*e05q4/Ld) + dt*hib*(-Rs*2*sht*e05q4/Ld + hom*cht*e05q4*(Lq/Ld - Ld/Lq));
+            A92(2, 5) = -dt*psi*hom/Lq + dt*hia*(Rs*sht*e05q4*(1/Lq - 1/Ld) -hom*2*Ld*cht*e05q4/Lq) + dt*hib*(-Rs*2*cht*e05q4/Lq + hom*sht*e05q4*(Lq/Ld - Ld/Lq));
+
+            A92(3, 1) = dt*kpp/J*((Ld - Lq)*(-2*hia*sht*cht*e2q4 + hib*1/2*(1 + e2q4*ch2t) - hib*1/2*(1 - e2q4*ch2t)) - psi*sht*e05q4);
+            A92(3, 2) = dt*kpp/J*((Ld - Lq)*(hia*1/2*(1 + e2q4*ch2t) - hia*1/2*(1 - e2q4*ch2t) + 2*hib*sht*cht*e2q4) + psi*cht*e05q4);
+            A92(3, 3) = 1;
+            A92(3, 4) = dt*kpp/J*((Ld - Lq)*(-(hia^2 + q1)*cht*e05q4 - hia*hib*2*sht*e05q4 + (hib^2 + q2)*cht*e05q4) - psi*hia);
+            A92(3, 5) = dt*kpp/J*((Ld - Lq)*(-(hia^2 + q1)*sht*e05q4 + hia*hib*2*cht*e05q4 + (hib^2 + q2)*sht*e05q4) + psi*hib);
+
+            A92(4,3) = dt*cht*e05q4;
+            A92(4,5) = dt*hom;
+            A92(5,3) = -dt*sht*e05q4;
+            A92(5,4) = -dt*hom;
+
+            D11 = zeros(5);
+            D11(1,1) = 1/q1 + (dt^2*(hom^2 + q3))/q2 + (Rs^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2)/(Ld^2*q1) + (Rs^2*dt^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) - (2*Rs*(cht^2 + q5)*dt)/(Ld*q1) - (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*Rs*dt*(sht^2 + q4))/(Lq*q1) + (dt^2*kpp^2*psi^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*(cht^2 + q5)*dt^2*(hom^2 + q3))/(Lq*q2) - (2*Lq*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Ld*q2) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hom^2 + q3))/(Lq^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Lq^2*q2) + (Lq^2*dt^2*(hom^2 + q3)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*Rs*cht*dt^2*hom*sht)/(Ld*q2) + (2*Rs*cht*dt^2*hom*sht)/(Lq*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld*q1) - (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld*q2) - (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq*q1) + (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq*q2) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Ld^2*dt^2*(hib^2 + q2)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*dt^2*(hib^2 + q2)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*cht*dt*hom*sht)/(Lq*q1) - (2*Lq*cht*dt*hom*sht)/(Ld*q1) + (2*Ld*dt^2*hib*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Lq*dt^2*hib*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2)*kpp^2)/(J^2*q3) - (2*Ld*Lq*dt^2*(hib^2 + q2)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) - (2*Ld*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld^2*q1) - (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq^2*q2) + (2*Lq*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (4*Ld^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (4*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) + (4*Lq^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (4*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) - (8*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Ld*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (2*Ld*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) - (4*Lq*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (2*Lq*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) - (8*Ld*Lq*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (8*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3);
+            D11(1,2) = (dt*hom)/q2 - (dt*hom)/q1 - (cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q1 + ((cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/q1 + (cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q2 - ((cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/q2 + (Rs*(cht^2 + q5)*dt^2*hom)/(Ld*q1) - (Rs*(cht^2 + q5)*dt^2*hom)/(Lq*q2) - (Rs*dt^2*hom*(sht^2 + q4))/(Ld*q2) + (Rs*dt^2*hom*(sht^2 + q4))/(Lq*q1) - (Rs*cht*dt*sht)/(Ld*q1) - (Rs*cht*dt*sht)/(Ld*q2) + (Rs*cht*dt*sht)/(Lq*q1) + (Rs*cht*dt*sht)/(Lq*q2) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Lq^2*q2) + (Lq*(cht^2 + q5)*dt*hom)/(Ld*q1) - (Ld*(cht^2 + q5)*dt*hom)/(Lq*q2) + (Ld*dt*hom*(sht^2 + q4))/(Lq*q1) - (Lq*dt*hom*(sht^2 + q4))/(Ld*q2) - (cht*dt^2*kpp^2*psi^2*sht)/(J^2*q3) + (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Ld*Lq*q1) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Ld*Lq*q1) - (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Ld*Lq*q2) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Ld*Lq*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/(Lq^2*q2) - (Lq*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hom)/(Ld^2*q1) + (Ld*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hom)/(Lq^2*q2) - (Ld*Rs*dt^2*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + (Lq*Rs*dt^2*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (Ld*cht*dt^2*(hom^2 + q3)*sht)/(Lq*q1) + (Lq*cht*dt^2*(hom^2 + q3)*sht)/(Ld*q1) - (Ld*cht*dt^2*(hom^2 + q3)*sht)/(Lq*q2) + (Lq*cht*dt^2*(hom^2 + q3)*sht)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld*q1) + (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq*q1) + (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq*q2) + (2*Ld^2*cht*dt^2*(hia^2 + q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*kpp^2*sht)/(J^2*q3) - (2*Ld^2*cht*dt^2*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (2*Lq^2*cht*dt^2*(hia^2 + q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*kpp^2*sht)/(J^2*q3) - (2*Lq^2*cht*dt^2*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (Ld*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld*(cht^3 + 3*cht*q5)*dt^2*hib*kpp^2*psi)/(J^2*q3) - (Lq*(cht^3 + 3*cht*q5)*dt^2*hib*kpp^2*psi)/(J^2*q3) + (Ld*dt^2*hia*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) - (Lq*dt^2*hia*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hib*kpp^2)/(J^2*q3) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hib*kpp^2)/(J^2*q3) + (Ld^2*dt^2*hia*hib*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*dt^2*hia*hib*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) - (4*Ld*Lq*cht*dt^2*(hia^2 + q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*kpp^2*sht)/(J^2*q3) + (4*Ld*Lq*cht*dt^2*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (6*Ld^2*(cht^2 + q5)*dt^2*hia*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (6*Lq^2*(cht^2 + q5)*dt^2*hia*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hib*kpp^2)/(J^2*q3) - (2*Ld*Lq*dt^2*hia*hib*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) - (3*Ld*(cht^2 + q5)*dt^2*hia*kpp^2*psi*sht)/(J^2*q3) - (3*Ld*cht*dt^2*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (3*Lq*(cht^2 + q5)*dt^2*hia*kpp^2*psi*sht)/(J^2*q3) + (3*Lq*cht*dt^2*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (12*Ld*Lq*(cht^2 + q5)*dt^2*hia*hib*kpp^2*(sht^2 + q4))/(J^2*q3);
+            D11(1,3) = (dt^2*hia*hom)/q2 - (dt*hib)/q1 + (dt*psi*sht)/(Lq*q1) - (cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/q1 + ((cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/q1 + (cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/q2 - ((cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/q2 + (Rs*(cht^2 + q5)*dt^2*hib)/(Ld*q1) + (Rs*dt^2*hib*(sht^2 + q4))/(Lq*q1) - (Rs*dt^2*psi*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/q2 - (dt*kpp*psi*sht)/(J*q3) + (Lq*(cht^2 + q5)*dt*hib)/(Ld*q1) + (Ld*dt*hib*(sht^2 + q4))/(Lq*q1) - (cht*dt^2*hom*psi)/(Lq*q2) + (Ld*(cht^2 + q5)*dt*hib*kpp)/(J*q3) - (Lq*(cht^2 + q5)*dt*hib*kpp)/(J*q3) - (Ld*dt*hib*kpp*(sht^2 + q4))/(J*q3) + (Lq*dt*hib*kpp*(sht^2 + q4))/(J*q3) - (Rs*cht*dt^2*hia*sht)/(Ld*q2) + (Rs*cht*dt^2*hia*sht)/(Lq*q2) - (Lq*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hib)/(Ld^2*q1) - (Ld*Rs*dt^2*hib*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) - (2*Ld*(cht^2 + q5)*dt^2*hia*hom)/(Lq*q2) + (Ld*(cht^3 + 3*cht*q5)*dt^2*hom*psi)/(Lq^2*q2) + (Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld*q1) - (Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld*q2) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Lq*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Lq*q2) - (2*Lq*dt^2*hia*hom*(sht^2 + q4))/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*psi*sht)/(Lq^2*q2) - (cht*dt^2*hom*psi*(sht^2 + q4))/(Ld*q1) + (cht*dt^2*hom*psi*(sht^2 + q4))/(Ld*q2) + (Ld*cht*dt*hia*sht)/(Lq*q1) - (Lq*cht*dt*hia*sht)/(Ld*q1) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hom)/(Lq^2*q2) - (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Lq*q2) + (Lq^2*dt^2*hia*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) + (Ld*cht*dt^2*hom*psi*(sht^2 + q4))/(Lq^2*q1) - (2*Ld*cht*dt*hia*kpp*sht)/(J*q3) + (2*Lq*cht*dt*hia*kpp*sht)/(J*q3) - (Ld*Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Ld^2*q2) - (Rs*(cht^2 + q5)*dt^2*psi*sht)/(Ld*Lq*q1) + (Rs*(cht^2 + q5)*dt^2*psi*sht)/(Ld*Lq*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/(Lq^2*q2) - (Ld*cht*dt^2*hib*hom*sht)/(Lq*q1) + (Lq*cht*dt^2*hib*hom*sht)/(Ld*q1) - (Ld*cht*dt^2*hib*hom*sht)/(Lq*q2) + (Lq*cht*dt^2*hib*hom*sht)/(Ld*q2) + (Lq^2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q1) - (Ld*Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Ld^2*q1) - (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Lq^2*q2) + (Lq*Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld^2*q2);
+            D11(1,4) = (dt*hom*psi)/(Lq*q1) - ((cht^3 + 3*cht*q5)*dt^2*hib*(hom^2 + q3))/q2 - (Rs*cht*dt*hib)/(Ld*q1) + (Rs*cht*dt*hib)/(Lq*q1) - (2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q1 - (2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q2 + (cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*Rs*dt*hia*sht)/(Lq*q1) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hib)/(Ld^2*q1) + (2*Rs^2*dt^2*hia*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (dt^2*hia*kpp^2*psi^2*sht)/(J^2*q3) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hib)/(Ld*Lq*q1) - (Rs*cht*dt^2*hia*hom)/(Ld*q2) + (Rs*cht*dt^2*hia*hom)/(Lq*q2) - (2*Rs*dt^2*hib*hom*sht)/(Ld*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hib*(hom^2 + q3))/(Lq^2*q2) + (2*Lq^2*dt^2*hia*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (Ld*cht*dt^2*hib*(hom^2 + q3))/(Lq*q2) + (Lq*cht*dt^2*hib*(hom^2 + q3))/(Ld*q2) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq*q2) - (2*Lq*dt^2*hia*(hom^2 + q3)*sht)/(Ld*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) - (Rs*dt^2*hom*psi*(sht^2 + q4))/(Lq^2*q1) - (cht*dt^2*(hom^2 + q3)*psi*sht)/(Ld*q1) + (Ld*cht*dt*hia*hom)/(Lq*q1) - (Lq*cht*dt*hia*hom)/(Ld*q1) + (2*Ld*dt*hib*hom*sht)/(Lq*q1) + (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld^2*q2) + (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld^2*q2) - (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Lq^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Lq^2*q2) + (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q1) - (Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q1) - (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q2) + (Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q2) - (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq*q1) + (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq*q2) + (Ld*cht*dt^2*(hom^2 + q3)*psi*sht)/(Lq^2*q1) - (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) + (2*Ld^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) - (Ld^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) - (Lq^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (Rs*(cht^2 + q5)*dt^2*hom*psi)/(Ld*Lq*q1) - (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q1) + (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q2) - (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q2) + (Lq^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Lq^2*q1) + (2*Ld^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Ld^2*q1) - (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) + (2*Ld*Lq*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (3*Ld*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q1) + (Ld*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld^2*q1) + (3*Lq*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q2) - (Ld*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld^2*q2) + (5*Ld^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (5*Lq^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Lq^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (Ld*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) + (Lq*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) + (3*Ld*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) - (Ld*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) - (3*Lq*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) + (Lq*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) + (3*Ld*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (3*Lq*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) - (10*Ld*Lq*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (8*Ld*Lq*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3);
+            D11(1,5) = (dt^2*hib*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q2 - (dt^2*(hom^2 + q3)*psi)/(Lq*q2) - (2*Rs*cht*dt*hia)/(Ld*q1) - (2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q1 + (2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q2 - ((cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q2 - (Rs*dt*hib*sht)/(Ld*q1) + (Rs*dt*hib*sht)/(Lq*q1) + (2*Rs^2*(cht^3 + 3*cht*q5)*dt^2*hia)/(Ld^2*q1) - (Rs^2*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (dt^2*(hom^2 + q3)*psi*(sht^2 + q4))/(Ld*q2) - (dt^2*hib*kpp^2*psi^2*sht)/(J^2*q3) - (2*Rs*cht*dt^2*hib*hom)/(Lq*q2) + (Rs^2*dt^2*hib*(sht^3 + 3*sht*q4))/(Ld*Lq*q1) - (Rs*dt^2*hia*hom*sht)/(Ld*q2) + (Rs*dt^2*hia*hom*sht)/(Lq*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hom^2 + q3))/(Lq^2*q2) - (Lq^2*dt^2*hib*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (2*Ld*cht*dt^2*hia*(hom^2 + q3))/(Lq*q2) - (Ld*dt^2*hib*(hom^2 + q3)*sht)/(Lq*q2) + (Lq*dt^2*hib*(hom^2 + q3)*sht)/(Ld*q2) + (Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld*q1) - (Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld*q2) - (Ld^2*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (Lq^2*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq*cht*dt*hib*hom)/(Ld*q1) + (Ld*dt*hia*hom*sht)/(Lq*q1) - (Lq*dt*hia*hom*sht)/(Ld*q1) + (Ld*(cht^2 + q5)*dt^2*(hom^2 + q3)*psi)/(Lq^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld^2*q2) + (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Lq^2*q2) - (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Lq^2*q2) - (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld*q1) + (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld*q2) - (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q1) - (Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q1) + (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q2) + (Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (2*Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Ld^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (2*Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (Ld^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Lq^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q1) - (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q2) - (Rs*cht*dt^2*hom*psi*sht)/(Lq^2*q2) + (Ld*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) - (Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (Lq^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (2*Lq^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Lq^2*q2) + (Ld*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (2*Ld*dt^2*(hib^2 + q2)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (Lq*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (2*Lq*dt^2*(hib^2 + q2)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Lq^2*q2) - (Ld*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (4*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (3*Lq*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld^2*q1) + (Ld*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q1) - (3*Ld*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq^2*q2) - (Ld*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q2) + (Rs*cht*dt^2*hom*psi*sht)/(Ld*Lq*q2) - (4*Ld^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (5*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (5*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (10*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Ld*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3) + (4*Lq*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3);
+
+            D11(2,1) = D11(1,2);
+            D11(2,2) = 1/q2 + (dt^2*(hom^2 + q3))/q1 + (Rs^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2)/(Lq^2*q2) + (Rs^2*dt^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*Rs*(cht^2 + q5)*dt)/(Lq*q2) + (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q1 - (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*Rs*dt*(sht^2 + q4))/(Ld*q2) - (2*Lq*(cht^2 + q5)*dt^2*(hom^2 + q3))/(Ld*q1) - (2*Ld*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Lq*q1) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hom^2 + q3))/(Ld^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Lq^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + ((cht^2 + q5)*dt^2*kpp^2*psi^2)/(J^2*q3) + (2*Rs*cht*dt^2*hom*sht)/(Ld*q1) - (2*Rs*cht*dt^2*hom*sht)/(Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q1) + (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld*q1) - (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld*q2) - (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq*q1) + (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq*q2) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1)*kpp^2)/(J^2*q3) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1)*kpp^2)/(J^2*q3) + (Ld^2*dt^2*(hia^2 + q1)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*dt^2*(hia^2 + q1)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q2) - (2*Ld*cht*dt*hom*sht)/(Lq*q2) + (2*Lq*cht*dt*hom*sht)/(Ld*q2) + (2*Ld*(cht^3 + 3*cht*q5)*dt^2*hia*kpp^2*psi)/(J^2*q3) - (2*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*kpp^2*psi)/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1)*kpp^2)/(J^2*q3) - (2*Ld*Lq*dt^2*(hia^2 + q1)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (2*Ld*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq^2*q2) - (2*Lq*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (4*Ld^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (8*Ld*Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (4*Ld*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) + (2*Lq*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (4*Lq*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (8*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3);
+            D11(2,3) = (dt*hia)/q2 + (dt^2*hib*hom)/q1 - (cht*dt*psi)/(Lq*q2) - (cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/q1 + ((cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/q1 + (cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/q2 - ((cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/q2 - (Rs*(cht^2 + q5)*dt^2*hia)/(Lq*q2) + (Rs*(cht^3 + 3*cht*q5)*dt^2*psi)/(Lq^2*q2) - (Rs*dt^2*hia*(sht^2 + q4))/(Ld*q2) + (2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/q1 - (2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/q2 + (cht*dt*kpp*psi)/(J*q3) - (Ld*(cht^2 + q5)*dt*hia)/(Lq*q2) - (Lq*dt*hia*(sht^2 + q4))/(Ld*q2) - (dt^2*hom*psi*sht)/(Lq*q1) + (Ld*(cht^2 + q5)*dt*hia*kpp)/(J*q3) - (Lq*(cht^2 + q5)*dt*hia*kpp)/(J*q3) - (Ld*dt*hia*kpp*(sht^2 + q4))/(J*q3) + (Lq*dt*hia*kpp*(sht^2 + q4))/(J*q3) + (Rs*cht*dt^2*hib*sht)/(Ld*q1) - (Rs*cht*dt^2*hib*sht)/(Lq*q1) + (Ld*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia)/(Lq^2*q2) + (Lq*Rs*dt^2*hia*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*Lq*(cht^2 + q5)*dt^2*hib*hom)/(Ld*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Ld*q1) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Ld*q2) - (Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq*q1) + (Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq*q2) - (2*Ld*dt^2*hib*hom*(sht^2 + q4))/(Lq*q1) + (Rs*cht*dt^2*psi*(sht^2 + q4))/(Lq^2*q1) + (Ld*dt^2*hom*psi*(sht^3 + 3*sht*q4))/(Lq^2*q1) + ((cht^2 + q5)*dt^2*hom*psi*sht)/(Ld*q1) - ((cht^2 + q5)*dt^2*hom*psi*sht)/(Ld*q2) - (Ld*cht*dt*hib*sht)/(Lq*q2) + (Lq*cht*dt*hib*sht)/(Ld*q2) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hib*hom)/(Ld^2*q1) - (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Lq*q2) + (Ld^2*dt^2*hib*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + (Ld*(cht^2 + q5)*dt^2*hom*psi*sht)/(Lq^2*q2) + (2*Ld*cht*dt*hib*kpp*sht)/(J*q3) - (2*Lq*cht*dt*hib*kpp*sht)/(J*q3) + (Ld*Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Ld^2*q1) - (Rs*cht*dt^2*psi*(sht^2 + q4))/(Ld*Lq*q1) + (Rs*cht*dt^2*psi*(sht^2 + q4))/(Ld*Lq*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/(Lq^2*q2) - (Ld*cht*dt^2*hia*hom*sht)/(Lq*q1) + (Lq*cht*dt^2*hia*hom*sht)/(Ld*q1) - (Ld*cht*dt^2*hia*hom*sht)/(Lq*q2) + (Lq*cht*dt^2*hia*hom*sht)/(Ld*q2) + (Lq^2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q2) + (Ld*Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Ld^2*q1) + (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2);
+            D11(2,4) = ((cht^3 + 3*cht*q5)*dt^2*hia*(hom^2 + q3))/q1 - (dt^2*(hom^2 + q3)*psi)/(Lq*q1) - (Rs*cht*dt*hia)/(Ld*q2) + (Rs*cht*dt*hia)/(Lq*q2) - (cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q1 + (2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q2 - (2*Rs*dt*hib*sht)/(Ld*q2) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hia)/(Lq^2*q2) + (2*Rs^2*dt^2*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2) + ((cht^2 + q5)*dt^2*(hom^2 + q3)*psi)/(Ld*q1) - (cht*dt^2*hia*kpp^2*psi^2)/(J^2*q3) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hia)/(Ld*Lq*q2) + (Rs*cht*dt^2*hib*hom)/(Ld*q1) - (Rs*cht*dt^2*hib*hom)/(Lq*q1) + (2*Rs*dt^2*hia*hom*sht)/(Lq*q1) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hom^2 + q3))/(Ld^2*q1) + (2*Ld^2*dt^2*hib*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Ld*cht*dt^2*hia*(hom^2 + q3))/(Lq*q1) + (Lq*cht*dt^2*hia*(hom^2 + q3))/(Ld*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld*q1) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld*q2) - (2*Ld*dt^2*hib*(hom^2 + q3)*sht)/(Lq*q1) - (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2)/(J^2*q3) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2)/(J^2*q3) - (Ld*cht*dt*hib*hom)/(Lq*q2) + (Lq*cht*dt*hib*hom)/(Ld*q2) - (2*Lq*dt*hia*hom*sht)/(Ld*q2) + (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld^2*q2) - (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Lq^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Lq^2*q1) + (Ld*dt^2*(hom^2 + q3)*psi*(sht^2 + q4))/(Lq^2*q1) - (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld*q1) + (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q1) - (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q2) + (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (2*Ld^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2)/(J^2*q3) + (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q1) - (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q2) + (Rs*cht*dt^2*hom*psi*sht)/(Lq^2*q1) - (2*Ld*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*psi)/(J^2*q3) + (Ld*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (2*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*psi)/(J^2*q3) - (Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (Ld^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (2*Lq^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Lq^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Lq^2*q2) + (Ld*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (Lq*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld^2*q1) + (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (2*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (4*Ld*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Ld*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld^2*q1) + (3*Ld*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld^2*q2) - (3*Lq*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q2) - (Rs*cht*dt^2*hom*psi*sht)/(Ld*Lq*q1) - (5*Ld^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (5*Lq^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (10*Ld*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (8*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Ld*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3) + (4*Lq*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3);
+            D11(2,5) = ((cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q1 - (dt^2*hia*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q1 - (2*Rs*cht*dt*hib)/(Lq*q2) - (dt*hom*psi)/(Lq*q2) + (2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q1 - (2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q2 - (2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q2 - (Rs*dt*hia*sht)/(Ld*q2) + (Rs*dt*hia*sht)/(Lq*q2) + (2*Rs^2*(cht^3 + 3*cht*q5)*dt^2*hib)/(Lq^2*q2) + (Rs^2*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (cht*dt^2*hib*kpp^2*psi^2)/(J^2*q3) + (2*Rs*cht*dt^2*hia*hom)/(Ld*q1) - (Rs^2*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld*Lq*q2) + (Rs*dt^2*hib*hom*sht)/(Ld*q1) - (Rs*dt^2*hib*hom*sht)/(Lq*q1) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hib*(hom^2 + q3))/(Ld^2*q1) + (Ld^2*dt^2*hia*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*Lq*cht*dt^2*hib*(hom^2 + q3))/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hom*psi)/(Lq^2*q2) - (Ld*dt^2*hia*(hom^2 + q3)*sht)/(Lq*q1) + (Lq*dt^2*hia*(hom^2 + q3)*sht)/(Ld*q1) - (Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq*q1) + (Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq*q2) + (Ld^2*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Lq^2*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (cht*dt^2*(hom^2 + q3)*psi*sht)/(Ld*q2) - (2*Ld*cht*dt*hia*hom)/(Lq*q2) - (Ld*dt*hib*hom*sht)/(Lq*q2) + (Lq*dt*hib*hom*sht)/(Ld*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld^2*q1) + (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Lq^2*q1) - (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Lq^2*q2) - (Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q1) + (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q1) + (Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q2) - (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q2) - (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq*q1) + (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq*q2) + (Ld*cht*dt^2*(hom^2 + q3)*psi*sht)/(Lq^2*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) - (Ld^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) + (2*Ld^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (Lq^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) + (2*Lq^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (Ld^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (Lq^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Rs*dt^2*hom*psi*(sht^2 + q4))/(Ld*Lq*q2) - (2*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q1) + (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q2) + (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q2) - (Lq^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Ld^2*q1) + (2*Ld^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Lq^2*q2) + (Lq^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q2) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq^2*q2) + (Ld*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (2*Ld*Lq*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) - (4*Ld*Lq*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld*Lq*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Ld*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q1) - (3*Lq*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld^2*q1) - (Ld*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q2) + (3*Ld*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq^2*q2) - (4*Ld^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (5*Ld^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (5*Lq^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (3*Ld*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) - (3*Lq*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) - (Ld*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) + (3*Ld*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) + (Lq*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) - (3*Lq*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) - (Ld*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (Lq*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (10*Ld*Lq*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3);
+
+            D11(3,1) = D11(1,3);
+            D11(3,2) = D11(2,3);
+            D11(3,3) = 1/q3 + ((cht^2 + q5)*dt^2)/q4 + (dt^2*(hib^2 + q2))/q1 + (dt^2*(hia^2 + q1))/q2 + (dt^2*(sht^2 + q4))/q5 + ((cht^2 + q5)*dt^2*psi^2)/(Lq^2*q2) + (dt^2*psi^2*(sht^2 + q4))/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/q2 - (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/q2 - (2*Lq*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Ld*q1) - (2*Ld*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Lq*q2) - (2*Ld*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq*q1) - (2*Lq*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*q2) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2))/(Ld^2*q1) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1))/(Lq^2*q2) - (2*cht*dt^2*hia*psi)/(Lq*q2) + (Ld^2*dt^2*(hib^2 + q2)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*dt^2*hib*psi*sht)/(Lq*q1) - (2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/q1 + (2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/q1 + (2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/q2 - (2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/q2 + (2*Ld*(cht^3 + 3*cht*q5)*dt^2*hia*psi)/(Lq^2*q2) + (2*Ld*dt^2*hib*psi*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq^2*q2) - (2*cht*dt^2*hia*psi*(sht^2 + q4))/(Ld*q1) + (2*(cht^2 + q5)*dt^2*hib*psi*sht)/(Ld*q1) + (2*cht*dt^2*hia*psi*(sht^2 + q4))/(Ld*q2) - (2*(cht^2 + q5)*dt^2*hib*psi*sht)/(Ld*q2) - (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/(Ld^2*q1) + (2*Ld^2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*Lq^2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/(Lq^2*q2) - (2*Ld*cht*dt^2*hia*hib*sht)/(Lq*q1) + (2*Lq*cht*dt^2*hia*hib*sht)/(Ld*q1) - (2*Ld*cht*dt^2*hia*hib*sht)/(Lq*q2) + (2*Lq*cht*dt^2*hia*hib*sht)/(Ld*q2) + (2*Ld*cht*dt^2*hia*psi*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*(cht^2 + q5)*dt^2*hib*psi*sht)/(Lq^2*q2);
+            D11(3,4) = (cht*dt)/q4 + (dt^2*hom*sht)/q5 + (Rs*cht*dt^2*(hib^2 + q2))/(Ld*q1) - (Rs*cht*dt^2*(hia^2 + q1))/(Ld*q2) - (Rs*cht*dt^2*(hib^2 + q2))/(Lq*q1) + (Rs*cht*dt^2*(hia^2 + q1))/(Lq*q2) + (dt^2*hom*psi^2*sht)/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/q1 + (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/q1 + (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/q2 - (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/q2 + (Rs*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2))/(Ld*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1))/(Lq*q2) - (dt*hia*kpp*psi)/(J*q3) - (dt^2*hib*hom*psi)/(Lq*q1) + ((cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/q1 - ((cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/q2 - (Ld*cht*dt*(hia^2 + q1)*kpp)/(J*q3) + (Ld*cht*dt*(hib^2 + q2)*kpp)/(J*q3) + (Lq*cht*dt*(hia^2 + q1)*kpp)/(J*q3) - (Lq*cht*dt*(hib^2 + q2)*kpp)/(J*q3) - (2*Rs*dt^2*hia*hib*sht)/(Ld*q2) + (2*Rs*dt^2*hia*hib*sht)/(Lq*q1) + (2*Ld^2*dt^2*(hib^2 + q2)*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq^2*dt^2*(hia^2 + q1)*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (3*cht*dt^2*hia*hib*hom*(sht^2 + q4))/q1 + (3*cht*dt^2*hia*hib*hom*(sht^2 + q4))/q2 - (Rs*(cht^2 + q5)*dt^2*hia*psi)/(Lq^2*q2) - (2*Ld*dt^2*(hib^2 + q2)*hom*sht)/(Lq*q1) - (2*Lq*dt^2*(hia^2 + q1)*hom*sht)/(Ld*q2) - (2*Rs*dt^2*hia*psi*(sht^2 + q4))/(Lq^2*q1) + ((cht^2 + q5)*dt^2*hib*hom*psi)/(Ld*q1) - ((cht^2 + q5)*dt^2*hib*hom*psi)/(Ld*q2) - (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2))/(Ld^2*q1) - (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1))/(Lq^2*q2) + (2*Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*q1) - (Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*q2) - (Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq*q1) + (2*Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq*q2) + (3*Ld*dt^2*hib*hom*psi*(sht^2 + q4))/(Lq^2*q1) + (Rs*(cht^2 + q5)*dt^2*hia*psi)/(Ld*Lq*q2) - (2*Ld*dt*hia*hib*kpp*sht)/(J*q3) + (2*Lq*dt*hia*hib*kpp*sht)/(J*q3) - (2*Ld*Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q1) + (Ld*Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld^2*q2) - (2*Lq*Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/(Ld^2*q1) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/(Lq^2*q2) - (Ld*cht*dt^2*hia*hib*hom)/(Lq*q1) + (Lq*cht*dt^2*hia*hib*hom)/(Ld*q1) - (Ld*cht*dt^2*hia*hib*hom)/(Lq*q2) + (Lq*cht*dt^2*hia*hib*hom)/(Ld*q2) + (Rs*cht*dt^2*hib*psi*sht)/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/(Lq^2*q2) - (2*cht*dt^2*hia*hom*psi*sht)/(Ld*q1) + (2*cht*dt^2*hia*hom*psi*sht)/(Ld*q2) - (2*Ld*Rs*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld*(cht^2 + q5)*dt^2*hib*hom*psi)/(Lq^2*q2) - (3*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq*q1) + (3*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq*q2) + (2*Ld*cht*dt^2*hia*hom*psi*sht)/(Lq^2*q1) + (Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld^2*q2) - (Rs*cht*dt^2*hib*psi*sht)/(Ld*Lq*q1) + (2*Rs*cht*dt^2*hib*psi*sht)/(Ld*Lq*q2) + (3*Ld^2*cht*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q1) - (3*Lq^2*cht*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q2);
+            D11(3,5) = (cht*dt^2*hom)/q4 - (dt*sht)/q5 - (dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/q1 + (dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/q2 + (Rs*dt^2*(hib^2 + q2)*sht)/(Ld*q1) - (Rs*dt^2*(hia^2 + q1)*sht)/(Ld*q2) - (Rs*dt^2*(hib^2 + q2)*sht)/(Lq*q1) + (Rs*dt^2*(hia^2 + q1)*sht)/(Lq*q2) + (cht*dt^2*hom*psi^2)/(Lq^2*q2) - (2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/q1 + (2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/q1 + (2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/q2 - (2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/q2 + (dt*hib*kpp*psi)/(J*q3) - (Rs*dt^2*(hia^2 + q1)*(sht^3 + 3*sht*q4))/(Ld*q2) - (Rs*dt^2*(hib^2 + q2)*(sht^3 + 3*sht*q4))/(Lq*q1) - (dt^2*hia*hom*psi)/(Lq*q2) + (2*Rs*cht*dt^2*hia*hib)/(Ld*q1) - (2*Rs*cht*dt^2*hia*hib)/(Lq*q2) - (Ld*dt*(hia^2 + q1)*kpp*sht)/(J*q3) + (Ld*dt*(hib^2 + q2)*kpp*sht)/(J*q3) + (Lq*dt*(hia^2 + q1)*kpp*sht)/(J*q3) - (Lq*dt*(hib^2 + q2)*kpp*sht)/(J*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*hom)/(Ld^2*q1) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hom)/(Lq^2*q2) + (3*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/q1 - (3*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/q2 - (2*Lq*cht*dt^2*(hib^2 + q2)*hom)/(Ld*q1) - (2*Ld*cht*dt^2*(hia^2 + q1)*hom)/(Lq*q2) + (2*Rs*(cht^2 + q5)*dt^2*hib*psi)/(Lq^2*q2) + (Rs*dt^2*hib*psi*(sht^2 + q4))/(Lq^2*q1) - (dt^2*hia*hom*psi*(sht^2 + q4))/(Ld*q1) + (dt^2*hia*hom*psi*(sht^2 + q4))/(Ld*q2) + (Ld*Rs*dt^2*(hib^2 + q2)*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq*Rs*dt^2*(hia^2 + q1)*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Ld*q1) - (2*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Lq*q2) + (Ld*dt^2*hia*hom*psi*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*cht*dt*hia*hib*kpp)/(J*q3) - (2*Lq*cht*dt*hia*hib*kpp)/(J*q3) + (2*Lq*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Ld^2*q1) - (Lq*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Ld^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Lq^2*q2) + (2*Ld*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Lq^2*q2) - (Rs*dt^2*hib*psi*(sht^2 + q4))/(Ld*Lq*q1) + (Ld^2*dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (Ld*dt^2*hia*hib*hom*sht)/(Lq*q1) + (Lq*dt^2*hia*hib*hom*sht)/(Ld*q1) - (Ld*dt^2*hia*hib*hom*sht)/(Lq*q2) + (Lq*dt^2*hia*hib*hom*sht)/(Ld*q2) - (Rs*cht*dt^2*hia*psi*sht)/(Lq^2*q2) + (Lq^2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq^2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq^2*q2) + (2*cht*dt^2*hib*hom*psi*sht)/(Ld*q1) - (2*cht*dt^2*hib*hom*psi*sht)/(Ld*q2) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hib)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hib)/(Lq^2*q2) + (3*Ld*(cht^2 + q5)*dt^2*hia*hom*psi)/(Lq^2*q2) - (Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld*q1) + (3*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld*q2) - (3*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq*q1) + (Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq*q2) + (2*Ld*cht*dt^2*hib*hom*psi*sht)/(Lq^2*q2) + (Ld*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld^2*q1) - (Ld*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld^2*q2) - (2*Rs*cht*dt^2*hia*psi*sht)/(Ld*Lq*q1) + (Rs*cht*dt^2*hia*psi*sht)/(Ld*Lq*q2) - (3*Lq^2*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/(Ld^2*q1) + (3*Ld^2*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/(Lq^2*q2);
+
+            D11(4,1) = D11(1,4);
+            D11(4,2) = D11(2,4);
+            D11(4,3) = D11(3,4);
+            D11(4,4) = 1/q4 + (dt^2*(hom^2 + q3))/q5 + (dt^2*(hom^2 + q3)*psi^2)/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/q1 - (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/q2 + (dt^2*(hia^2 + q1)*kpp^2*psi^2)/(J^2*q3) + (Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Ld^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Ld^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Lq^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Lq^2*q2) + (4*Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q1) - (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q1 + (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q2 - (2*Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Ld*Lq*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2)/(J^2*q3) + (Ld^2*(cht^2 + q5)*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Lq^2*q2) - (2*cht*dt^2*hia*(hom^2 + q3)*psi)/(Ld*q1) + (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) + (4*Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld*q1) - (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld*q2) - (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq*q1) + (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq*q2) + (4*Ld*dt^2*hib*(hom^2 + q3)*psi*sht)/(Lq^2*q1) + (2*Ld*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi)/(J^2*q3) - (2*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi)/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) + (4*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^2 + q5)*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2)/(J^2*q3) - (2*Ld*Lq*(cht^2 + q5)*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2)/(J^2*q3) + (2*Rs*cht*dt^2*hib*hom*psi)/(Lq^2*q1) - (4*Rs*dt^2*hia*hom*psi*sht)/(Lq^2*q1) - (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld*q1) + (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq*q1) + (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq*q2) + (2*Ld*cht*dt^2*hia*(hom^2 + q3)*psi)/(Lq^2*q1) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld^2*q2) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Lq^2*q1) - (4*Ld^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (4*Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (4*Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) + (2*Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq^2*q1) + (2*Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld^2*q1) - (2*Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq^2*q2) - (2*Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld^2*q2) - (2*Rs*cht*dt^2*hib*hom*psi)/(Ld*Lq*q1) - (4*Ld*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq^2*q1) + (4*Ld*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq^2*q1) + (4*Lq*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld^2*q2) - (4*Lq*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld^2*q2) - (8*Ld*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q1) + (8*Lq*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q2) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) - (8*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q1) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q2) - (2*Ld*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (2*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (4*Ld^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Lq^2*q1) - (4*Lq^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Ld^2*q2) + (4*Ld*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) - (4*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (8*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3);
+            D11(4,5) = (dt*hom)/q4 - (dt*hom)/q5 + (2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/q1 - (2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/q2 - (2*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/q1 + (4*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/q1 + (4*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/q2 - (2*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/q2 - (2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/q2 - (dt^2*hia*hib*kpp^2*psi^2)/(J^2*q3) + (2*cht*dt^2*hib*(hom^2 + q3)*psi)/(Ld*q1) - (cht*dt^2*hib*(hom^2 + q3)*psi)/(Ld*q2) - (dt^2*hia*(hom^2 + q3)*psi*sht)/(Ld*q1) + (2*dt^2*hia*(hom^2 + q3)*psi*sht)/(Ld*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Ld^2*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Lq^2*q2) + (2*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Ld*q1) - (2*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Lq*q1) + (2*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Lq*q2) + (Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Ld^2*q2) + (Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Lq^2*q1) + (Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Lq^2*q2) + (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Ld^2*q2) - (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Lq^2*q1) + (2*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld*q1) - (2*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld*q2) - (2*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq*q1) + (2*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq*q2) + (Ld*dt^2*hia*(hom^2 + q3)*psi*sht)/(Lq^2*q1) + (2*Ld^2*(cht^2 + q5)*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2)/(J^2*q3) + (2*Lq^2*(cht^2 + q5)*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2)/(J^2*q3) - (2*Ld^2*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Ld^2*q1) - (2*Lq*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Ld^2*q1) - (2*Ld*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Lq^2*q2) + (2*Ld*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Lq^2*q2) - (2*Ld*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld^2*q2) - (2*Lq*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi)/(J^2*q3) - (Lq*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi)/(J^2*q3) + (Ld*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi*sht)/(J^2*q3) - (Lq*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi*sht)/(J^2*q3) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Ld*Lq*q1) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Ld*Lq*q2) + (4*Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Ld*Lq*q2) + (4*Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Ld*Lq*q2) - (Rs*cht*dt^2*hia*hom*psi)/(Lq^2*q2) + (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Ld*Lq*q2) + (Rs*dt^2*hib*hom*psi*sht)/(Lq^2*q1) - (2*Lq^2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/(Ld^2*q1) + (2*Ld^2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/(Lq^2*q2) + (Ld^2*cht*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*sht)/(J^2*q3) + (Ld^2*cht*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/(Lq^2*q1) + (Lq^2*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/(Ld^2*q2) + (Ld^2*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/(Lq^2*q2) + (2*Ld^2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (2*Lq^2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld*cht*dt^2*hib*(hom^2 + q3)*psi)/(Lq^2*q2) + (4*Ld*Lq*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld*q1) + (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld*q2) - (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq*q1) + (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq*q2) - (2*Rs*cht*dt^2*hia*hom*psi)/(Ld*Lq*q1) + (Rs*cht*dt^2*hia*hom*psi)/(Ld*Lq*q2) - (Rs*dt^2*hib*hom*psi*sht)/(Ld*Lq*q1) + (2*Rs*dt^2*hib*hom*psi*sht)/(Ld*Lq*q2) - (6*Ld^2*cht*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (6*Lq^2*cht*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*cht*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*cht*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*sht)/(J^2*q3) - (3*Ld*cht*dt^2*(hia^2 + q1)*hib*kpp^2*psi)/(J^2*q3) + (3*Lq*cht*dt^2*(hia^2 + q1)*hib*kpp^2*psi)/(J^2*q3) - (3*Ld*dt^2*hia*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) + (3*Lq*dt^2*hia*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) - (4*Ld*Lq*(cht^2 + q5)*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2)/(J^2*q3) + (2*Ld*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq^2*q1) + (2*Lq*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld^2*q1) - (2*Ld*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq^2*q2) - (2*Lq*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld^2*q2) + (12*Ld*Lq*cht*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*sht)/(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) = 1/q5 + (dt^2*(hom^2 + q3))/q4 + (dt^2*(hom^2 + q3)*psi^2)/(Lq^2*q2) - (2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/q1 - (2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/q2 + (dt^2*(hib^2 + q2)*kpp^2*psi^2)/(J^2*q3) + (4*Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Ld^2*q1) + (4*Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Lq^2*q2) + (Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q1) + (Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld^2*q2) + (Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq^2*q1) + (Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q2) + (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q1 - (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q2 - (2*Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*Lq*q2) + (4*Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Ld^2*q1) + (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/(Lq^2*q2) + (Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) + (Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q2) - (2*dt^2*hib*(hom^2 + q3)*psi*sht)/(Ld*q2) + (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld*q1) - (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld*q2) - (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq*q1) + (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq*q2) - (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld*q1) + (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld*q2) - (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq*q1) + (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq*q2) + (2*Ld*dt^2*hib*(hom^2 + q3)*psi*sht)/(Lq^2*q2) + (2*Ld*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi*sht)/(J^2*q3) - (2*Lq*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi*sht)/(J^2*q3) + (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) + (4*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) - (2*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Rs*cht*dt^2*hib*hom*psi)/(Lq^2*q2) - (2*Rs*dt^2*hia*hom*psi*sht)/(Lq^2*q2) + (4*Ld*cht*dt^2*hia*(hom^2 + q3)*psi)/(Lq^2*q2) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld^2*q1) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Lq^2*q2) + (4*Ld^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (4*Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) + (4*Lq^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) - (8*Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld^2*q1) + (8*Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq^2*q2) + (4*Lq*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld^2*q1) - (4*Lq*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld^2*q1) - (4*Ld*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq^2*q2) + (4*Ld*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq^2*q2) + (2*Ld*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q1) - (2*Ld*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q2) - (2*Lq*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q2) - (8*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) + (2*Rs*dt^2*hia*hom*psi*sht)/(Ld*Lq*q2) + (4*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q1) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q2) + (4*Ld*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) - (4*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Ld^2*q1) + (4*Ld^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Lq^2*q2) - (2*Ld*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) + (2*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) - (8*Ld*Lq*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3);
+
+            D12 = -A92'*iQ5;
+            D21 = D12';
+            D22 = iQ5 + C5'*iR*C5;
+
+            Jj12 = D22 - D21/(Jj12 + D11)*D12;
+            tmp = inv(Jj12);        
+            iJn12(1,t) = tmp(3,3);
+            iJn12(2,t) = tmp(4,4);
+            iJn12(3,t) = tmp(5,5);
+        end
+
+        %control & simulation
+        if(control == 1) %ome = ref_ome, the = integral(ome), ial=ibe=0
+            x_sys(1, t+1) = 0;
+            x_sys(2, t+1) = 0;
+            x_sys(3, t+1) = ref_ome(t);
+            x_sys(4, t+1) = the + dt*ome;
+        else
+            if(control == 6) %rnd. err. ref_ome
+               ref_ome(t) = ref_ome(t) + amp*randn(); 
+            end
+
+            %PI (only PI for control == 2)        
+            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;        
+
+            if(control == 3) %inj. sin -> dq
+                u_dq(1,t) = u_dq(1,t) + amp*sin(injom*dt*t);
+                u_dq(2,t) = u_dq(2,t) + amp*cos(injom*dt*t);
+            elseif(control == 4) %inj. rect. -> dq
+                u_dq(1,t) = u_dq(1,t) + amp*sign(sin(injom*dt*t));
+                u_dq(2,t) = u_dq(2,t) + amp*sign(cos(injom*dt*t));
+            elseif(control == 5) %inj. const. -> d
+    %             u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
+                u_dq(1,t) = amp;            
+            elseif(control == 9) %bic. sign. ome           
+                u_dq(1,t) = u_dq(1,t) - amp*sign(ome);
+                u_dq(2,t) = u_dq(2,t) + amp*sign(ome);
+            end
+
+            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);
+
+            if(control == 7) %inj. sin -> al-be
+                ual = ual + amp*cos(injom*dt*t);
+                ube = ube + amp*sin(injom*dt*t);
+            elseif(control == 8) %inj. rect. -> al-be
+                ual = ual + amp*sign(cos(injom*dt*t));
+                ube = ube + amp*sign(sin(injom*dt*t));        
+            elseif((control == 10)&&(rand() > (1/5))) %bic rnd. 5
+                duab = sign(rand(2,1)-0.5)*amp;
+                ual = ual + duab(1);
+                ube = ube + duab(2);
+            end
+
+            ud = ual*cos(the) + ube*sin(the);
+            uq = ube*cos(the) - ual*sin(the);
+
+            %simulation model Ldq
+            idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*ud;
+            iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*uq;
+
+            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) = (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
+
+    end
+
+    ssz = sum(model);
+    var_ome = zeros(T,ssz);
+    var_th = zeros(T,ssz);
+    var_th2 = zeros(T,ssz);
+    ind = 1;
+    if(model(1) == 1)
+        var_ome(:,ind) = iJn1(1,:);
+        var_th(:,ind) = iJn1(2,:);   
+        ind = ind+1;
+    end
+    if(model(2) == 1)
+        var_ome(:,ind) = iJn2(1,:);
+        var_th(:,ind) = iJn2(2,:);
+        ind = ind+1;
+    end
+    if(model(3) == 1)
+        var_ome(:,ind) = iJn3(1,:);
+        var_th(:,ind) = iJn3(2,:);   
+        var_th2(:,ind) = iJn3(3,:); 
+        ind = ind+1;
+    end
+    if(model(4) == 1)
+        var_ome(:,ind) = iJn4(1,:);
+        var_th(:,ind) = iJn4(2,:);
+        ind = ind+1;
+    end
+    if(model(5) == 1)
+        var_ome(:,ind) = iJn5(1,:);
+        var_th(:,ind) = iJn5(2,:);
+        ind = ind+1;
+    end
+    if(model(6) == 1)
+        var_ome(:,ind) = iJn6(1,:);
+        var_th(:,ind) = iJn6(2,:);   
+        var_th2(:,ind) = iJn6(3,:); 
+        ind = ind+1;
+    end
+    if(model(7) == 1)
+        var_ome(:,ind) = iJn7(1,:);
+        var_th(:,ind) = iJn7(2,:);
+        ind = ind+1;
+    end
+    if(model(8) == 1)
+        var_ome(:,ind) = iJn8(1,:);
+        var_th(:,ind) = iJn8(2,:);
+        ind = ind+1;
+    end
+    if(model(9) == 1)
+        var_ome(:,ind) = iJn9(1,:);
+        var_th(:,ind) = iJn9(2,:);   
+        var_th2(:,ind) = iJn9(3,:); 
+        ind = ind+1;
+    end
+    if(model(10) == 1)
+        var_ome(:,ind) = iJn10(1,:);
+        var_th(:,ind) = iJn10(2,:);
+        ind = ind+1;
+    end
+    if(model(11) == 1)
+        var_ome(:,ind) = iJn11(1,:);
+        var_th(:,ind) = iJn11(2,:);
+        ind = ind+1;
+    end
+    if(model(12) == 1)
+        var_ome(:,ind) = iJn12(1,:);
+        var_th(:,ind) = iJn12(2,:);   
+        var_th2(:,ind) = iJn12(3,:);         
+    end
+    
+end
Index: /applications/dual/vahala/pcrb/pcrb_E2.m
===================================================================
--- /applications/dual/vahala/pcrb/pcrb_E2.m (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb_E2.m (revision 1416)
@@ -0,0 +1,723 @@
+%simulator PMSM for PCRB, choice: Ls/Ldq & correct/incorrect expectation
+clear all;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%   settings
+    % ON/OFF  MODEL    IS CORR. E   INDUCT. L
+model1 = 1; % alpha-beta    nE      Ls
+model2 = 1; % d-q           nE      Ls
+model3 = 1; % 5sc           nE      Ls
+
+model4 = 1; % alpha-beta    E       Ls
+model5 = 1; % d-q           E       Ls
+model6 = 1; % 5sc           E       Ls
+
+model7 = 1; % alpha-beta    nE      Ldq
+model8 = 1; % d-q           nE      Ldq
+model9 = 1; % 5sc           nE      Ldq
+
+model10 = 1; % alpha-beta   E       Ldq
+model11 = 1; % d-q          E       Ldq
+model12 = 1; % 5sc          E       Ldq
+
+    % CONTROL
+    % 1 -   ome = ref_ome, the = integral(ome), ial = ibe = 0
+    % 2 -   PI control    
+    % 3 -   PI + injection (sin -> dq)
+    % 4 -   PI + injection (rect. -> dq)
+    % 5 -   PI + injection (const. -> ud) *J4 ~ 3e-6 for all exc. dq
+    % 6 -   PI + rnd error (ref_ome) *J4: dq bad; ab,5sc Ls ~ 4e-4; ab,5sc Ldq ~5e-6  
+    % 7 -   PI + injection (sin -> al-be)
+    % 8 -   PI + injection (rect. -> al-be)
+    % 9 -   PI + bicrit. (sign) *J4 ~ 1e-3 for all exc. dq
+    % 10 -  PI + rnd bicrit. selection (5)    
+control = 9;
+    % param. amp*phi(injom*dt*t) //phi = sin,cos,sign(sin),sing(cos)
+amp = 10.0;
+injom = 500;
+
+    %cov Q, R matrices
+%         %noise
+% Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
+% R = diag([0.0006 0.0006]);
+%         %EKF
+% Q = diag([0.1 0.1 0.1 0.001]);
+% R = diag([0.05 0.05]);
+        %test
+Q = diag([0.1 0.1 0.1 1]);
+R = diag([0.05 0.05]);
+
+    % plot system variables
+plotsys = 0;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%constants
+
+%   machine parameters
+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;
+
+Lq = 1.0*Ls;
+Ld = 0.9*Ls;
+
+%aliases
+kpp = kp*pp*pp;
+kppj = kpp/J;
+psi = psipm;
+
+a = 0.9898;
+b = 0.0072;
+c = 0.0361;
+d = 1.0;
+e = 0.0149;
+
+%horizont
+T = 120000; 
+
+%reference signal
+ref_ome = zeros(1, T); 
+% 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
+
+
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   system variables
+
+%system state and control
+x_sys = zeros(4, T); 
+u_ab = zeros(2, T); 
+u_dq = zeros(2, T);
+
+%covariance alternatives
+Q4 = Q;
+Q5 = zeros(5);
+Q5(1:4,1:4) = Q;
+Q5(5,5) = Q(4,4);
+q1 = Q4(1,1);
+q2 = Q4(2,2);
+q3 = Q4(3,3);
+q4 = Q4(4,4);
+q5 = Q5(5,5);
+iQ4 = inv(Q4);
+iQ5 = inv(Q5);
+iR = inv(R);
+e05q4 = exp(-1/2*q4);
+e2q4 = exp(-2*q4);
+e92q4 = exp(-9/2*q4);
+e4q4 = exp(4*q4);
+e8q4 = exp(-8*q4);
+e6q4 = exp(6*q4);
+
+%PCRB J matrices
+iJn1 = zeros(4,4,T);
+iJn2 = zeros(4,4,T);
+iJn3 = zeros(5,5,T);
+iJn4 = zeros(4,4,T);
+iJn5 = zeros(4,4,T);
+iJn6 = zeros(5,5,T);
+iJn7 = zeros(4,4,T);
+iJn8 = zeros(4,4,T);
+iJn9 = zeros(5,5,T);
+iJn10 = zeros(4,4,T);
+iJn11 = zeros(4,4,T);
+iJn12 = zeros(5,5,T);
+Jj1 = inv(Q4);
+Jj2 = inv(Q4);
+Jj3 = inv(Q5);
+Jj4 = inv(Q4);
+Jj5 = inv(Q4);
+Jj6 = inv(Q5);
+Jj7 = inv(Q4);
+Jj8 = inv(Q4);
+Jj9 = inv(Q5);
+Jj10 = inv(Q4);
+Jj11 = inv(Q4);
+Jj12 = inv(Q5);
+
+%PI control
+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;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   init
+
+% measurement function derivatives
+C4 = zeros(2,4);
+C4(1,1) = 1.0;
+C4(2,2) = 1.0;
+
+C5 = zeros(2,5);
+C5(1,1) = 1.0;
+C5(2,2) = 1.0;
+
+% system function derivatives
+    %alpha-beta, equal Ls
+A14 = zeros(4);
+A14(1,1) = a;
+A14(2,2) = a;
+A14(3,3) = d;
+A14(4,3) = dt;
+A14(4,4) = 1.0;
+A4 = zeros(4);
+    %d-q, equal Ls
+A25 = zeros(4);
+A25(1,1) = a;
+A25(2,2) = a;
+A25(2,3) = -b;
+A25(3,2) = e;
+A25(3,3) = d;
+A25(4,3) = dt;
+A25(4,4) = 1.0;
+    %5sc, equal Ls
+A36 = zeros(5);
+A36(1,1) = a;
+A36(2,2) = a;
+A36(3,3) = d;
+A36(4,4) = 1.0;
+A36(5,5) = 1.0;
+    %alpha-beta, Ldq
+A70 = zeros(4);
+% A70(4,4) = 1.0;
+% A70(5,5) = 1.0;
+    %d-q, Ldq
+A81 = zeros(4);
+A81(1,1) = 1.0 - Rs*dt/Ld;
+A81(2,2) = 1.0 - Rs*dt/Lq;
+A81(3,3) = 1-B*dt/J;
+A81(4,3) = dt;
+A81(4,4) = 1.0;
+    %5sc, Ldq
+A92 = zeros(5);
+A92(4,4) = 1.0;
+A92(5,5) = 1.0;
+
+
+%expectation corrections
+    %cor. 2 -> 5
+D11_cor5 = diag([dt^2*q3/q2, dt^2*q3/q1, dt^2*q1/q2 + dt^2*q2/q1, 0.0]);
+    %cor. 3 -> 6
+D11_cor6 = 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]);
+    %cor. 8 -> 11
+D11_cor11 = 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]);
+
+% new figure              
+figure;
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   main loop
+
+for t = 1:T-1,
+    %aliases
+    ial = x_sys(1, t);
+    ibe = x_sys(2, t);
+    ome = x_sys(3, t);
+    the = x_sys(4, t); 
+    
+    ia = ial;
+    ib = ibe;
+    
+    st = sin(the);
+    ct = cos(the);
+    
+    id = ial*cos(the) + ibe*sin(the);
+    iq = ibe*cos(the) - ial*sin(the);
+    
+    hia	= ial;
+    hib	= ibe;
+    hom	= ome;
+    sht	= sin(the);
+    cht	= cos(the);
+    sh2t = sin(2*the);
+    ch2t = cos(2*the);
+    sh3t = sin(3*the);
+    ch3t = cos(3*the);
+    ch4t = cos(4*the);    
+    
+    %PCRB models
+    if(model1 == 1) %al-be/nE/Ls
+        A14(1,3) = b*sin(the);
+        A14(1,4) = b*ome*cos(the);
+        A14(2,3) = -b*cos(the);
+        A14(2,4) = b*ome*sin(the);
+        A14(3,1) = -e*sin(the);
+        A14(3,2) = e*cos(the);
+        A14(3,4) = -e*(ial*cos(the)+ibe*sin(the));
+
+        D11 = A14'*iQ4*A14;
+        D12 = -A14'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;
+
+        Jj1 = D22 - D21/(Jj1 + D11)*D12;
+        iJn1(:,:,t) = inv(Jj1);        
+    end
+    if(model2 == 1) %d-q/nE/Ls
+        A25(1,2) = dt*ome;
+        A25(1,3) = dt*iq;
+        A25(2,1) = -dt*ome;
+        A25(2,3) = -dt*id-b;
+       
+        D11 = A25'*iQ4*A25;
+        D12 = -A25'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;    
+
+        Jj2 = D22 - D21/(Jj2 + D11)*D12;
+        iJn2(:,:,t) = inv(Jj2);
+    end
+    if(model3 == 1) %5sc/nE/Ls
+        A36(1,3) = b*sin(the);
+        A36(1,4) = b*ome;
+        A36(2,3) = -b*cos(the);
+        A36(2,5) = -b*ome;
+        A36(3,1) = -e*sin(the);
+        A36(3,2) = e*cos(the);
+        A36(3,4) = -e*ial;
+        A36(3,5) = e*ibe;
+        A36(4,3) = dt*cos(the);
+        A36(4,5) = dt*ome;
+        A36(5,3) = -dt*sin(the);
+        A36(5,4) = -dt*ome;
+
+        D11 = A36'*iQ5*A36;    
+        D12 = -A36'*iQ5;
+        D21 = D12';
+        D22 = iQ5 + C5'*iR*C5;
+
+        Jj3 = D22 - D21/(Jj3 + D11)*D12;
+        iJn3(:,:,t) = inv(Jj3);
+    end
+    if(model4 == 1) %al-be/E/Ls
+        D11 = zeros(4);
+        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));
+
+        A4(1,1) = a;
+        A4(1,2) = 0.0;
+        A4(1,3) = b*sin(the)*exp(-1/2*q4);
+        A4(1,4) = b*ome*cos(the)*exp(-1/2*q4);
+        A4(2,1) = 0.0;
+        A4(2,2) = a;
+        A4(2,3) = -b*cos(the)*exp(-1/2*q4);
+        A4(2,4) = b*ome*sin(the)*exp(-1/2*q4);
+        A4(3,1) = -e*sin(the)*exp(-1/2*q4);
+        A4(3,2) = e*cos(the)*exp(-1/2*q4);
+        A4(3,3) = d;
+        A4(3,4) = -e*(ial*cos(the)*exp(-1/2*q4) + ibe*sin(the)*exp(-1/2*q4));
+        A4(4,1) = 0.0;
+        A4(4,2) = 0.0;
+        A4(4,3) = dt;
+        A4(4,4) = 1.0;
+
+        D12 = -A4'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;
+
+        Jj4 = D22 - D21/(Jj4 + D11)*D12;
+        iJn4(:,:,t) = inv(Jj4);
+    end
+    if(model5 == 1) %d-q/E/Ls
+        A25(1,2) = dt*ome;
+        A25(1,3) = dt*iq;
+        A25(2,1) = -dt*ome;
+        A25(2,3) = -dt*id-b;
+       
+        D11 = A25'*iQ4*A25 + D11_cor5;
+        D12 = -A25'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;    
+
+        Jj5 = D22 - D21/(Jj5 + D11)*D12;
+        iJn5(:,:,t) = inv(Jj5);
+    end
+    if(model6 == 1) %5sc/E/Ls
+        A36(1,3) = b*sin(the);
+        A36(1,4) = b*ome;
+        A36(2,3) = -b*cos(the);
+        A36(2,5) = -b*ome;
+        A36(3,1) = -e*sin(the);
+        A36(3,2) = e*cos(the);
+        A36(3,4) = -e*ial;
+        A36(3,5) = e*ibe;
+        A36(4,3) = dt*cos(the);
+        A36(4,5) = dt*ome;
+        A36(5,3) = -dt*sin(the);
+        A36(5,4) = -dt*ome;
+
+        D11 = A36'*iQ5*A36 + D11_cor6;    
+        D12 = -A36'*iQ5;
+        D21 = D12';
+        D22 = iQ5 + C5'*iR*C5;
+
+        Jj6 = D22 - D21/(Jj6 + D11)*D12;
+        iJn6(:,:,t) = inv(Jj6);
+    end
+    if(model7 == 1) %al-be/nE/Ldq        
+        A70 = [[ (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 = A70'*iQ4*A70;
+        D12 = -A70'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;
+
+        Jj7 = D22 - D21/(Jj7 + D11)*D12;
+        iJn7(:,:,t) = inv(Jj7);        
+    end
+    if(model8 == 1) %d-q/nE/Ldq
+        A81(1,2) = Lq*dt/Ld*ome;
+        A81(1,3) = Lq*dt/Ld*iq;
+        A81(2,1) = -Ld*dt/Lq*ome;
+        A81(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
+        A81(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
+        A81(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);    
+        
+        D11 = A81'*iQ4*A81;
+        D12 = -A81'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;
+    
+        Jj8 = D22 - D21/(Jj8 + D11)*D12;
+        iJn8(:,:,t) = inv(Jj8);
+    end
+    if(model9 == 1) %5sc/nE/Ldq        
+        A92(1, 1) = 1.0 + dt*(-Rs*(st^2/Lq + ct^2/Ld) + ome*st*ct*(Ld/Lq - Lq/Ld));
+        A92(1, 2) = dt*(-ome + Rs*st*ct*(1/Lq - 1/Ld) + ome*(Ld*st^2/Lq + Lq*ct^2/Ld));
+        A92(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);
+        A92(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);
+        A92(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);
+
+        A92(2, 1) = dt*(ome + Rs*st*ct*(1/Lq - 1/Ld) - ome*(Lq*st^2/Ld + Ld*ct^2/Lq));
+        A92(2, 2) = 1 + dt*(-Rs*(st^2/Ld + ct^2/Lq) + ome*st*ct*(Lq/Ld - Ld/Lq));
+        A92(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);
+        A92(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));
+        A92(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));
+
+        A92(3, 1) = dt*kpp/J*((Ld - Lq)*(-2*ial*st*ct + ibe*ct^2 - ibe*st^2) - psi*st);
+        A92(3, 2) = dt*kpp/J*((Ld - Lq)*(ial*ct^2 - ial*st^2 + 2*ibe*st*ct) + psi*ct);
+        A92(3, 3) = 1;
+        A92(3, 4) = dt*kpp/J*((Ld - Lq)*(-ial^2*ct - ial*ibe*2*st + ibe^2*ct) - psi*ial);
+        A92(3, 5) = dt*kpp/J*((Ld - Lq)*(-ial^2*st + ial*ibe*2*ct + ibe^2*st) + psi*ibe);
+
+        A92(4,3) = dt*cos(the);
+        A92(4,5) = dt*ome;
+        A92(5,3) = -dt*sin(the);
+        A92(5,4) = -dt*ome;
+    
+        D11 = A92'*iQ5*A92;
+        D12 = -A92'*iQ5;
+        D21 = D12';
+        D22 = iQ5 + C5'*iR*C5;
+
+        Jj9 = D22 - D21/(Jj9 + D11)*D12;
+        iJn9(:,:,t) = inv(Jj9);
+    end
+    if(model10 == 1) %al-be/E/Ldq
+        A70 = [[ (Lq - Rs*dt*1/2*(1 - e2q4*ch2t))/Lq - (dt*hom*sht*e05q4*Lq^2*cht*e05q4 + Rs*dt*Lq*1/2*(1 + e2q4*ch2t))/(Ld*Lq) + (Ld*dt*hom*sht*cht*e2q4)/Lq,                                       (dt*(Ld - Lq)*(- Lq*hom*1/2*(1 + e2q4*ch2t) + Rs*sht*cht*e2q4 + Ld*hom*1/2*(1 - e2q4*ch2t)))/(Ld*Lq),    dt*cht*e05q4*(hia*sht*e05q4 - hib*cht*e05q4 + (Lq*(hib*cht*e05q4 - hia*sht*e05q4))/Ld) + dt*sht*e05q4*(psi/Lq - hia*cht*e05q4 - hib*sht*e05q4 + (Ld*(hia*cht*e05q4 + hib*sht*e05q4))/Lq),                                               (dt*(hom*psi*cht*e05q4 + Rs*hib*ch2t*e2q4 - Rs*hia*sh2t*e2q4))/Lq + (Ld*dt*(hia*hom*ch2t*e2q4 + hib*hom*sh2t*e2q4))/Lq - (dt*(Lq^2*hia*hom*ch2t*e2q4 + Lq^2*hib*hom*sh2t*e2q4 + Lq*Rs*hib*ch2t*e2q4 - Lq*Rs*hia*sh2t*e2q4))/(Ld*Lq)];...
+                [                                       (dt*(Ld - Lq)*(- Ld*hom*1/2*(1 + e2q4*ch2t) + Rs*sht*cht*e2q4 + Lq*hom*1/2*(1 - e2q4*ch2t)))/(Ld*Lq), (Lq - Rs*dt*1/2*(1 + e2q4*ch2t))/Lq - (Lq*Rs*dt*1/2*(1 - e2q4*ch2t) - Lq^2*dt*hom*sht*cht*e2q4)/(Ld*Lq) - (Ld*dt*hom*sht*cht*e2q4)/Lq, (dt*(Lq*hia - psi*cht*e05q4))/Lq + (dt*((Lq^2*hia*ch2t*e2q4)/2 - (Lq^2*hia)/2 + (Lq^2*hib*sh2t*e2q4)/2))/(Ld*Lq) - (Ld*dt*(hia/2 + (hia*ch2t*e2q4)/2 + (hib*sh2t*e2q4)/2))/Lq, (dt*hom*psi*sht*e05q4 - Rs*dt*hia*(2*1/2*(1 - e2q4*ch2t) - 1) + Rs*dt*hib*sh2t*e2q4)/Lq + (Ld*(dt*hib*hom*(2*1/2*(1 - e2q4*ch2t) - 1) + dt*hia*hom*sh2t*e2q4))/Lq - (Lq*Rs*dt*hib*sh2t*e2q4 + Lq^2*dt*hib*hom*(2*1/2*(1 - e2q4*ch2t) - 1) + Lq^2*dt*hia*hom*sh2t*e2q4 - Lq*Rs*dt*hia*(2*1/2*(1 - e2q4*ch2t) - 1))/(Ld*Lq)];...
+                [     -dt*kppj*(psi*sht*e05q4 - cht*e05q4*(Ld - Lq)*(hib*cht*e05q4 - hia*sht*e05q4) + sht*e05q4*(Ld - Lq)*(hia*cht*e05q4 + hib*sht*e05q4)),      dt*kppj*(psi*cht*e05q4 + cht*e05q4*(Ld - Lq)*(hia*cht*e05q4 + hib*sht*e05q4) + sht*e05q4*(Ld - Lq)*(hib*cht*e05q4 - hia*sht*e05q4)),                                                                                                                                                                         1.0,                                                                                                                                                   -dt*kppj*(psi*(hia*cht*e05q4 + hib*sht*e05q4) + (Ld - Lq)*(hia*cht*e05q4 + hib*sht*e05q4)^2 - (Ld - Lq)*(hib*cht*e05q4 - hia*sht*e05q4)^2)];...
+                [                                                                                                                             0.0,                                                                                                                             0.0,                                                                                                                                                                        dt,                                                                                                                                                                                                                                                                                1.0]];
+        
+        D11 = zeros(4);
+        D11(1,1) = 1/q1 + (dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 + (dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q2 - (2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 + (4*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Rs^2*dt^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (2*Rs*dt*1/2*(1 + e2q4*ch2t))/(Ld*q1) + (dt^2*kppj^2*psi^2*1/2*(1 - e2q4*ch2t))/q3 - (2*Rs*dt*1/2*(1 - e2q4*ch2t))/(Lq*q1) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) + (2*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Ld*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (2*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*dt*hom*sht*cht*e2q4)/(Lq*q1) - (2*Lq*dt*hom*sht*cht*e2q4)/(Ld*q1) + (4*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (2*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (4*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (2*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (8*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3;
+        D11(1,2) = (dt*hom*1/2*(1 + e2q4*ch2t))/q2 - (dt*hom*1/2*(1 + e2q4*ch2t))/q1 - (dt*hom*1/2*(1 - e2q4*ch2t))/q1 + (dt*hom*1/2*(1 - e2q4*ch2t))/q2 - (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Lq*dt*hom*1/2*(1 + e2q4*ch2t))/(Ld*q1) - (Ld*dt*hom*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Ld*dt*hom*1/2*(1 - e2q4*ch2t))/(Lq*q1) - (Lq*dt*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (dt^2*kppj^2*psi^2*sht*cht*e2q4)/q3 - (Rs*dt*sht*cht*e2q4)/(Ld*q1) - (Rs*dt*sht*cht*e2q4)/(Ld*q2) + (Rs*dt*sht*cht*e2q4)/(Lq*q1) + (Rs*dt*sht*cht*e2q4)/(Lq*q2) + (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) - (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) - (Rs^2*dt^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (Rs^2*dt^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q2) + (Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (Lq^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (Ld^2*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (2*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (2*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (Ld^2*dt^2*hia*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*hia*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (Ld*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (Lq*dt^2*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (Lq*Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (Ld*Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) - (3*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (3*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (3*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (3*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (Ld*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (Lq*Rs*dt^2*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (4*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (4*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (4*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*Lq*dt^2*hia*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*hia*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (6*Ld^2*dt^2*hia*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (6*Lq^2*dt^2*hia*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (12*Ld*Lq*dt^2*hia*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3;
+        D11(1,3) = (3*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*q2) - (dt*hib*1/2*(1 - e2q4*ch2t))/q1 - (dt*hib*1/2*(1 + e2q4*ch2t))/q1 - (dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) + (3*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*q2) - (dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) - (dt*kppj*psi*sht*e05q4)/q3 + (dt*psi*sht*e05q4)/(Lq*q1) + (Lq*dt*hib*1/2*(1 + e2q4*ch2t))/(Ld*q1) + (Ld*dt*hib*1/2*(1 - e2q4*ch2t))/(Lq*q1) - (2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/q1 + (dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/q2 + (Rs*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) - (dt^2*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq*q2) - (dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Ld*dt*hib*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Lq*dt*hib*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Ld*dt*hib*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Lq*dt*hib*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Ld*dt^2*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q2) - (Ld*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) - (3*Lq*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*Ld*q2) + (Ld*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq*q2) - (Lq*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld*q2) - (3*Rs*dt^2*hia*sht*cht*e2q4)/(2*Ld*q2) + (3*Rs*dt^2*hia*sht*cht*e2q4)/(2*Lq*q2) + (Ld^2*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) + (Ld^2*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq^2*q2) + (Lq^2*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) + (Lq^2*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld^2*q2) - (2*Ld*dt*hia*kppj*sht*cht*e2q4)/q3 + (2*Lq*dt*hia*kppj*sht*cht*e2q4)/q3 + (Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Ld*q2) + (Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Ld*q2) - (Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) - (Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Lq*q2) + (Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Lq*q2) - (Rs*dt^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) - (dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) + (dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq*q2) + (Ld*dt*hia*sht*cht*e2q4)/(Lq*q1) - (Lq*dt*hia*sht*cht*e2q4)/(Ld*q1) + (Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (Lq*Rs*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (3*Ld*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*Lq*q2) - (Lq*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) - (Ld*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq*q2) + (Lq*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld*q2) - (Ld*Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (Lq*Rs*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs*dt^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) + (Rs*dt^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q2) - (Lq^2*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld^2*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq^2*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (Ld^2*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (Ld*Rs*dt^2*hia*sht*cht*e2q4)/(2*Lq^2*q2) + (Lq*Rs*dt^2*hia*sht*cht*e2q4)/(2*Ld^2*q2) + (Lq^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (Lq^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(2*Ld^2*q2) - (Ld^2*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(2*Lq^2*q2) - (Ld*Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Lq*Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld*Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Lq^2*q2) - (Ld*Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Lq^2*q2) + (Lq*Rs*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Ld^2*q2) - (Lq*Rs*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Ld^2*q2) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (Ld*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (Lq*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (Ld*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1);
+        D11(1,4) = (dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/q2 - (dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/q2 - (2*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 + (Rs^2*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*q2) + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq*q2) - (Rs*dt*hib*1/2*(1 + e2q4*ch2t))/(Ld*q1) + (Rs*dt*hib*1/2*(1 + e2q4*ch2t))/(Lq*q1) + (dt^2*hib*kppj^2*psi^2*1/2*(1 - e2q4*ch2t))/q3 + (Rs*dt*hib*1/2*(1 - e2q4*ch2t))/(Ld*q1) - (Rs*dt*hib*1/2*(1 - e2q4*ch2t))/(Lq*q1) + (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 - (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 + (2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (4*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 + (2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (dt*hom*psi*cht*e05q4)/(Lq*q1) - (Rs*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (Rs*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (2*Rs*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(Lq*q2) - (Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (2*Rs*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (4*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Rs*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (2*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (2*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Lq*dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (2*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Lq*q2) + (Lq*dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (2*Ld*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (2*Lq*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (Rs^2*dt^2*hia*sht*cht*e2q4)/(Ld^2*q2) + (Rs^2*dt^2*hia*sht*cht*e2q4)/(Lq^2*q2) - (Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (2*Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (Ld*dt*hia*hom*1/2*(1 + e2q4*ch2t))/(Lq*q1) - (Lq*dt*hia*hom*1/2*(1 + e2q4*ch2t))/(Ld*q1) - (Rs^2*dt^2*hib*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) - (Ld*dt*hia*hom*1/2*(1 - e2q4*ch2t))/(Lq*q1) + (Lq*dt*hia*hom*1/2*(1 - e2q4*ch2t))/(Ld*q1) - (Rs^2*dt^2*hib*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) - (2*Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (dt^2*hia*kppj^2*psi^2*sht*cht*e2q4)/q3 + (Ld^2*dt^2*hib*(hom^2 + q3)*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) + (2*Rs*dt*hia*sht*cht*e2q4)/(Ld*q1) - (2*Rs*dt*hia*sht*cht*e2q4)/(Lq*q1) - (Lq^2*dt^2*hib*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) + (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq*q2) + (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (10*Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (10*Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*dt*hib*hom*sht*cht*e2q4)/(Lq*q1) - (2*Lq*dt*hib*hom*sht*cht*e2q4)/(Ld*q1) - (2*Rs^2*dt^2*hia*sht*cht*e2q4)/(Ld*Lq*q2) + (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (2*Ld*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) - (2*Lq*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (2*Lq*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (3*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (3*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (6*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) + (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) - (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) + (Lq*Rs*dt^2*hia*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hia*hom*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) + (Ld*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hia*hom*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (2*Lq*Rs*dt^2*hia*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (3*Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (2*Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (3*Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) + (2*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) + (4*Rs^2*dt^2*hia*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (Rs*dt^2*hib*hom*sht*cht*e2q4)/(Ld*q2) + (Rs*dt^2*hib*hom*sht*cht*e2q4)/(Lq*q2) + (4*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (Lq^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (Lq^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (Ld^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) + (2*Lq^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (2*Ld^2*dt^2*hia*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (2*Ld*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (2*Lq*dt^2*hib*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*Lq*q1) + (6*Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (6*Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (4*Rs^2*dt^2*hib*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) + (3*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (6*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (3*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (4*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (20*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (3*Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (3*Lq*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (4*Lq*Rs*dt^2*hib*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (7*Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (7*Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (3*Ld*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (3*Lq*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (4*Ld*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (2*Lq*Rs*dt^2*hia*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) - (12*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (12*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (Ld*Rs*dt^2*hib*hom*sht*cht*e2q4)/(Lq^2*q2) - (Lq*Rs*dt^2*hib*hom*sht*cht*e2q4)/(Ld^2*q2);
+
+        D11(2,1) = D11(1,2);
+        D11(2,2) = 1/q2 + (dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 + (4*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 - (2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (Rs^2*dt^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (dt^2*kppj^2*psi^2*1/2*(1 + e2q4*ch2t))/q3 - (2*Rs*dt*1/2*(1 + e2q4*ch2t))/(Lq*q2) - (2*Rs*dt*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (2*Ld*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Ld*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (2*Lq*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (2*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) + (2*Rs^2*dt^2*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (4*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Lq^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Ld^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Lq^2*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld*dt*hom*sht*cht*e2q4)/(Lq*q2) + (2*Lq*dt*hom*sht*cht*e2q4)/(Ld*q2) - (2*Ld*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (4*Ld*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*Lq*dt^2*hia*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (4*Lq*dt^2*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (4*Ld^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (4*Lq^2*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (4*Ld*Lq*dt^2*(hia^2 + q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (8*Ld*Lq*dt^2*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld*Rs*dt^2*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (2*Lq*Rs*dt^2*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (8*Ld*Lq*dt^2*hia*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3;
+        D11(2,3) = (dt*hia)/q2 + (dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt*kppj*psi*cht*e05q4)/q3 - (dt*psi*cht*e05q4)/(Lq*q2) - (Ld*dt*hia)/(2*Lq*q2) - (Lq*dt*hia)/(2*Ld*q2) - (Ld*dt*hia*1/2*(1 + e2q4*ch2t))/(2*Lq*q2) + (Lq*dt*hia*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) + (Ld*dt*hia*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) - (Lq*dt*hia*1/2*(1 - e2q4*ch2t))/(2*Ld*q2) + (4*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/q1 - (2*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/q2 + (Rs*dt^2*hia*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) - (Rs*dt^2*hia*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld*q2) - (Rs*dt^2*hia*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Rs*dt^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q2) - (Rs*dt^2*hia*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (Rs*dt^2*hia*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) - (Rs*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq*q2) - (dt^2*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq*q1) - (dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (Ld*dt*hia*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Lq*dt*hia*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Ld*dt*hia*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Lq*dt*hia*kppj*1/2*(1 - e2q4*ch2t))/q3 - (2*Ld*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Ld*dt^2*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) + (Lq^2*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (2*Ld*dt*hib*kppj*sht*cht*e2q4)/q3 - (2*Lq*dt*hib*kppj*sht*cht*e2q4)/q3 + (Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) - (2*Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Rs*dt^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) - (dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q2) - (dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq*q1) - (Ld*dt*hib*sht*cht*e2q4)/(Lq*q2) + (Lq*dt*hib*sht*cht*e2q4)/(Ld*q2) - (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Ld*q2) - (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Lq*q2) + (Ld*Rs*dt^2*hia*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) + (Ld*Rs*dt^2*hia*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*Lq^2*q2) + (Lq*Rs*dt^2*hia*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) + (Lq*Rs*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*Ld^2*q2) - (2*Lq*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Ld*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Lq^2*q2) - (Lq*Rs*dt^2*hia*1/8*(1 - e8q4*ch4t))/(2*Ld^2*q2) - (Rs*dt^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q1) + (Rs*dt^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) - (Lq^2*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld^2*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Ld*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (2*Lq*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (Lq^2*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Ld^2*q2) + (Lq^2*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Ld^2*q2) - (Ld^2*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(2*Lq^2*q2) + (Ld^2*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(2*Lq^2*q2) - (Ld*dt^2*hia*hom*sht*cht*e2q4)/(Lq*q2) + (Lq*dt^2*hia*hom*sht*cht*e2q4)/(Ld*q2) + (Lq^2*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (Ld*Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (Ld*Rs*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (Lq*Rs*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Ld*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (Ld*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (Lq*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (Lq*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Lq^2*dt^2*hia*hom*sht*cht*e2q4)/(2*Ld^2*q2) + (Ld^2*dt^2*hia*hom*sht*cht*e2q4)/(2*Lq^2*q2) + (Ld*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2);
+        D11(2,4) = (dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 - (Rs*dt*hia)/(Ld*q2) + (Rs*dt*hia)/(Lq*q2) + (Rs^2*dt^2*hia*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (2*Rs^2*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (dt*hom*psi*sht*e05q4)/(Lq*q2) + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q1) - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq*q1) - (dt^2*hia*kppj^2*psi^2*1/2*(1 + e2q4*ch2t))/q3 + (2*Rs*dt*hia*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (2*Rs*dt*hia*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (4*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (Ld*dt*hib*hom)/(Lq*q2) + (Lq*dt*hib*hom)/(Ld*q2) - (Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 + (4*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 - (2*dt^2*hib*(hom^2 + q3)*sht*cht*e2q4)/q2 - (Rs^2*dt^2*hia*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) + (2*Rs*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*hib*hom*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (Rs*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Ld^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (2*Lq^2*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Ld*dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Lq*dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld*dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Lq*dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (2*Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (2*Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (Rs^2*dt^2*hia*1/2*(1 + e2q4*ch2t))/(Ld*Lq*q2) + (2*Ld*dt*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Lq*dt*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (Rs^2*dt^2*hia*1/2*(1 - e2q4*ch2t))/(Ld*Lq*q2) + (2*Rs^2*dt^2*hia*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q2) + (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (dt^2*hib*kppj^2*psi^2*sht*cht*e2q4)/q3 - (Lq^2*dt^2*hia*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (2*Rs*dt*hib*sht*cht*e2q4)/(Ld*q2) + (2*Rs*dt*hib*sht*cht*e2q4)/(Lq*q2) - (Ld^2*dt^2*hia*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq*q1) + (Lq^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Lq^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (2*Ld^2*dt^2*hia*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (10*Ld^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (10*Lq^2*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (2*Ld*dt*hia*hom*sht*cht*e2q4)/(Lq*q2) - (2*Lq*dt*hia*hom*sht*cht*e2q4)/(Ld*q2) + (4*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (4*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Ld*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Lq*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Lq*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (Lq^2*dt^2*hib*(hom^2 + q3)*sht*cht*e2q4)/(Ld^2*q2) + (Ld^2*dt^2*hib*(hom^2 + q3)*sht*cht*e2q4)/(Lq^2*q2) - (3*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (4*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q2) - (3*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) + (2*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) + (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) - (Ld*dt^2*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (Lq*Rs*dt^2*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*hib*hom*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) - (Ld*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (Lq*Rs*dt^2*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) + (2*Lq*Rs*dt^2*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (2*Ld*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (3*Ld*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (2*Lq*dt^2*(hia^2 + q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (3*Lq*dt^2*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) - (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) - (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (2*Rs^2*dt^2*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q2) + (Rs*dt^2*hia*hom*sht*cht*e2q4)/(Ld*q2) + (Rs*dt^2*hia*hom*sht*cht*e2q4)/(Lq*q2) + (Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (2*Lq^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (2*Ld^2*dt^2*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (6*Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Ld^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (6*Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (6*Lq^2*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (2*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (Rs*dt^2*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*Lq*q2) - (2*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (4*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (2*Rs^2*dt^2*hia*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) - (Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (5*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (2*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (2*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (5*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) + (Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) - (4*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) + (20*Ld*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (3*Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (3*Lq*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (4*Lq*Rs*dt^2*hia*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (7*Ld*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (7*Lq*dt^2*hia*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (3*Ld*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (3*Lq*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (4*Ld*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Lq*Rs*dt^2*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Rs*dt^2*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) - (12*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (12*Ld*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (Ld*Rs*dt^2*hia*hom*sht*cht*e2q4)/(Lq^2*q2) - (Lq*Rs*dt^2*hia*hom*sht*cht*e2q4)/(Ld^2*q2);
+
+        D11(3,1) = D11(1,3);
+        D11(3,2) = D11(2,3);
+        D11(3,3) = 1/q3 + dt^2/q4 + (3*dt^2*(hia^2 + q1))/(2*q2) + (dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 - (dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) + (dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 - (dt^2*(hia^2 + q1)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(2*q2) + (Lq^2*dt^2*(hia^2 + q1))/(4*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1))/(4*Lq^2*q2) - (2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/q1 + (4*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/q1 + (dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/q2 - (2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/q2 + (dt^2*psi^2*1/2*(1 + e2q4*ch2t))/(Lq^2*q2) - (Ld*dt^2*(hia^2 + q1))/(Lq*q2) - (Lq*dt^2*(hia^2 + q1))/(Ld*q2) + (dt^2*psi^2*1/2*(1 - e2q4*ch2t))/(Lq^2*q1) + (Lq^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Lq^2*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) + (Lq^2*dt^2*(hia^2 + q1)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(4*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(4*Lq^2*q2) + (dt^2*hia*psi*cht*e05q4)/(Ld*q2) - (2*dt^2*hia*psi*cht*e05q4)/(Lq*q2) + (Ld^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Lq^2*q2) + (Lq^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(4*Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(4*Lq^2*q2) - (dt^2*hia*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q2) - (2*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq*q1) - (2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 + (2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 - (2*Lq*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (Ld*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(Lq*q2) + (Lq*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(Ld*q2) - (2*Ld*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Ld*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (Lq*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (2*Ld*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) - (2*Ld*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (2*Lq*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*q1) + (Lq^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (Lq^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(2*Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(2*Lq^2*q2) + (Lq^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) + (2*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) + (dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (2*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q2) - (2*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq*q1) + (Ld*dt^2*hia*psi*cht*e05q4)/(Lq^2*q2) + (Ld*dt^2*hia*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q2) - (2*Lq^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (Lq^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (Lq^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q2) - (Ld^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (Ld^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (2*Ld*dt^2*hia*hib*sht*cht*e2q4)/(Lq*q2) + (2*Lq*dt^2*hia*hib*sht*cht*e2q4)/(Ld*q2) - (2*Ld*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Ld*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Lq*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Lq*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (Lq^2*dt^2*hia*hib*sht*cht*e2q4)/(Ld^2*q2) + (Ld^2*dt^2*hia*hib*sht*cht*e2q4)/(Lq^2*q2) + (2*Ld*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) - (Ld*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) + (2*Ld*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2);
+        D11(3,4) = dt/q4 - (3*Rs*dt^2*(hia^2 + q1))/(2*Ld*q2) + (3*Rs*dt^2*(hia^2 + q1))/(2*Lq*q2) - (Rs*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (5*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Ld*q2) + (Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) + (2*Rs*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (7*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Lq*q2) + (Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (Ld*Rs*dt^2*(hia^2 + q1))/(2*Lq^2*q2) + (Lq*Rs*dt^2*(hia^2 + q1))/(2*Ld^2*q2) + (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 - (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 + (2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 + (2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q2 + (4*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (Ld*dt*(hia^2 + q1)*kppj*1/2*(1 + e2q4*ch2t))/q3 + (Ld*dt*(hib^2 + q2)*kppj*1/2*(1 + e2q4*ch2t))/q3 + (Lq*dt*(hia^2 + q1)*kppj*1/2*(1 + e2q4*ch2t))/q3 - (Lq*dt*(hib^2 + q2)*kppj*1/2*(1 + e2q4*ch2t))/q3 + (Ld*dt*(hia^2 + q1)*kppj*1/2*(1 - e2q4*ch2t))/q3 - (Ld*dt*(hib^2 + q2)*kppj*1/2*(1 - e2q4*ch2t))/q3 - (Lq*dt*(hia^2 + q1)*kppj*1/2*(1 - e2q4*ch2t))/q3 + (Lq*dt*(hib^2 + q2)*kppj*1/2*(1 - e2q4*ch2t))/q3 + (dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 - (dt^2*hia*hib*hom*1/2*(1 + e2q4*ch2t))/q2 + (dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/q2 - (2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 - (dt*hia*kppj*psi*cht*e05q4)/q3 - (dt*hib*kppj*psi*sht*e05q4)/q3 - (2*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/q2 + (2*Rs*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Ld*q2) - (Rs*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Lq*q2) - (Rs*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) + (dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q1) - (dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq*q1) - (Ld*dt^2*hia*hib*hom)/(Lq*q2) + (Lq*dt^2*hia*hib*hom)/(Ld*q2) + (dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*q1) - (dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(2*Ld*q2) + (2*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (2*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (2*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq*q2) + (2*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (Lq*Rs*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (Ld*Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) - (Lq*Rs*dt^2*(hia^2 + q1)*1/2*(1 + e2q4*ch2t))/(2*Ld^2*q2) - (Ld*Rs*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (3*Ld*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Lq^2*q2) - (Lq*Rs*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) - (Ld*Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (Lq*Rs*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) - (Lq^2*dt^2*hia*hib*hom)/(2*Ld^2*q2) + (Ld^2*dt^2*hia*hib*hom)/(2*Lq^2*q2) + (dt^2*hom*psi^2*sht*cht*e2q4)/(Lq^2*q1) - (dt^2*hom*psi^2*sht*cht*e2q4)/(Lq^2*q2) - (Rs*dt^2*hia*psi*cht*e05q4)/(Lq^2*q2) - (6*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/q1 + (6*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/q2 - (dt^2*hib*hom*psi*cht*e05q4)/(Ld*q2) - (dt^2*hia*hom*psi*sht*e05q4)/(2*Ld*q2) + (dt^2*hia*hom*psi*sht*e05q4)/(Lq*q2) - (2*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) - (2*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) + (5*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(2*Ld*q2) + (3*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq*q1) + (Ld*dt^2*hib*hom*psi*cht*e05q4)/(Lq^2*q2) - (Ld*dt^2*hia*hom*psi*sht*e05q4)/(2*Lq^2*q2) - (2*Ld*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (2*Ld*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (2*Lq*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (2*Lq*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (Lq^2*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq^2*q2) + (Lq^2*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Lq^2*q2) + (Rs*dt^2*hia*psi*cht*e05q4)/(Ld*Lq*q2) - (Ld*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (Lq*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (Ld*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (Lq*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) + (2*Ld*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) - (2*Lq*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) - (Ld*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q1) + (Ld*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(2*Lq^2*q2) - (2*Rs*dt^2*hia*hib*sht*cht*e2q4)/(Ld*q2) + (4*Rs*dt^2*hia*hib*sht*cht*e2q4)/(Lq*q2) - (2*Ld*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Lq*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (Lq*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (Ld*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (Lq*Rs*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (2*Ld*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Lq*Rs*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (2*Lq^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (2*Ld^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Lq^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q2) + (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (Ld^2*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) - (2*Lq^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (2*Ld^2*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (Rs*dt^2*hib*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*Lq*q1) - (Lq^2*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Lq^2*dt^2*hia*hib*hom*1/2*(1 + e2q4*ch2t))/(2*Ld^2*q2) + (Ld^2*dt^2*hia*hib*hom*1/2*(1 + e2q4*ch2t))/(2*Lq^2*q2) - (Ld^2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq^2*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(2*Ld^2*q2) - (3*Ld^2*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(2*Lq^2*q2) + (Lq^2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (Ld^2*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (4*Ld*dt*hia*hib*kppj*sht*cht*e2q4)/q3 + (4*Lq*dt*hia*hib*kppj*sht*cht*e2q4)/q3 - (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (5*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) - (3*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) + (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q2) + (2*Ld*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq*q2) - (2*Lq*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld*q2) + (5*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) + (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) - (3*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) + (Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q2) - (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) + (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (2*Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q2) + (3*Lq^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (3*Ld^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (3*Lq^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) - (3*Ld^2*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (3*Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (3*Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) + (3*Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (Ld*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q2) + (3*Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (Lq*Rs*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q2) + (2*Ld*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) + (3*Ld*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) - (5*Ld*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(2*Lq^2*q2) - (3*Ld*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) + (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q1) - (Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) - (2*Rs*dt^2*hia*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) + (2*Rs*dt^2*hib*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q2) - (2*Ld*Rs*dt^2*hia*hib*sht*cht*e2q4)/(Lq^2*q2);
+
+        D11(4,1) = D11(1,4);
+        D11(4,2) = D11(2,4);
+        D11(4,3) = D11(3,4);
+        D11(4,4) = 1/q4 - (2*dt^2*(hib^2 + q2)*(hom^2 + q3))/q2 + (Rs^2*dt^2*(hia^2 + q1))/(Ld^2*q2) + (Rs^2*dt^2*(hia^2 + q1))/(Lq^2*q2) - (2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q1 - (2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q1 + (8*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/q2 - (8*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q2 + (4*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 - (8*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q1 - (8*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/q2 + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) - (4*Rs^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (Rs^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (4*Rs^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (dt^2*(hom^2 + q3)*psi^2*1/2*(1 + e2q4*ch2t))/(Lq^2*q1) + (Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (dt^2*(hom^2 + q3)*psi^2*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (dt^2*(hia^2 + q1)*kppj^2*psi^2*1/2*(1 + e2q4*ch2t))/q3 + (dt^2*(hib^2 + q2)*kppj^2*psi^2*1/2*(1 - e2q4*ch2t))/q3 - (2*Rs^2*dt^2*(hia^2 + q1))/(Ld*Lq*q2) + (Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Lq^2*q2) - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) + (8*Rs^2*dt^2*(hia^2 + q1)*1/2*(1 - e2q4*ch2t))/(Ld*Lq*q2) - (8*Rs^2*dt^2*(hia^2 + q1)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q2) + (Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) - (4*Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (4*Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) + (2*dt^2*hib*(hom^2 + q3)*psi*sht*e05q4)/(Ld*q2) - (2*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Rs*dt^2*hia*hib*hom)/(Ld*q2) + (2*Rs*dt^2*hia*hib*hom)/(Lq*q2) - (2*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (8*dt^2*hia*hib*(hom^2 + q3)*sht*cht*e2q4)/q2 - (2*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 - (2*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*Ld*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (2*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*q1) + (2*Ld*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (2*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (4*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (4*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (8*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q1 - (8*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q1 - (16*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q2 - (2*Rs^2*dt^2*(hib^2 + q2)*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*Lq*q1) + (4*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/q3 + (2*dt^2*hia*hib*kppj^2*psi^2*sht*cht*e2q4)/q3 + (2*Rs*dt^2*hia*hom*psi*sht*e05q4)/(Lq^2*q2) + (4*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/q3 + (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) - (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (4*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (8*Rs^2*dt^2*(hia^2 + q1)*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) + (4*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q1) - (8*Rs^2*dt^2*(hib^2 + q2)*1/8*(1 - e8q4*ch4t))/(Ld*Lq*q2) - (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) + (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q1) - (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*q1) + (8*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (8*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*q2) - (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) + (4*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q1) - (4*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq*q1) + (8*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (8*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq*q2) - (2*Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) - (2*Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) + (4*Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Ld^2*q2) + (4*Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) - (2*Ld*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (2*Ld*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*Lq*dt^2*(hia^3 + 3*hia*q1)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (2*Lq*dt^2*(hib^3 + 3*hib*q2)*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 + (2*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q1) - (4*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*q1) - (4*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*q2) - (2*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (2*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) + (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q1) + (20*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (20*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (2*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q1) - (8*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld*q2) + (8*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld*q2) - (2*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q1) - (8*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq*q2) + (8*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq*q2) - (2*Ld*dt^2*hib*(hom^2 + q3)*psi*sht*e05q4)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hia*hib*hom)/(Lq^2*q2) - (2*Lq*Rs*dt^2*hia*hib*hom)/(Ld^2*q2) - (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q2) + (4*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (4*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 + (8*Ld^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (8*Ld^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (8*Ld^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (8*Ld^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (8*Lq^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (8*Lq^2*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (8*Lq^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (8*Lq^2*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (2*Ld*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Lq^2*q1) + (4*Ld*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Lq^2*q2) + (4*Rs^2*dt^2*hia*hib*sht*cht*e2q4)/(Ld^2*q2) + (4*Rs^2*dt^2*hia*hib*sht*cht*e2q4)/(Lq^2*q2) - (2*Ld*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 + (2*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/q3 - (4*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld*q2) + (4*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Ld*q2) - (4*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq*q2) + (4*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Lq*q2) - (2*Ld*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 + (2*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/q3 - (4*Lq^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (4*Lq^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (4*Ld^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (4*Ld^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) + (8*Lq^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (8*Ld^2*dt^2*hia*hib*(hom^2 + q3)*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(3*e4q4*cht + ch3t))/(Ld*Lq*q1) + (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(3*e4q4*sht - sh3t))/(Ld*Lq*q2) + (4*Ld*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Lq^2*q2) + (4*Lq*Rs*dt^2*(hia^2 + q1)*hom*sht*cht*e2q4)/(Ld^2*q2) - (4*Ld*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Lq^2*q2) - (4*Lq*Rs*dt^2*(hib^2 + q2)*hom*sht*cht*e2q4)/(Ld^2*q2) - (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) - (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (4*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) - (40*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kppj^2*1/8*(1 - e8q4*ch4t))/q3 - (8*Rs^2*dt^2*hia*hib*sht*cht*e2q4)/(Ld*Lq*q2) + (4*Ld*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) - (4*Ld*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) + (4*Lq*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) - (4*Lq*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (4*Ld*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q1) + (4*Ld*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Lq^2*q1) - (4*Lq*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q1) + (4*Lq*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld^2*q1) - (8*Ld*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) - (8*Lq*Rs*dt^2*(hia^2 + q1)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) + (8*Ld*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Lq^2*q2) + (8*Lq*Rs*dt^2*(hib^2 + q2)*hom*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld^2*q2) - (16*Ld*Lq*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 + (16*Ld*Lq*dt^2*hia*(hib^3 + 3*hib*q2)*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 + (16*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 - ch2t)*sh2t)/q3 - (16*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*hib*kppj^2*1/4*e8q4*(e6q4 + ch2t)*sh2t)/q3 - (4*Lq^2*dt^2*hia*hib*(hom^2 + q3)*sht*cht*e2q4)/(Ld^2*q2) - (4*Ld^2*dt^2*hia*hib*(hom^2 + q3)*sht*cht*e2q4)/(Lq^2*q2) + (12*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q1) - (8*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld*q2) + (12*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q1) - (8*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq*q2) - (2*Ld*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q1) + (4*Ld*dt^2*hib*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Lq^2*q1) + (4*Ld*dt^2*hia*(hom^2 + q3)*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Lq^2*q2) + (2*Ld*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*hom*1/8*(3 + 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (2*Ld*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q1) + (8*Ld*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Lq^2*q2) + (8*Lq*Rs*dt^2*hia*hib*hom*1/2*(1 - e2q4*ch2t))/(Ld^2*q2) - (8*Ld*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Lq^2*q2) - (8*Lq*Rs*dt^2*hia*hib*hom*1/8*(3 - 4*e2q4*ch2t + e8q4*ch4t))/(Ld^2*q2) + (10*Ld*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 + (10*Ld*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (10*Lq*dt^2*hia*(hib^2 + q2)*kppj^2*psi*1/4*e92q4*(e4q4*cht - ch3t))/q3 - (10*Lq*dt^2*(hia^2 + q1)*hib*kppj^2*psi*1/4*e92q4*(e4q4*sht + sh3t))/q3 - (2*Rs*dt^2*hia*hom*psi*sht*e05q4)/(Ld*Lq*q2) - (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q1) + (8*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 + ch2t)*sh2t)/(Ld*Lq*q1) + (16*Rs^2*dt^2*hia*hib*1/4*e8q4*(e6q4 - ch2t)*sh2t)/(Ld*Lq*q2) + (4*Rs*dt^2*hia*hom*psi*1/4*e92q4*(e4q4*sht + sh3t))/(Ld*Lq*q1) + (2*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q1) - (4*Rs*dt^2*hib*hom*psi*1/4*e92q4*(e4q4*cht - ch3t))/(Ld*Lq*q2) - (12*Ld*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q1) - (12*Lq*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q1) + (8*Ld*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Lq^2*q2) + (8*Lq*Rs*dt^2*hia*hib*hom*1/8*(1 - e8q4*ch4t))/(Ld^2*q2); 
+        
+        
+        D12 = -A70'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;
+
+        Jj10 = D22 - D21/(Jj10 + D11)*D12;
+        iJn10(:,:,t) = inv(Jj10);
+    end
+    if(model11 == 1) %d-q/E/Ldq
+        A81(1,2) = Lq*dt/Ld*ome;
+        A81(1,3) = Lq*dt/Ld*iq;
+        A81(2,1) = -Ld*dt/Lq*ome;
+        A81(2,3) = -Ld*dt/Lq*id-psipm*dt/Lq;
+        A81(3,1) = kp*pp*pp*dt*(Ld-Lq)/J*iq;
+        A81(3,2) = kp*pp*pp*dt/J*((Ld-Lq)*id+psipm);    
+        
+        D11 = A81'*iQ4*A81 + D11_cor11;
+        D12 = -A81'*iQ4;
+        D21 = D12';
+        D22 = iQ4 + C4'*iR*C4;
+    
+        Jj11 = D22 - D21/(Jj11 + D11)*D12;
+        iJn11(:,:,t) = inv(Jj11);
+    end
+    if(model12 == 1) %5sc/E/Ldq
+        A92(1, 1) = 1.0 + dt*(-Rs*(1/2*(1 - e2q4*ch2t)/Lq + 1/2*(1 + e2q4*ch2t)/Ld) + hom*sht*cht*e2q4*(Ld/Lq - Lq/Ld));
+        A92(1, 2) = dt*(-hom + Rs*sht*cht*e2q4*(1/Lq - 1/Ld) + hom*(Ld*1/2*(1 - e2q4*ch2t)/Lq + Lq*1/2*(1 + e2q4*ch2t)/Ld));
+        A92(1, 3) = dt*psi*sht*e05q4/Lq + dt*hia*sht*cht*e2q4*(Ld/Lq - Lq/Ld) + dt*hib*(-1 + Ld/Lq*1/2*(1 - e2q4*ch2t) + Lq/Ld*1/2*(1 + e2q4*ch2t));
+        A92(1, 4) = dt*psi*hom/Lq + dt*hia*(-Rs*2*sht*e05q4/Lq + hom*cht*e05q4*(Ld/Lq - Lq/Ld)) + dt*hib*(Rs*cht*e05q4*(1/Lq - 1/Ld) + hom*2*Ld*sht*e05q4/Lq);
+        A92(1, 5) = dt*hia*(-Rs*2*cht*e05q4/Ld + hom*sht*e05q4*(Ld/Lq - Lq/Ld)) + dt*hib*(Rs*sht*e05q4*(1/Lq - 1/Ld) + hom*2*Lq*cht*e05q4/Ld);
+
+        A92(2, 1) = dt*(hom + Rs*sht*cht*e2q4*(1/Lq - 1/Ld) - hom*(Lq*1/2*(1 - e2q4*ch2t)/Ld + Ld*1/2*(1 + e2q4*ch2t)/Lq));
+        A92(2, 2) = 1 + dt*(-Rs*(1/2*(1 - e2q4*ch2t)/Ld + 1/2*(1 + e2q4*ch2t)/Lq) + hom*sht*cht*e2q4*(Lq/Ld - Ld/Lq));
+        A92(2, 3) = -dt*psi*cht*e05q4/Lq + dt*hia*(1 - Lq*1/2*(1 - e2q4*ch2t)/Ld - Ld*1/2*(1 + e2q4*ch2t)/Lq) + dt*hib*sht*cht*e2q4*(Lq/Ld - Ld/Lq);
+        A92(2, 4) = dt*hia*(Rs*cht*e05q4*(1/Lq - 1/Ld) - hom*2*Lq*sht*e05q4/Ld) + dt*hib*(-Rs*2*sht*e05q4/Ld + hom*cht*e05q4*(Lq/Ld - Ld/Lq));
+        A92(2, 5) = -dt*psi*hom/Lq + dt*hia*(Rs*sht*e05q4*(1/Lq - 1/Ld) -hom*2*Ld*cht*e05q4/Lq) + dt*hib*(-Rs*2*cht*e05q4/Lq + hom*sht*e05q4*(Lq/Ld - Ld/Lq));
+
+        A92(3, 1) = dt*kpp/J*((Ld - Lq)*(-2*hia*sht*cht*e2q4 + hib*1/2*(1 + e2q4*ch2t) - hib*1/2*(1 - e2q4*ch2t)) - psi*sht*e05q4);
+        A92(3, 2) = dt*kpp/J*((Ld - Lq)*(hia*1/2*(1 + e2q4*ch2t) - hia*1/2*(1 - e2q4*ch2t) + 2*hib*sht*cht*e2q4) + psi*cht*e05q4);
+        A92(3, 3) = 1;
+        A92(3, 4) = dt*kpp/J*((Ld - Lq)*(-(hia^2 + q1)*cht*e05q4 - hia*hib*2*sht*e05q4 + (hib^2 + q2)*cht*e05q4) - psi*hia);
+        A92(3, 5) = dt*kpp/J*((Ld - Lq)*(-(hia^2 + q1)*sht*e05q4 + hia*hib*2*cht*e05q4 + (hib^2 + q2)*sht*e05q4) + psi*hib);
+
+        A92(4,3) = dt*cht*e05q4;
+        A92(4,5) = dt*hom;
+        A92(5,3) = -dt*sht*e05q4;
+        A92(5,4) = -dt*hom;
+        
+        D11 = zeros(5);
+        D11(1,1) = 1/q1 + (dt^2*(hom^2 + q3))/q2 + (Rs^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2)/(Ld^2*q1) + (Rs^2*dt^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) - (2*Rs*(cht^2 + q5)*dt)/(Ld*q1) - (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*Rs*dt*(sht^2 + q4))/(Lq*q1) + (dt^2*kpp^2*psi^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*(cht^2 + q5)*dt^2*(hom^2 + q3))/(Lq*q2) - (2*Lq*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Ld*q2) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hom^2 + q3))/(Lq^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Lq^2*q2) + (Lq^2*dt^2*(hom^2 + q3)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*Rs*cht*dt^2*hom*sht)/(Ld*q2) + (2*Rs*cht*dt^2*hom*sht)/(Lq*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld*q1) - (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld*q2) - (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq*q1) + (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq*q2) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Ld^2*dt^2*(hib^2 + q2)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*dt^2*(hib^2 + q2)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*cht*dt*hom*sht)/(Lq*q1) - (2*Lq*cht*dt*hom*sht)/(Ld*q1) + (2*Ld*dt^2*hib*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Lq*dt^2*hib*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2)*kpp^2)/(J^2*q3) - (2*Ld*Lq*dt^2*(hib^2 + q2)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) - (2*Ld*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld^2*q1) - (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq^2*q2) + (2*Lq*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (4*Ld^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (4*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) + (4*Lq^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (4*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) - (8*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Ld*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (2*Ld*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) - (4*Lq*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (2*Lq*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) - (8*Ld*Lq*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (8*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3);
+        D11(1,2) = (dt*hom)/q2 - (dt*hom)/q1 - (cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q1 + ((cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/q1 + (cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q2 - ((cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/q2 + (Rs*(cht^2 + q5)*dt^2*hom)/(Ld*q1) - (Rs*(cht^2 + q5)*dt^2*hom)/(Lq*q2) - (Rs*dt^2*hom*(sht^2 + q4))/(Ld*q2) + (Rs*dt^2*hom*(sht^2 + q4))/(Lq*q1) - (Rs*cht*dt*sht)/(Ld*q1) - (Rs*cht*dt*sht)/(Ld*q2) + (Rs*cht*dt*sht)/(Lq*q1) + (Rs*cht*dt*sht)/(Lq*q2) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Lq^2*q2) + (Lq*(cht^2 + q5)*dt*hom)/(Ld*q1) - (Ld*(cht^2 + q5)*dt*hom)/(Lq*q2) + (Ld*dt*hom*(sht^2 + q4))/(Lq*q1) - (Lq*dt*hom*(sht^2 + q4))/(Ld*q2) - (cht*dt^2*kpp^2*psi^2*sht)/(J^2*q3) + (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Ld*Lq*q1) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Ld*Lq*q1) - (Rs^2*cht*dt^2*(sht^3 + 3*sht*q4))/(Ld*Lq*q2) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*sht)/(Ld*Lq*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hom^2 + q3)*sht)/(Lq^2*q2) - (Lq*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hom)/(Ld^2*q1) + (Ld*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hom)/(Lq^2*q2) - (Ld*Rs*dt^2*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + (Lq*Rs*dt^2*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (Ld*cht*dt^2*(hom^2 + q3)*sht)/(Lq*q1) + (Lq*cht*dt^2*(hom^2 + q3)*sht)/(Ld*q1) - (Ld*cht*dt^2*(hom^2 + q3)*sht)/(Lq*q2) + (Lq*cht*dt^2*(hom^2 + q3)*sht)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld*q1) + (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq*q1) + (2*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq*q2) + (2*Ld^2*cht*dt^2*(hia^2 + q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*kpp^2*sht)/(J^2*q3) - (2*Ld^2*cht*dt^2*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (2*Lq^2*cht*dt^2*(hia^2 + q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*kpp^2*sht)/(J^2*q3) - (2*Lq^2*cht*dt^2*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (Ld*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld*(cht^3 + 3*cht*q5)*dt^2*hib*kpp^2*psi)/(J^2*q3) - (Lq*(cht^3 + 3*cht*q5)*dt^2*hib*kpp^2*psi)/(J^2*q3) + (Ld*dt^2*hia*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) - (Lq*dt^2*hia*kpp^2*psi*(sht^3 + 3*sht*q4))/(J^2*q3) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hib*kpp^2)/(J^2*q3) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hib*kpp^2)/(J^2*q3) + (Ld^2*dt^2*hia*hib*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*dt^2*hia*hib*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) - (4*Ld*Lq*cht*dt^2*(hia^2 + q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*kpp^2*sht)/(J^2*q3) + (4*Ld*Lq*cht*dt^2*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (6*Ld^2*(cht^2 + q5)*dt^2*hia*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (6*Lq^2*(cht^2 + q5)*dt^2*hia*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hib*kpp^2)/(J^2*q3) - (2*Ld*Lq*dt^2*hia*hib*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) - (3*Ld*(cht^2 + q5)*dt^2*hia*kpp^2*psi*sht)/(J^2*q3) - (3*Ld*cht*dt^2*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (3*Lq*(cht^2 + q5)*dt^2*hia*kpp^2*psi*sht)/(J^2*q3) + (3*Lq*cht*dt^2*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (12*Ld*Lq*(cht^2 + q5)*dt^2*hia*hib*kpp^2*(sht^2 + q4))/(J^2*q3);
+        D11(1,3) = (dt^2*hia*hom)/q2 - (dt*hib)/q1 + (dt*psi*sht)/(Lq*q1) - (cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/q1 + ((cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/q1 + (cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/q2 - ((cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/q2 + (Rs*(cht^2 + q5)*dt^2*hib)/(Ld*q1) + (Rs*dt^2*hib*(sht^2 + q4))/(Lq*q1) - (Rs*dt^2*psi*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/q2 - (dt*kpp*psi*sht)/(J*q3) + (Lq*(cht^2 + q5)*dt*hib)/(Ld*q1) + (Ld*dt*hib*(sht^2 + q4))/(Lq*q1) - (cht*dt^2*hom*psi)/(Lq*q2) + (Ld*(cht^2 + q5)*dt*hib*kpp)/(J*q3) - (Lq*(cht^2 + q5)*dt*hib*kpp)/(J*q3) - (Ld*dt*hib*kpp*(sht^2 + q4))/(J*q3) + (Lq*dt*hib*kpp*(sht^2 + q4))/(J*q3) - (Rs*cht*dt^2*hia*sht)/(Ld*q2) + (Rs*cht*dt^2*hia*sht)/(Lq*q2) - (Lq*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hib)/(Ld^2*q1) - (Ld*Rs*dt^2*hib*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) - (2*Ld*(cht^2 + q5)*dt^2*hia*hom)/(Lq*q2) + (Ld*(cht^3 + 3*cht*q5)*dt^2*hom*psi)/(Lq^2*q2) + (Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld*q1) - (Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld*q2) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Lq*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Lq*q2) - (2*Lq*dt^2*hia*hom*(sht^2 + q4))/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*psi*sht)/(Lq^2*q2) - (cht*dt^2*hom*psi*(sht^2 + q4))/(Ld*q1) + (cht*dt^2*hom*psi*(sht^2 + q4))/(Ld*q2) + (Ld*cht*dt*hia*sht)/(Lq*q1) - (Lq*cht*dt*hia*sht)/(Ld*q1) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia*hom)/(Lq^2*q2) - (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Lq*q2) + (Lq^2*dt^2*hia*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) + (Ld*cht*dt^2*hom*psi*(sht^2 + q4))/(Lq^2*q1) - (2*Ld*cht*dt*hia*kpp*sht)/(J*q3) + (2*Lq*cht*dt*hia*kpp*sht)/(J*q3) - (Ld*Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hib*(sht^2 + q4))/(Ld^2*q2) - (Rs*(cht^2 + q5)*dt^2*psi*sht)/(Ld*Lq*q1) + (Rs*(cht^2 + q5)*dt^2*psi*sht)/(Ld*Lq*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hib*hom*sht)/(Lq^2*q2) - (Ld*cht*dt^2*hib*hom*sht)/(Lq*q1) + (Lq*cht*dt^2*hib*hom*sht)/(Ld*q1) - (Ld*cht*dt^2*hib*hom*sht)/(Lq*q2) + (Lq*cht*dt^2*hib*hom*sht)/(Ld*q2) + (Lq^2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q1) - (Ld*Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Ld^2*q1) - (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*sht)/(Lq^2*q2) + (Lq*Rs*cht*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld^2*q2);
+        D11(1,4) = (dt*hom*psi)/(Lq*q1) - ((cht^3 + 3*cht*q5)*dt^2*hib*(hom^2 + q3))/q2 - (Rs*cht*dt*hib)/(Ld*q1) + (Rs*cht*dt*hib)/(Lq*q1) - (2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q1 - (2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q2 + (cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*Rs*dt*hia*sht)/(Lq*q1) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hib)/(Ld^2*q1) + (2*Rs^2*dt^2*hia*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (dt^2*hia*kpp^2*psi^2*sht)/(J^2*q3) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hib)/(Ld*Lq*q1) - (Rs*cht*dt^2*hia*hom)/(Ld*q2) + (Rs*cht*dt^2*hia*hom)/(Lq*q2) - (2*Rs*dt^2*hib*hom*sht)/(Ld*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hib*(hom^2 + q3))/(Lq^2*q2) + (2*Lq^2*dt^2*hia*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (Ld*cht*dt^2*hib*(hom^2 + q3))/(Lq*q2) + (Lq*cht*dt^2*hib*(hom^2 + q3))/(Ld*q2) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq*q2) - (2*Lq*dt^2*hia*(hom^2 + q3)*sht)/(Ld*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) - (Rs*dt^2*hom*psi*(sht^2 + q4))/(Lq^2*q1) - (cht*dt^2*(hom^2 + q3)*psi*sht)/(Ld*q1) + (Ld*cht*dt*hia*hom)/(Lq*q1) - (Lq*cht*dt*hia*hom)/(Ld*q1) + (2*Ld*dt*hib*hom*sht)/(Lq*q1) + (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld^2*q2) + (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld^2*q2) - (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Lq^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Lq^2*q2) + (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q1) - (Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q1) - (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q2) + (Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q2) - (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq*q1) + (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq*q2) + (Ld*cht*dt^2*(hom^2 + q3)*psi*sht)/(Lq^2*q1) - (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) + (2*Ld^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) - (Ld^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) - (Lq^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (Rs*(cht^2 + q5)*dt^2*hom*psi)/(Ld*Lq*q1) - (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q1) + (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q2) - (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q2) + (Lq^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Lq^2*q1) + (2*Ld^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Ld^2*q1) - (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) + (2*Ld*Lq*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (3*Ld*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q1) + (Ld*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld^2*q1) + (3*Lq*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q2) - (Ld*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld^2*q2) + (5*Ld^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (5*Lq^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Lq^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (Ld*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) + (Lq*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) + (3*Ld*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) - (Ld*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) - (3*Lq*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) + (Lq*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) + (3*Ld*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (3*Lq*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) - (10*Ld*Lq*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (8*Ld*Lq*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3);
+        D11(1,5) = (dt^2*hib*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q2 - (dt^2*(hom^2 + q3)*psi)/(Lq*q2) - (2*Rs*cht*dt*hia)/(Ld*q1) - (2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q1 + (2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q2 - ((cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q2 - (Rs*dt*hib*sht)/(Ld*q1) + (Rs*dt*hib*sht)/(Lq*q1) + (2*Rs^2*(cht^3 + 3*cht*q5)*dt^2*hia)/(Ld^2*q1) - (Rs^2*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (dt^2*(hom^2 + q3)*psi*(sht^2 + q4))/(Ld*q2) - (dt^2*hib*kpp^2*psi^2*sht)/(J^2*q3) - (2*Rs*cht*dt^2*hib*hom)/(Lq*q2) + (Rs^2*dt^2*hib*(sht^3 + 3*sht*q4))/(Ld*Lq*q1) - (Rs*dt^2*hia*hom*sht)/(Ld*q2) + (Rs*dt^2*hia*hom*sht)/(Lq*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hom^2 + q3))/(Lq^2*q2) - (Lq^2*dt^2*hib*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (2*Ld*cht*dt^2*hia*(hom^2 + q3))/(Lq*q2) - (Ld*dt^2*hib*(hom^2 + q3)*sht)/(Lq*q2) + (Lq*dt^2*hib*(hom^2 + q3)*sht)/(Ld*q2) + (Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld*q1) - (Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld*q2) - (Ld^2*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (Lq^2*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq*cht*dt*hib*hom)/(Ld*q1) + (Ld*dt*hia*hom*sht)/(Lq*q1) - (Lq*dt*hia*hom*sht)/(Ld*q1) + (Ld*(cht^2 + q5)*dt^2*(hom^2 + q3)*psi)/(Lq^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld^2*q2) + (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Lq^2*q2) - (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Lq^2*q2) - (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld*q1) + (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld*q2) - (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q1) - (Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q1) + (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q2) + (Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (2*Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Ld^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (2*Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (Ld^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Lq^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Ld*Lq*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q1) - (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q2) - (Rs*cht*dt^2*hom*psi*sht)/(Lq^2*q2) + (Ld*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) - (Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (Lq^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (2*Lq^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Lq^2*q2) + (Ld*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (2*Ld*dt^2*(hib^2 + q2)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (Lq*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (2*Lq*dt^2*(hib^2 + q2)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Lq^2*q2) - (Ld*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (4*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (3*Lq*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld^2*q1) + (Ld*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q1) - (3*Ld*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq^2*q2) - (Ld*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q2) + (Rs*cht*dt^2*hom*psi*sht)/(Ld*Lq*q2) - (4*Ld^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (5*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (5*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (10*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Ld*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3) + (4*Lq*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3);
+
+        D11(2,1) = D11(1,2);
+        D11(2,2) = 1/q2 + (dt^2*(hom^2 + q3))/q1 + (Rs^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2)/(Lq^2*q2) + (Rs^2*dt^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*Rs*(cht^2 + q5)*dt)/(Lq*q2) + (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q1 - (2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*Rs*dt*(sht^2 + q4))/(Ld*q2) - (2*Lq*(cht^2 + q5)*dt^2*(hom^2 + q3))/(Ld*q1) - (2*Ld*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Lq*q1) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hom^2 + q3))/(Ld^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Lq^2*q1) + (Ld^2*dt^2*(hom^2 + q3)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + ((cht^2 + q5)*dt^2*kpp^2*psi^2)/(J^2*q3) + (2*Rs*cht*dt^2*hom*sht)/(Ld*q1) - (2*Rs*cht*dt^2*hom*sht)/(Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q1) + (2*Rs^2*(cht^2 + q5)*dt^2*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld*q1) - (2*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld*q2) - (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq*q1) + (2*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq*q2) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1)*kpp^2)/(J^2*q3) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1)*kpp^2)/(J^2*q3) + (Ld^2*dt^2*(hia^2 + q1)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*dt^2*(hia^2 + q1)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q2) - (2*Ld*cht*dt*hom*sht)/(Lq*q2) + (2*Lq*cht*dt*hom*sht)/(Ld*q2) + (2*Ld*(cht^3 + 3*cht*q5)*dt^2*hia*kpp^2*psi)/(J^2*q3) - (2*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*kpp^2*psi)/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1)*kpp^2)/(J^2*q3) - (2*Ld*Lq*dt^2*(hia^2 + q1)*kpp^2*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(J^2*q3) + (2*Ld*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hom*sht)/(Lq^2*q2) - (2*Lq*Rs*cht*dt^2*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (4*Ld^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (4*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (8*Ld*Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (4*Ld*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) + (2*Lq*cht*dt^2*hia*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (4*Lq*(cht^2 + q5)*dt^2*hib*kpp^2*psi*sht)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*hia*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (8*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*hib*kpp^2*sht)/(J^2*q3);
+        D11(2,3) = (dt*hia)/q2 + (dt^2*hib*hom)/q1 - (cht*dt*psi)/(Lq*q2) - (cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/q1 + ((cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/q1 + (cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/q2 - ((cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/q2 - (Rs*(cht^2 + q5)*dt^2*hia)/(Lq*q2) + (Rs*(cht^3 + 3*cht*q5)*dt^2*psi)/(Lq^2*q2) - (Rs*dt^2*hia*(sht^2 + q4))/(Ld*q2) + (2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/q1 - (2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/q2 + (cht*dt*kpp*psi)/(J*q3) - (Ld*(cht^2 + q5)*dt*hia)/(Lq*q2) - (Lq*dt*hia*(sht^2 + q4))/(Ld*q2) - (dt^2*hom*psi*sht)/(Lq*q1) + (Ld*(cht^2 + q5)*dt*hia*kpp)/(J*q3) - (Lq*(cht^2 + q5)*dt*hia*kpp)/(J*q3) - (Ld*dt*hia*kpp*(sht^2 + q4))/(J*q3) + (Lq*dt*hia*kpp*(sht^2 + q4))/(J*q3) + (Rs*cht*dt^2*hib*sht)/(Ld*q1) - (Rs*cht*dt^2*hib*sht)/(Lq*q1) + (Ld*Rs*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hia)/(Lq^2*q2) + (Lq*Rs*dt^2*hia*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*Lq*(cht^2 + q5)*dt^2*hib*hom)/(Ld*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Ld*q1) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Ld*q2) - (Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq*q1) + (Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq*q2) - (2*Ld*dt^2*hib*hom*(sht^2 + q4))/(Lq*q1) + (Rs*cht*dt^2*psi*(sht^2 + q4))/(Lq^2*q1) + (Ld*dt^2*hom*psi*(sht^3 + 3*sht*q4))/(Lq^2*q1) + ((cht^2 + q5)*dt^2*hom*psi*sht)/(Ld*q1) - ((cht^2 + q5)*dt^2*hom*psi*sht)/(Ld*q2) - (Ld*cht*dt*hib*sht)/(Lq*q2) + (Lq*cht*dt*hib*sht)/(Ld*q2) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*hib*hom)/(Ld^2*q1) - (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Lq*q2) + (Ld^2*dt^2*hib*hom*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + (Ld*(cht^2 + q5)*dt^2*hom*psi*sht)/(Lq^2*q2) + (2*Ld*cht*dt*hib*kpp*sht)/(J*q3) - (2*Lq*cht*dt*hib*kpp*sht)/(J*q3) + (Ld*Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hia*(sht^2 + q4))/(Ld^2*q1) - (Rs*cht*dt^2*psi*(sht^2 + q4))/(Ld*Lq*q1) + (Rs*cht*dt^2*psi*(sht^2 + q4))/(Ld*Lq*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*cht*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hom*sht)/(Lq^2*q2) - (Ld*cht*dt^2*hia*hom*sht)/(Lq*q1) + (Lq*cht*dt^2*hia*hom*sht)/(Ld*q1) - (Ld*cht*dt^2*hia*hom*sht)/(Lq*q2) + (Lq*cht*dt^2*hia*hom*sht)/(Ld*q2) + (Lq^2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q2) + (Ld*Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Ld^2*q1) + (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*sht)/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2);
+        D11(2,4) = ((cht^3 + 3*cht*q5)*dt^2*hia*(hom^2 + q3))/q1 - (dt^2*(hom^2 + q3)*psi)/(Lq*q1) - (Rs*cht*dt*hia)/(Ld*q2) + (Rs*cht*dt*hia)/(Lq*q2) - (cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q1 + (2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/q2 - (2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/q2 - (2*Rs*dt*hib*sht)/(Ld*q2) - (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hia)/(Lq^2*q2) + (2*Rs^2*dt^2*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2) + ((cht^2 + q5)*dt^2*(hom^2 + q3)*psi)/(Ld*q1) - (cht*dt^2*hia*kpp^2*psi^2)/(J^2*q3) + (Rs^2*(cht^3 + 3*cht*q5)*dt^2*hia)/(Ld*Lq*q2) + (Rs*cht*dt^2*hib*hom)/(Ld*q1) - (Rs*cht*dt^2*hib*hom)/(Lq*q1) + (2*Rs*dt^2*hia*hom*sht)/(Lq*q1) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hom^2 + q3))/(Ld^2*q1) + (2*Ld^2*dt^2*hib*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Ld*cht*dt^2*hia*(hom^2 + q3))/(Lq*q1) + (Lq*cht*dt^2*hia*(hom^2 + q3))/(Ld*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld*q1) - (Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld*q2) - (2*Ld*dt^2*hib*(hom^2 + q3)*sht)/(Lq*q1) - (Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2)/(J^2*q3) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2)/(J^2*q3) - (Ld*cht*dt*hib*hom)/(Lq*q2) + (Lq*cht*dt*hib*hom)/(Ld*q2) - (2*Lq*dt*hia*hom*sht)/(Ld*q2) + (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld^2*q2) - (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Lq^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Lq^2*q1) + (Ld*dt^2*(hom^2 + q3)*psi*(sht^2 + q4))/(Lq^2*q1) - (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld*q1) + (3*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q1) - (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq*q2) + (3*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq*q2) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq^2*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (2*Ld^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Lq^2*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2)/(J^2*q3) + (2*Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q1) - (Rs^2*cht*dt^2*hia*(sht^2 + q4))/(Ld*Lq*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hib*sht)/(Ld*Lq*q2) + (Rs*cht*dt^2*hom*psi*sht)/(Lq^2*q1) - (2*Ld*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*psi)/(J^2*q3) + (Ld*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (2*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*kpp^2*psi)/(J^2*q3) - (Lq*(cht^2 + q5)*dt^2*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (Ld^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (2*Lq^2*cht*dt^2*hia*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Lq^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*hib*(hom^2 + q3)*sht)/(Lq^2*q2) + (Ld*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (Lq*dt^2*(hia^2 + q1)*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Ld^2*q1) + (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hib*hom)/(Lq^2*q2) - (2*Ld*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (2*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*(cht^2 + q5)*dt^2*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (4*Ld*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Ld*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld^2*q1) + (3*Ld*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Lq^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hia*hom*sht)/(Ld^2*q2) - (3*Lq*Rs*cht*dt^2*hib*hom*(sht^2 + q4))/(Ld^2*q2) - (Rs*cht*dt^2*hom*psi*sht)/(Ld*Lq*q1) - (5*Ld^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (5*Lq^2*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*hia*(hib^2 + q2)*kpp^2)/(J^2*q3) + (10*Ld*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (8*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Ld*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3) + (4*Lq*cht*dt^2*hia*hib*kpp^2*psi*sht)/(J^2*q3);
+        D11(2,5) = ((cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q1 - (dt^2*hia*(hom^2 + q3)*(sht^3 + 3*sht*q4))/q1 - (2*Rs*cht*dt*hib)/(Lq*q2) - (dt*hom*psi)/(Lq*q2) + (2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q1 - (2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/q2 - (2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/q2 - (Rs*dt*hia*sht)/(Ld*q2) + (Rs*dt*hia*sht)/(Lq*q2) + (2*Rs^2*(cht^3 + 3*cht*q5)*dt^2*hib)/(Lq^2*q2) + (Rs^2*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (cht*dt^2*hib*kpp^2*psi^2)/(J^2*q3) + (2*Rs*cht*dt^2*hia*hom)/(Ld*q1) - (Rs^2*dt^2*hia*(sht^3 + 3*sht*q4))/(Ld*Lq*q2) + (Rs*dt^2*hib*hom*sht)/(Ld*q1) - (Rs*dt^2*hib*hom*sht)/(Lq*q1) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hib*(hom^2 + q3))/(Ld^2*q1) + (Ld^2*dt^2*hia*(hom^2 + q3)*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*Lq*cht*dt^2*hib*(hom^2 + q3))/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hom*psi)/(Lq^2*q2) - (Ld*dt^2*hia*(hom^2 + q3)*sht)/(Lq*q1) + (Lq*dt^2*hia*(hom^2 + q3)*sht)/(Ld*q1) - (Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq*q1) + (Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq*q2) + (Ld^2*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Lq^2*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (cht*dt^2*(hom^2 + q3)*psi*sht)/(Ld*q2) - (2*Ld*cht*dt*hia*hom)/(Lq*q2) - (Ld*dt*hib*hom*sht)/(Lq*q2) + (Lq*dt*hib*hom*sht)/(Ld*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld^2*q1) + (Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Lq^2*q1) - (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Lq^2*q2) - (Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q1) + (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q1) + (Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld*q2) - (3*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld*q2) - (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq*q1) + (3*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq*q2) + (Ld*cht*dt^2*(hom^2 + q3)*psi*sht)/(Lq^2*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) - (Ld^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) + (2*Ld^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (Lq^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) + (2*Lq^2*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (Ld^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (Lq^2*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Rs*dt^2*hom*psi*(sht^2 + q4))/(Ld*Lq*q2) - (2*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q1) + (Rs^2*(cht^2 + q5)*dt^2*hia*sht)/(Ld*Lq*q2) + (2*Rs^2*cht*dt^2*hib*(sht^2 + q4))/(Ld*Lq*q2) - (Lq^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Ld^2*q1) + (2*Ld^2*(cht^2 + q5)*dt^2*hia*(hom^2 + q3)*sht)/(Lq^2*q2) + (Lq^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*cht*dt^2*hib*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q2) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hom)/(Lq^2*q2) + (Ld*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq*Rs*dt^2*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (2*Ld*Lq*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*kpp^2*sht)/(J^2*q3) - (4*Ld*Lq*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld*Lq*dt^2*hia*(hib^2 + q2)*kpp^2*(sht^3 + 3*sht*q4))/(J^2*q3) + (Ld*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q1) - (3*Lq*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Ld^2*q1) - (Ld*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hia*hom*(sht^2 + q4))/(Ld^2*q2) + (3*Ld*Rs*(cht^2 + q5)*dt^2*hib*hom*sht)/(Lq^2*q2) - (4*Ld^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (5*Ld^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (5*Lq^2*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) + (3*Ld*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) - (3*Lq*(cht^2 + q5)*dt^2*hia*hib*kpp^2*psi)/(J^2*q3) - (Ld*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) + (3*Ld*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) + (Lq*cht*dt^2*(hia^2 + q1)*kpp^2*psi*sht)/(J^2*q3) - (3*Lq*cht*dt^2*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) - (Ld*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) + (Lq*dt^2*hia*hib*kpp^2*psi*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hib*kpp^2)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*(hia^2 + q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (10*Ld*Lq*(cht^2 + q5)*dt^2*hia*(hib^2 + q2)*kpp^2*sht)/(J^2*q3);
+
+        D11(3,1) = D11(1,3);
+        D11(3,2) = D11(2,3);
+        D11(3,3) = 1/q3 + ((cht^2 + q5)*dt^2)/q4 + (dt^2*(hib^2 + q2))/q1 + (dt^2*(hia^2 + q1))/q2 + (dt^2*(sht^2 + q4))/q5 + ((cht^2 + q5)*dt^2*psi^2)/(Lq^2*q2) + (dt^2*psi^2*(sht^2 + q4))/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/q1 + (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/q2 - (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/q2 - (2*Lq*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Ld*q1) - (2*Ld*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Lq*q2) - (2*Ld*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq*q1) - (2*Lq*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*q2) + (Lq^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hib^2 + q2))/(Ld^2*q1) + (Ld^2*(cht^4 + 6*cht^2*q5 + 3*q5^2)*dt^2*(hia^2 + q1))/(Lq^2*q2) - (2*cht*dt^2*hia*psi)/(Lq*q2) + (Ld^2*dt^2*(hib^2 + q2)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Lq^2*q1) + (Lq^2*dt^2*(hia^2 + q1)*(sht^4 + 6*sht^2*q4 + 3*q4^2))/(Ld^2*q2) - (2*dt^2*hib*psi*sht)/(Lq*q1) - (2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/q1 + (2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/q1 + (2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/q2 - (2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/q2 + (2*Ld*(cht^3 + 3*cht*q5)*dt^2*hia*psi)/(Lq^2*q2) + (2*Ld*dt^2*hib*psi*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq^2*q2) - (2*cht*dt^2*hia*psi*(sht^2 + q4))/(Ld*q1) + (2*(cht^2 + q5)*dt^2*hib*psi*sht)/(Ld*q1) + (2*cht*dt^2*hia*psi*(sht^2 + q4))/(Ld*q2) - (2*(cht^2 + q5)*dt^2*hib*psi*sht)/(Ld*q2) - (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/(Ld^2*q1) + (2*Ld^2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (2*Lq^2*cht*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*sht)/(Lq^2*q2) - (2*Ld*cht*dt^2*hia*hib*sht)/(Lq*q1) + (2*Lq*cht*dt^2*hia*hib*sht)/(Ld*q1) - (2*Ld*cht*dt^2*hia*hib*sht)/(Lq*q2) + (2*Lq*cht*dt^2*hia*hib*sht)/(Ld*q2) + (2*Ld*cht*dt^2*hia*psi*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*(cht^2 + q5)*dt^2*hib*psi*sht)/(Lq^2*q2);
+        D11(3,4) = (cht*dt)/q4 + (dt^2*hom*sht)/q5 + (Rs*cht*dt^2*(hib^2 + q2))/(Ld*q1) - (Rs*cht*dt^2*(hia^2 + q1))/(Ld*q2) - (Rs*cht*dt^2*(hib^2 + q2))/(Lq*q1) + (Rs*cht*dt^2*(hia^2 + q1))/(Lq*q2) + (dt^2*hom*psi^2*sht)/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/q1 + (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/q1 + (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/q2 - (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/q2 + (Rs*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2))/(Ld*q1) + (Rs*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1))/(Lq*q2) - (dt*hia*kpp*psi)/(J*q3) - (dt^2*hib*hom*psi)/(Lq*q1) + ((cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/q1 - ((cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/q2 - (Ld*cht*dt*(hia^2 + q1)*kpp)/(J*q3) + (Ld*cht*dt*(hib^2 + q2)*kpp)/(J*q3) + (Lq*cht*dt*(hia^2 + q1)*kpp)/(J*q3) - (Lq*cht*dt*(hib^2 + q2)*kpp)/(J*q3) - (2*Rs*dt^2*hia*hib*sht)/(Ld*q2) + (2*Rs*dt^2*hia*hib*sht)/(Lq*q1) + (2*Ld^2*dt^2*(hib^2 + q2)*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq^2*dt^2*(hia^2 + q1)*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (3*cht*dt^2*hia*hib*hom*(sht^2 + q4))/q1 + (3*cht*dt^2*hia*hib*hom*(sht^2 + q4))/q2 - (Rs*(cht^2 + q5)*dt^2*hia*psi)/(Lq^2*q2) - (2*Ld*dt^2*(hib^2 + q2)*hom*sht)/(Lq*q1) - (2*Lq*dt^2*(hia^2 + q1)*hom*sht)/(Ld*q2) - (2*Rs*dt^2*hia*psi*(sht^2 + q4))/(Lq^2*q1) + ((cht^2 + q5)*dt^2*hib*hom*psi)/(Ld*q1) - ((cht^2 + q5)*dt^2*hib*hom*psi)/(Ld*q2) - (Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2))/(Ld^2*q1) - (Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1))/(Lq^2*q2) + (2*Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*q1) - (Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*q2) - (Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq*q1) + (2*Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq*q2) + (3*Ld*dt^2*hib*hom*psi*(sht^2 + q4))/(Lq^2*q1) + (Rs*(cht^2 + q5)*dt^2*hia*psi)/(Ld*Lq*q2) - (2*Ld*dt*hia*hib*kpp*sht)/(J*q3) + (2*Lq*dt*hia*hib*kpp*sht)/(J*q3) - (2*Ld*Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q1) + (Ld*Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld^2*q2) - (2*Lq*Rs*cht*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q2) - (Lq^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/(Ld^2*q1) + (Ld^2*(cht^3 + 3*cht*q5)*dt^2*hia*hib*hom)/(Lq^2*q2) - (Ld*cht*dt^2*hia*hib*hom)/(Lq*q1) + (Lq*cht*dt^2*hia*hib*hom)/(Ld*q1) - (Ld*cht*dt^2*hia*hib*hom)/(Lq*q2) + (Lq*cht*dt^2*hia*hib*hom)/(Ld*q2) + (Rs*cht*dt^2*hib*psi*sht)/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom*sht)/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom*sht)/(Lq^2*q2) - (2*cht*dt^2*hia*hom*psi*sht)/(Ld*q1) + (2*cht*dt^2*hia*hom*psi*sht)/(Ld*q2) - (2*Ld*Rs*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Ld*(cht^2 + q5)*dt^2*hib*hom*psi)/(Lq^2*q2) - (3*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld*q1) + (Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld*q2) - (Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq*q1) + (3*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq*q2) + (2*Ld*cht*dt^2*hia*hom*psi*sht)/(Lq^2*q1) + (Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq^2*q1) + (Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Lq^2*q2) - (Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*sht)/(Ld^2*q2) - (Rs*cht*dt^2*hib*psi*sht)/(Ld*Lq*q1) + (2*Rs*cht*dt^2*hib*psi*sht)/(Ld*Lq*q2) + (3*Ld^2*cht*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q1) - (3*Lq^2*cht*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q2);
+        D11(3,5) = (cht*dt^2*hom)/q4 - (dt*sht)/q5 - (dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/q1 + (dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/q2 + (Rs*dt^2*(hib^2 + q2)*sht)/(Ld*q1) - (Rs*dt^2*(hia^2 + q1)*sht)/(Ld*q2) - (Rs*dt^2*(hib^2 + q2)*sht)/(Lq*q1) + (Rs*dt^2*(hia^2 + q1)*sht)/(Lq*q2) + (cht*dt^2*hom*psi^2)/(Lq^2*q2) - (2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/q1 + (2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/q1 + (2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/q2 - (2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/q2 + (dt*hib*kpp*psi)/(J*q3) - (Rs*dt^2*(hia^2 + q1)*(sht^3 + 3*sht*q4))/(Ld*q2) - (Rs*dt^2*(hib^2 + q2)*(sht^3 + 3*sht*q4))/(Lq*q1) - (dt^2*hia*hom*psi)/(Lq*q2) + (2*Rs*cht*dt^2*hia*hib)/(Ld*q1) - (2*Rs*cht*dt^2*hia*hib)/(Lq*q2) - (Ld*dt*(hia^2 + q1)*kpp*sht)/(J*q3) + (Ld*dt*(hib^2 + q2)*kpp*sht)/(J*q3) + (Lq*dt*(hia^2 + q1)*kpp*sht)/(J*q3) - (Lq*dt*(hib^2 + q2)*kpp*sht)/(J*q3) + (2*Lq^2*(cht^3 + 3*cht*q5)*dt^2*(hib^2 + q2)*hom)/(Ld^2*q1) + (2*Ld^2*(cht^3 + 3*cht*q5)*dt^2*(hia^2 + q1)*hom)/(Lq^2*q2) + (3*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/q1 - (3*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/q2 - (2*Lq*cht*dt^2*(hib^2 + q2)*hom)/(Ld*q1) - (2*Ld*cht*dt^2*(hia^2 + q1)*hom)/(Lq*q2) + (2*Rs*(cht^2 + q5)*dt^2*hib*psi)/(Lq^2*q2) + (Rs*dt^2*hib*psi*(sht^2 + q4))/(Lq^2*q1) - (dt^2*hia*hom*psi*(sht^2 + q4))/(Ld*q1) + (dt^2*hia*hom*psi*(sht^2 + q4))/(Ld*q2) + (Ld*Rs*dt^2*(hib^2 + q2)*(sht^3 + 3*sht*q4))/(Lq^2*q1) + (Lq*Rs*dt^2*(hia^2 + q1)*(sht^3 + 3*sht*q4))/(Ld^2*q2) + (Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Ld*q1) - (2*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Lq*q1) + (Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Lq*q2) + (Ld*dt^2*hia*hom*psi*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*cht*dt*hia*hib*kpp)/(J*q3) - (2*Lq*cht*dt*hia*hib*kpp)/(J*q3) + (2*Lq*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Ld^2*q1) - (Lq*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Ld^2*q1) - (Ld*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*sht)/(Lq^2*q2) + (2*Ld*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*sht)/(Lq^2*q2) - (Rs*dt^2*hib*psi*(sht^2 + q4))/(Ld*Lq*q1) + (Ld^2*dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/(Lq^2*q1) - (Lq^2*dt^2*hia*hib*hom*(sht^3 + 3*sht*q4))/(Ld^2*q2) - (Ld*dt^2*hia*hib*hom*sht)/(Lq*q1) + (Lq*dt^2*hia*hib*hom*sht)/(Ld*q1) - (Ld*dt^2*hia*hib*hom*sht)/(Lq*q2) + (Lq*dt^2*hia*hib*hom*sht)/(Ld*q2) - (Rs*cht*dt^2*hia*psi*sht)/(Lq^2*q2) + (Lq^2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*cht*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Lq^2*q1) + (Lq^2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*cht*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq^2*q2) + (2*cht*dt^2*hib*hom*psi*sht)/(Ld*q1) - (2*cht*dt^2*hib*hom*psi*sht)/(Ld*q2) - (2*Lq*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hib)/(Ld^2*q1) + (2*Ld*Rs*(cht^3 + 3*cht*q5)*dt^2*hia*hib)/(Lq^2*q2) + (3*Ld*(cht^2 + q5)*dt^2*hia*hom*psi)/(Lq^2*q2) - (Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld*q1) + (3*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld*q2) - (3*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq*q1) + (Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq*q2) + (2*Ld*cht*dt^2*hib*hom*psi*sht)/(Lq^2*q2) + (Ld*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq^2*q1) + (Lq*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld^2*q1) - (Ld*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Lq^2*q2) - (Lq*Rs*cht*dt^2*hia*hib*(sht^2 + q4))/(Ld^2*q2) - (2*Rs*cht*dt^2*hia*psi*sht)/(Ld*Lq*q1) + (Rs*cht*dt^2*hia*psi*sht)/(Ld*Lq*q2) - (3*Lq^2*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/(Ld^2*q1) + (3*Ld^2*(cht^2 + q5)*dt^2*hia*hib*hom*sht)/(Lq^2*q2);
+
+        D11(4,1) = D11(1,4);
+        D11(4,2) = D11(2,4);
+        D11(4,3) = D11(3,4);
+        D11(4,4) = 1/q4 + (dt^2*(hom^2 + q3))/q5 + (dt^2*(hom^2 + q3)*psi^2)/(Lq^2*q1) - (2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/q1 - (2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/q2 + (dt^2*(hia^2 + q1)*kpp^2*psi^2)/(J^2*q3) + (Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Ld^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Ld^2*q2) + (Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Lq^2*q1) + (Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Lq^2*q2) + (4*Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q2) + (4*Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q1) - (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q1 + (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q2 - (2*Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Ld*Lq*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Ld*Lq*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2)/(J^2*q3) + (Ld^2*(cht^2 + q5)*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2)/(J^2*q3) + (Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/(Ld^2*q1) + (Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/(Lq^2*q1) + (Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Ld^2*q2) + (Ld^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Lq^2*q2) - (2*cht*dt^2*hia*(hom^2 + q3)*psi)/(Ld*q1) + (4*Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) + (4*Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld*q1) - (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld*q2) - (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq*q1) + (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq*q2) + (4*Ld*dt^2*hib*(hom^2 + q3)*psi*sht)/(Lq^2*q1) + (2*Ld*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi)/(J^2*q3) - (2*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi)/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) + (4*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*(cht^2 + q5)*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2)/(J^2*q3) - (2*Ld*Lq*(cht^2 + q5)*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2)/(J^2*q3) + (2*Rs*cht*dt^2*hib*hom*psi)/(Lq^2*q1) - (4*Rs*dt^2*hia*hom*psi*sht)/(Lq^2*q1) - (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld*q1) + (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq*q1) + (2*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq*q2) + (2*Ld*cht*dt^2*hia*(hom^2 + q3)*psi)/(Lq^2*q1) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld^2*q2) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Lq^2*q1) - (4*Ld^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (4*Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (4*Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) + (2*Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq^2*q1) + (2*Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld^2*q1) - (2*Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq^2*q2) - (2*Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld^2*q2) - (2*Rs*cht*dt^2*hib*hom*psi)/(Ld*Lq*q1) - (4*Ld*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq^2*q1) + (4*Ld*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq^2*q1) + (4*Lq*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld^2*q2) - (4*Lq*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld^2*q2) - (8*Ld*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q1) + (8*Lq*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q2) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) - (8*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q1) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q2) - (2*Ld*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (2*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) + (4*Ld^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Lq^2*q1) - (4*Lq^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Ld^2*q2) + (4*Ld*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) - (4*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (8*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3);
+        D11(4,5) = (dt*hom)/q4 - (dt*hom)/q5 + (2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/q1 - (2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/q2 - (2*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/q1 + (4*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/q1 + (4*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/q2 - (2*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/q2 - (2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/q1 + (2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/q2 - (dt^2*hia*hib*kpp^2*psi^2)/(J^2*q3) + (2*cht*dt^2*hib*(hom^2 + q3)*psi)/(Ld*q1) - (cht*dt^2*hib*(hom^2 + q3)*psi)/(Ld*q2) - (dt^2*hia*(hom^2 + q3)*psi*sht)/(Ld*q1) + (2*dt^2*hia*(hom^2 + q3)*psi*sht)/(Ld*q2) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Ld^2*q1) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Lq^2*q2) + (2*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Ld*q1) - (2*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Ld*q2) - (2*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Lq*q1) + (2*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Lq*q2) + (Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Ld^2*q1) + (Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Ld^2*q2) + (Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Lq^2*q1) + (Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Lq^2*q2) + (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Ld^2*q2) - (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Lq^2*q1) + (2*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld*q1) - (2*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld*q2) - (2*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq*q1) + (2*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq*q2) + (Ld*dt^2*hia*(hom^2 + q3)*psi*sht)/(Lq^2*q1) + (2*Ld^2*(cht^2 + q5)*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) - (2*Ld^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2)/(J^2*q3) + (2*Lq^2*(cht^2 + q5)*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) - (2*Lq^2*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2)/(J^2*q3) - (2*Ld^2*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Ld^2*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq^2*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) + (2*Lq*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Ld^2*q1) - (2*Lq*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Ld^2*q1) - (2*Ld*Rs*(cht^2 + q5)*dt^2*(hia^2 + q1)*hom)/(Lq^2*q2) + (2*Ld*Rs*(cht^2 + q5)*dt^2*(hib^2 + q2)*hom)/(Lq^2*q2) - (2*Ld*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Lq^2*q1) + (2*Ld*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*(hia^2 + q1)*hom*(sht^2 + q4))/(Ld^2*q2) - (2*Lq*Rs*dt^2*(hib^2 + q2)*hom*(sht^2 + q4))/(Ld^2*q2) + (Ld*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi)/(J^2*q3) - (Lq*cht*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi)/(J^2*q3) + (Ld*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi*sht)/(J^2*q3) - (Lq*dt^2*(hia^3 + 3*hia*q1)*kpp^2*psi*sht)/(J^2*q3) - (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Ld*Lq*q1) + (2*Rs^2*(cht^2 + q5)*dt^2*hia*hib)/(Ld*Lq*q2) + (4*Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Ld*Lq*q1) - (2*Rs^2*cht*dt^2*(hia^2 + q1)*sht)/(Ld*Lq*q2) + (4*Rs^2*cht*dt^2*(hib^2 + q2)*sht)/(Ld*Lq*q2) - (Rs*cht*dt^2*hia*hom*psi)/(Lq^2*q2) + (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*dt^2*hia*hib*(sht^2 + q4))/(Ld*Lq*q2) + (Rs*dt^2*hib*hom*psi*sht)/(Lq^2*q1) - (2*Lq^2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/(Ld^2*q1) + (2*Ld^2*(cht^2 + q5)*dt^2*hia*hib*(hom^2 + q3))/(Lq^2*q2) + (Ld^2*cht*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*sht)/(J^2*q3) + (Ld^2*cht*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*sht)/(J^2*q3) + (Lq^2*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/(Ld^2*q1) + (Ld^2*cht*dt^2*(hia^2 + q1)*(hom^2 + q3)*sht)/(Lq^2*q1) + (Lq^2*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/(Ld^2*q2) + (Ld^2*cht*dt^2*(hib^2 + q2)*(hom^2 + q3)*sht)/(Lq^2*q2) + (2*Ld^2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) - (2*Lq^2*dt^2*hia*hib*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld*cht*dt^2*hib*(hom^2 + q3)*psi)/(Lq^2*q2) + (4*Ld*Lq*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Ld*Lq*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*(sht^2 + q4))/(J^2*q3) - (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld*q1) + (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld*q2) - (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq*q1) + (6*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq*q2) - (2*Rs*cht*dt^2*hia*hom*psi)/(Ld*Lq*q1) + (Rs*cht*dt^2*hia*hom*psi)/(Ld*Lq*q2) - (Rs*dt^2*hib*hom*psi*sht)/(Ld*Lq*q1) + (2*Rs*dt^2*hib*hom*psi*sht)/(Ld*Lq*q2) - (6*Ld^2*cht*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (6*Lq^2*cht*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*cht*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*sht)/(J^2*q3) - (2*Ld*Lq*cht*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*sht)/(J^2*q3) - (3*Ld*cht*dt^2*(hia^2 + q1)*hib*kpp^2*psi)/(J^2*q3) + (3*Lq*cht*dt^2*(hia^2 + q1)*hib*kpp^2*psi)/(J^2*q3) - (3*Ld*dt^2*hia*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) + (3*Lq*dt^2*hia*(hib^2 + q2)*kpp^2*psi*sht)/(J^2*q3) - (4*Ld*Lq*(cht^2 + q5)*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2)/(J^2*q3) + (4*Ld*Lq*(cht^2 + q5)*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2)/(J^2*q3) + (2*Ld*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq^2*q1) + (2*Lq*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld^2*q1) - (2*Ld*Rs*cht*dt^2*hia*hib*hom*sht)/(Lq^2*q2) - (2*Lq*Rs*cht*dt^2*hia*hib*hom*sht)/(Ld^2*q2) + (12*Ld*Lq*cht*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*sht)/(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) = 1/q5 + (dt^2*(hom^2 + q3))/q4 + (dt^2*(hom^2 + q3)*psi^2)/(Lq^2*q2) - (2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/q1 - (2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/q2 + (dt^2*(hib^2 + q2)*kpp^2*psi^2)/(J^2*q3) + (4*Rs^2*(cht^2 + q5)*dt^2*(hia^2 + q1))/(Ld^2*q1) + (4*Rs^2*(cht^2 + q5)*dt^2*(hib^2 + q2))/(Lq^2*q2) + (Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld^2*q1) + (Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld^2*q2) + (Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Lq^2*q1) + (Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Lq^2*q2) + (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q1 - (4*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/q2 - (2*Rs^2*dt^2*(hib^2 + q2)*(sht^2 + q4))/(Ld*Lq*q1) - (2*Rs^2*dt^2*(hia^2 + q1)*(sht^2 + q4))/(Ld*Lq*q2) + (4*Lq^2*(cht^2 + q5)*dt^2*(hib^2 + q2)*(hom^2 + q3))/(Ld^2*q1) + (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hom^2 + q3))/(Lq^2*q2) + (Ld^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Ld^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (Lq^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q1) + (Ld^2*dt^2*(hia^2 + q1)*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q1) + (Lq^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/(Ld^2*q2) + (Ld^2*dt^2*(hib^2 + q2)*(hom^2 + q3)*(sht^2 + q4))/(Lq^2*q2) - (2*dt^2*hib*(hom^2 + q3)*psi*sht)/(Ld*q2) + (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld*q1) - (4*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld*q2) - (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq*q1) + (4*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq*q2) - (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld*q1) + (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld*q2) - (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq*q1) + (2*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq*q2) + (2*Ld*dt^2*hib*(hom^2 + q3)*psi*sht)/(Lq^2*q2) + (2*Ld*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi*sht)/(J^2*q3) - (2*Lq*dt^2*(hib^3 + 3*hib*q2)*kpp^2*psi*sht)/(J^2*q3) + (4*Ld^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) + (4*Lq^2*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) - (2*Ld^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Lq^2*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*dt^2*(hia^4 + 6*hia^2*q1 + 3*q1^2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (2*Ld*Lq*dt^2*(hib^4 + 6*hib^2*q2 + 3*q2^2)*kpp^2*(sht^2 + q4))/(J^2*q3) + (4*Rs*cht*dt^2*hib*hom*psi)/(Lq^2*q2) - (2*Rs*dt^2*hia*hom*psi*sht)/(Lq^2*q2) + (4*Ld*cht*dt^2*hia*(hom^2 + q3)*psi)/(Lq^2*q2) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld^2*q1) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Lq^2*q2) + (4*Ld^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (4*Ld^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) + (4*Lq^2*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) - (4*Lq^2*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3) - (8*Lq*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Ld^2*q1) + (8*Ld*Rs*(cht^2 + q5)*dt^2*hia*hib*hom)/(Lq^2*q2) + (4*Lq*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Ld^2*q1) - (4*Lq*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Ld^2*q1) - (4*Ld*Rs*cht*dt^2*(hia^2 + q1)*hom*sht)/(Lq^2*q2) + (4*Ld*Rs*cht*dt^2*(hib^2 + q2)*hom*sht)/(Lq^2*q2) + (2*Ld*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q1) + (2*Lq*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q1) - (2*Ld*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Lq^2*q2) - (2*Lq*Rs*dt^2*hia*hib*hom*(sht^2 + q4))/(Ld^2*q2) - (8*Ld*Lq*(cht^2 + q5)*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2)/(J^2*q3) + (2*Rs*dt^2*hia*hom*psi*sht)/(Ld*Lq*q2) + (4*Ld*Lq*dt^2*(hia^2 + q1)*(hib^2 + q2)*kpp^2*(sht^2 + q4))/(J^2*q3) - (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q1) + (4*Rs^2*cht*dt^2*hia*hib*sht)/(Ld*Lq*q2) + (4*Ld*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) - (4*Lq*cht*dt^2*hia*(hib^2 + q2)*kpp^2*psi)/(J^2*q3) - (4*Lq^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Ld^2*q1) + (4*Ld^2*cht*dt^2*hia*hib*(hom^2 + q3)*sht)/(Lq^2*q2) - (2*Ld*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) + (2*Lq*dt^2*(hia^2 + q1)*hib*kpp^2*psi*sht)/(J^2*q3) - (8*Ld*Lq*cht*dt^2*hia*(hib^3 + 3*hib*q2)*kpp^2*sht)/(J^2*q3) + (8*Ld*Lq*cht*dt^2*(hia^3 + 3*hia*q1)*hib*kpp^2*sht)/(J^2*q3);
+        
+        D12 = -A92'*iQ5;
+        D21 = D12';
+        D22 = iQ5 + C5'*iR*C5;
+
+        Jj12 = D22 - D21/(Jj12 + D11)*D12;
+        iJn12(:,:,t) = inv(Jj12);
+    end
+    
+    %control & simulation
+    if(control == 1) %ome = ref_ome, the = integral(ome), ial=ibe=0
+        x_sys(1, t+1) = 0;
+        x_sys(2, t+1) = 0;
+        x_sys(3, t+1) = ref_ome(t);
+        x_sys(4, t+1) = the + dt*ome;
+    else
+        if(control == 6) %rnd. err. ref_ome
+           ref_ome(t) = ref_ome(t) + amp*randn(); 
+        end
+        
+        %PI (only PI for control == 2)        
+        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;        
+               
+        if(control == 3) %inj. sin -> dq
+            u_dq(1,t) = u_dq(1,t) + amp*sin(injom*dt*t);
+            u_dq(2,t) = u_dq(2,t) + amp*cos(injom*dt*t);
+        elseif(control == 4) %inj. rect. -> dq
+            u_dq(1,t) = u_dq(1,t) + amp*sign(sin(injom*dt*t));
+            u_dq(2,t) = u_dq(2,t) + amp*sign(cos(injom*dt*t));
+        elseif(control == 5) %inj. const. -> d
+%             u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
+            u_dq(1,t) = amp;            
+        elseif(control == 9) %bic. sign. ome           
+            u_dq(1,t) = u_dq(1,t) - amp*sign(ome);
+            u_dq(2,t) = u_dq(2,t) + amp*sign(ome);
+        end
+       
+        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);
+        
+        if(control == 7) %inj. sin -> al-be
+            ual = ual + amp*cos(injom*dt*t);
+            ube = ube + amp*sin(injom*dt*t);
+        elseif(control == 8) %inj. rect. -> al-be
+            ual = ual + amp*sign(cos(injom*dt*t));
+            ube = ube + amp*sign(sin(injom*dt*t));        
+        elseif((control == 10)&&(rand() > (1/5))) %bic rnd. 5
+            duab = sign(rand(2,1)-0.5)*amp;
+            ual = ual + duab(1);
+            ube = ube + duab(2);
+        end
+      
+        ud = ual*cos(the) + ube*sin(the);
+        uq = ube*cos(the) - ual*sin(the);
+       
+        %simulation model Ldq
+        idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*ud;
+        iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*uq;
+
+        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) = (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
+        
+end
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   plot
+
+%axis
+xax = 1:T-1;
+timex = (xax)*dt;
+
+if(plotsys == 1)
+    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))));
+    figure;
+end
+
+if(model1 == 1)
+    hold on;
+    plot(timex, squeeze(iJn1(4,4,xax)), '-b');
+end
+if(model2 == 1)
+    hold on;
+    plot(timex, squeeze(iJn2(4,4,xax)), '-r');
+end
+if(model3 == 1)
+    hold on;
+    plot(timex, squeeze(iJn3(4,4,xax)), '-g',timex, squeeze(iJn3(5,5,xax)), '-g');
+end
+if(model4 == 1)
+    hold on;
+    plot(timex, squeeze(iJn4(4,4,xax)), '--b');
+end
+if(model5 == 1)
+    hold on;
+    plot(timex, squeeze(iJn5(4,4,xax)), '--r');
+end
+if(model6 == 1)
+    hold on;
+    plot(timex, squeeze(iJn6(4,4,xax)), '--g',timex, squeeze(iJn6(5,5,xax)), '--g');
+end
+if(model7 == 1)
+    hold on;
+    plot(timex, squeeze(iJn7(4,4,xax)), '-c');
+end
+if(model8 == 1)
+    hold on;
+    plot(timex, squeeze(iJn8(4,4,xax)), '-m');
+end
+if(model9 == 1)
+    hold on;
+    plot(timex, squeeze(iJn9(4,4,xax)), '-y',timex, squeeze(iJn9(5,5,xax)), '-y');
+end
+if(model10 == 1)
+    hold on;
+    plot(timex, squeeze(iJn10(4,4,xax)), '--c');
+end
+if(model11 == 1)
+    hold on;
+    plot(timex, squeeze(iJn11(4,4,xax)), '--m');
+end
+if(model12 == 1)
+    hold on;
+    plot(timex, squeeze(iJn12(4,4,xax)), '--y',timex, squeeze(iJn12(5,5,xax)), '--y');
+end
Index: /applications/dual/vahala/pcrb/pcrb_E3.m
===================================================================
--- /applications/dual/vahala/pcrb/pcrb_E3.m (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb_E3.m (revision 1416)
@@ -0,0 +1,94 @@
+%simulator PMSM for PCRB, choice: Ls/Ldq & correct/incorrect expectation
+clear all;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%for experiments
+    %ctrl.> 2,5,9,10
+    %model> {1},{7},{4,6,10},{12} - {eq.} sort from worst
+
+%   settings
+    % ON/OFF  MODEL    IS CORR. E   INDUCT. L
+model(1) = 1; % alpha-beta    nE      Ls
+model(2) = 0; % d-q           nE      Ls
+model(3) = 0; % 5sc           nE      Ls
+
+model(4) = 0; % alpha-beta    E       Ls
+model(5) = 0; % d-q           E       Ls
+model(6) = 0; % 5sc           E       Ls
+
+model(7) = 1; % alpha-beta    nE      Ldq
+model(8) = 0; % d-q           nE      Ldq
+model(9) = 0; % 5sc           nE      Ldq
+
+model(10) = 0; % alpha-beta   E       Ldq
+model(11) = 0; % d-q          E       Ldq
+model(12) = 0; % 5sc          E       Ldq
+
+    % CONTROL
+    % 1 -   ome = ref_ome, the = integral(ome), ial = ibe = 0
+    % 2 -   PI control    
+    % 3 -   PI + injection (sin -> dq)
+    % 4 -   PI + injection (rect. -> dq)
+    % 5 -   PI + injection (const. -> ud) *J4 ~ 3e-6 for all exc. dq
+    % 6 -   PI + rnd error (ref_ome) *J4: dq bad; ab,5sc Ls ~ 4e-4; ab,5sc Ldq ~5e-6  
+    % 7 -   PI + injection (sin -> al-be)
+    % 8 -   PI + injection (rect. -> al-be)
+    % 9 -   PI + bicrit. (sign) *J4 ~ 1e-3 for all exc. dq
+    % 10 -  PI + rnd bicrit. selection (5) 
+    
+% control = 2;
+% control = 5;
+% control = 8; %high frekq.
+% control = 9;
+control = 10;
+    % param. amp*phi(injom*dt*t) //phi = sin,cos,sign(sin),sing(cos)
+    
+amp = 10.0;
+injom = 5000;
+
+    %cov Q, R matrices
+%         %noise
+% Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
+% R = diag([0.0006 0.0006]);
+%         %EKF
+% Q = diag([0.1 0.1 0.1 0.001]);
+% R = diag([0.05 0.05]);
+%         %test
+Q = diag([0.1 0.1 0.1 10]);
+R = diag([0.5 0.5]);
+
+%horizont
+T = 120000; 
+
+%reference 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);
+
+[var_ome, var_th, var_th2] = fpcrb(model, control, amp, injom, Q, R, T, ref_profile);
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   plot
+
+%axis
+xax = 1:T-1;
+dt = 0.000125;
+timex = (xax)*dt;
+cmp = lines(sum(model));
+for i=1:sum(model)
+    figure(1);
+    hold on;
+    plot(timex, var_ome(xax,i),'Color',cmp(i,:));
+    
+    figure(2);
+    hold on;
+    plot(timex, var_th(xax,i),'Color',cmp(i,:));
+    
+    figure(3);
+    hold on;
+    plot(timex, var_th2(xax,i),'Color',cmp(i,:));
+end
+
+sound(chirp(0:0.001:2,0,1,150));
Index: /applications/dual/vahala/pcrb/fpcrb2.m
===================================================================
--- /applications/dual/vahala/pcrb/fpcrb2.m (revision 1416)
+++ /applications/dual/vahala/pcrb/fpcrb2.m (revision 1416)
@@ -0,0 +1,290 @@
+function [var_ome, var_th] = fpcrb2(model, control, amp, injom, Q, R, T, ref_profile, Q0)
+    %function for computing PCRB of PMSM model
+       
+
+    %   machine parameters
+    Rs = 0.28;
+    Ls = 0.003465;
+    psipm = 0.1989;
+    B = 0;    
+    kp = 1.5;
+    pp = 4.0;
+    J = 0.04;
+
+    dt = 0.000125;
+
+    Lq = 1.0*Ls;
+    Ld = 0.9*Ls;
+
+    %aliases
+    kpp = kp*pp*pp;
+    kppj = kpp/J;
+    psi = psipm;
+
+    a = 0.9898;
+    b = 0.0072;
+    c = 0.0361;
+    d = 1.0;
+    e = 0.0149;    
+
+    %reference signal
+    ref_ome = zeros(1, T);  
+    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
+
+
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %   system variables
+
+    %system state and control
+    x_sys = zeros(4, T);     
+    u_dq = zeros(2, T);
+
+    %covariance alternatives
+    Q4 = Q; 
+    Q2 = Q4(3:4,3:4);
+    iQ4 = inv(Q4);  
+    iQ2 = inv(Q2);
+    iR = inv(R);
+    
+
+    %PCRB J matrices
+    if(model(1) == 1)
+        iJn1 = zeros(2,T);
+        Jj1 = inv(Q0);
+    end
+    if(model(2) == 1)
+        iJn2 = zeros(2,T);
+        Jj2 = inv(Q0);
+    end
+    if(model(3) == 1)
+        iJn3 = zeros(2,T);
+        Jj3 = inv(Q0(3:4,3:4));
+    end
+    if(model(4) == 1)
+        iJn4 = zeros(2,T);
+        Jj4 = inv(Q0(3:4,3:4));
+    end
+    
+    
+
+    %PI control
+    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;
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %   init
+
+    % measurement function derivatives
+    C4 = zeros(2,4);
+    C4(1,1) = 1.0;
+    C4(2,2) = 1.0;
+
+    % system function derivatives
+        %alpha-beta, equal Ls
+    A14 = zeros(4);
+    A14(1,1) = a;
+    A14(2,2) = a;
+    A14(3,3) = d;
+    A14(4,3) = dt;
+    A14(4,4) = 1.0;       
+
+    % new figure              
+%     figure;
+
+    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+    %   main loop
+
+    for t = 1:T-1,
+        %aliases
+        ial = x_sys(1, t);
+        ibe = x_sys(2, t);
+        ome = x_sys(3, t);
+        the = x_sys(4, t); 
+
+        ia = ial;
+        ib = ibe;
+
+        id = ial*cos(the) + ibe*sin(the);
+        iq = ibe*cos(the) - ial*sin(the);
+        %PCRB models
+        if(model(1) == 1) %al-be/Ls
+            A14(1,3) = b*sin(the);
+            A14(1,4) = b*ome*cos(the);
+            A14(2,3) = -b*cos(the);
+            A14(2,4) = b*ome*sin(the);
+            A14(3,1) = -e*sin(the);
+            A14(3,2) = e*cos(the);
+            A14(3,4) = -e*(ial*cos(the)+ibe*sin(the));
+
+            D11 = A14'*iQ4*A14;
+            D12 = -A14'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj1 = D22 - D21/(Jj1 + D11)*D12;
+            tmp = inv(Jj1);        
+            iJn1(1,t) = tmp(3,3);
+            iJn1(2,t) = tmp(4,4);
+        end        
+        if(model(2) == 1) %al-be/Ldq        
+            A70 = [[ (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 = A70'*iQ4*A70;
+            D12 = -A70'*iQ4;
+            D21 = D12';
+            D22 = iQ4 + C4'*iR*C4;
+
+            Jj2 = D22 - D21/(Jj2 + D11)*D12;
+            tmp = inv(Jj2);        
+            iJn2(1,t) = tmp(3,3);
+            iJn2(2,t) = tmp(4,4);        
+        end
+        if(model(3) == 1) %reduced al-be/Ls
+            A3 = [d, -e*(ibe*sin(the)+ial*cos(the)); dt, 1];
+            
+            D11 = A3'*iQ2*A3;
+            D12 = -A3'*iQ2;
+            D21 = D12';
+            D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + Q4(3,3))]*iR;
+%             D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2)]*iR;
+%             tmp = inv(Jj3);
+%             D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + tmp(1,1))]*iR;
+% tady nahore to dela extremni rozdil
+            
+            
+            Jj3 = D22 - D21/(Jj3 + D11)*D12;
+            tmp = inv(Jj3);        
+            iJn3(1,t) = tmp(1,1);
+            iJn3(2,t) = tmp(2,2);
+        end  
+        if(model(4) == 1) %reduced al-be/Ldq
+            A3 = [d, -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); dt, 1.0];
+            
+            D11 = A3'*iQ2*A3;
+            D12 = -A3'*iQ2;
+            D21 = D12';
+            D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + Q4(3,3))]*iR;
+%             D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2)]*iR;
+%             tmp = inv(Jj3);
+%             D22 = iQ2 + [b^2, 0; 0, b^2*(ome^2 + tmp(1,1))]*iR;
+% tady nahore to dela extremni rozdil
+            
+            
+            Jj4 = D22 - D21/(Jj4 + D11)*D12;
+            tmp = inv(Jj4);        
+            iJn4(1,t) = tmp(1,1);
+            iJn4(2,t) = tmp(2,2);
+        end 
+
+        %control & simulation
+        if(control == 1) %ome = ref_ome, the = integral(ome), ial=ibe=0
+            x_sys(1, t+1) = 0;
+            x_sys(2, t+1) = 0;
+            x_sys(3, t+1) = ref_ome(t);
+            x_sys(4, t+1) = the + dt*ome;
+        else
+            if(control == 6) %rnd. err. ref_ome
+               ref_ome(t) = ref_ome(t) + amp*randn(); 
+            end
+
+            %PI (only PI for control == 2)        
+            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;        
+
+            if(control == 3) %inj. sin -> dq
+                u_dq(1,t) = u_dq(1,t) + amp*sin(injom*dt*t);
+                u_dq(2,t) = u_dq(2,t) + amp*cos(injom*dt*t);
+            elseif(control == 4) %inj. rect. -> dq
+                u_dq(1,t) = u_dq(1,t) + amp*sign(sin(injom*dt*t));
+                u_dq(2,t) = u_dq(2,t) + amp*sign(cos(injom*dt*t));
+            elseif(control == 5) %inj. const. -> d
+    %             u_dq(1,t) = 0.1*Ld/dt - (1.0*Ld/dt - Rs)*id - Lq*ome*iq;
+                u_dq(1,t) = amp;            
+            elseif(control == 9) %bic. sign. ome           
+                u_dq(1,t) = u_dq(1,t) - amp*sign(ome);
+                u_dq(2,t) = u_dq(2,t) + amp*sign(ome);
+            end
+
+            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);
+
+            if(control == 7) %inj. sin -> al-be
+                ual = ual + amp*cos(injom*dt*t);
+                ube = ube + amp*sin(injom*dt*t);
+            elseif(control == 8) %inj. rect. -> al-be
+                ual = ual + amp*sign(cos(injom*dt*t));
+                ube = ube + amp*sign(sin(injom*dt*t));        
+            elseif((control == 10)&&(rand() > (1/5))) %bic rnd. 5
+                duab = sign(rand(2,1)-0.5)*amp;
+                ual = ual + duab(1);
+                ube = ube + duab(2);
+            end
+
+            ud = ual*cos(the) + ube*sin(the);
+            uq = ube*cos(the) - ual*sin(the);
+
+            %simulation model Ldq
+            idpl = (1.0 - Rs*dt/Ld)*id + Lq*dt/Ld*ome*iq + dt/Ld*ud;
+            iqpl = (1.0 - Rs*dt/Lq)*iq - psipm*dt/Lq*ome - Ld*dt/Lq*ome*id + dt/Lq*uq;
+
+            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) = (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
+
+    end
+
+    ssz = sum(model);
+    var_ome = zeros(T,ssz);
+    var_th = zeros(T,ssz);    
+    ind = 1;
+    if(model(1) == 1)
+        var_ome(:,ind) = iJn1(1,:);
+        var_th(:,ind) = iJn1(2,:);   
+        ind = ind+1;
+    end
+    if(model(2) == 1)
+        var_ome(:,ind) = iJn2(1,:);
+        var_th(:,ind) = iJn2(2,:);
+        ind = ind+1;
+    end    
+    if(model(3) == 1)
+        var_ome(:,ind) = iJn3(1,:);
+        var_th(:,ind) = iJn3(2,:);
+        ind = ind+1;
+    end
+    if(model(4) == 1)
+        var_ome(:,ind) = iJn4(1,:);
+        var_th(:,ind) = iJn4(2,:);
+        ind = ind+1;
+    end
+end
Index: /applications/dual/vahala/pcrb/pcrb_E4.m
===================================================================
--- /applications/dual/vahala/pcrb/pcrb_E4.m (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb_E4.m (revision 1416)
@@ -0,0 +1,75 @@
+%simulator PMSM for PCRB, choice: Ls/Ldq & control
+clear all;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%   settings
+    % ON/OFF  MODEL       INDUCT. L
+model(1) = 1; % alpha-beta          Ls
+model(2) = 0; % alpha-beta          Ldq
+model(3) = 0; % reduced alpha-beta          Ls
+model(4) = 0;%zatim nefunguje % reduced alpha-beta          Ldq
+
+    % CONTROL
+    % 1 -   ome = ref_ome, the = integral(ome), ial = ibe = 0
+    % 2 -   PI control    
+    % 3 -   PI + injection (sin -> dq)
+    % 4 -   PI + injection (rect. -> dq)
+    % 5 -   PI + injection (const. -> ud) *J4 ~ 3e-6 for all exc. dq
+    % 6 -   PI + rnd error (ref_ome) *J4: dq bad; ab,5sc Ls ~ 4e-4; ab,5sc Ldq ~5e-6  
+    % 7 -   PI + injection (sin -> al-be)
+    % 8 -   PI + injection (rect. -> al-be)
+    % 9 -   PI + bicrit. (sign) *J4 ~ 1e-3 for all exc. dq
+    % 10 -  PI + rnd bicrit. selection (5) 
+    
+control = 2;
+% control = 5;
+% control = 8; %high frekq.
+% control = 9;
+% control = 10;
+    % param. amp*phi(injom*dt*t) //phi = sin,cos,sign(sin),sing(cos)
+    
+amp = 10.0;
+injom = 5000;
+
+    %cov Q, R matrices
+%         %noise
+% Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
+% R = diag([0.0006 0.0006]);
+%         %EKF
+Q = diag([0.1 0.1 0.1 0.001]);
+R = diag([0.05 0.05]);
+%         %test
+% Q = diag([0.1 0.1 0.1 10]);
+% R = diag([0.5 0.5]);
+
+%horizont
+T = 120000; 
+
+%reference 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);
+
+[var_ome, var_th] = fpcrb2(model, control, amp, injom, Q, R, T, ref_profile, eye(4));
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   plot
+
+%axis
+xax = 1:T-1;
+dt = 0.000125;
+timex = (xax)*dt;
+cmp = lines(sum(model));
+for i=1:sum(model)
+    figure(1);
+    hold on;
+    plot(timex, var_ome(xax,i),'Color',cmp(i,:));
+    
+    figure(2);
+    hold on;
+    plot(timex, var_th(xax,i),'Color',cmp(i,:));    
+end
+
+% sound(chirp(0:0.001:2,0,1,150));
Index: /applications/dual/vahala/pcrb/pcrb_amptest.m
===================================================================
--- /applications/dual/vahala/pcrb/pcrb_amptest.m (revision 1416)
+++ /applications/dual/vahala/pcrb/pcrb_amptest.m (revision 1416)
@@ -0,0 +1,120 @@
+%simulator PMSM for PCRB, choice: Ls/Ldq & control
+clear all;
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+
+%   settings
+    % ON/OFF  MODEL       INDUCT. L
+model(1) = 1; % alpha-beta          Ls
+model(2) = 0; % alpha-beta          Ldq
+model(3) = 0; % reduced alpha-beta          Ls
+model(4) = 0;%zatim nefunguje % reduced alpha-beta          Ldq
+
+    % CONTROL
+    % 1 -   ome = ref_ome, the = integral(ome), ial = ibe = 0
+    % 2 -   PI control    
+    % 3 -   PI + injection (sin -> dq)
+    % 4 -   PI + injection (rect. -> dq)
+    % 5 -   PI + injection (const. -> ud) *J4 ~ 3e-6 for all exc. dq
+    % 6 -   PI + rnd error (ref_ome) *J4: dq bad; ab,5sc Ls ~ 4e-4; ab,5sc Ldq ~5e-6  
+    % 7 -   PI + injection (sin -> al-be)
+    % 8 -   PI + injection (rect. -> al-be)
+    % 9 -   PI + bicrit. (sign) *J4 ~ 1e-3 for all exc. dq
+    % 10 -  PI + rnd bicrit. selection (5) 
+    
+% control = 2;
+control = 5;
+% control = 8; %high frekq.
+% control = 9;
+% control = 10;
+    % param. amp*phi(injom*dt*t) //phi = sin,cos,sign(sin),sing(cos)
+
+    
+% amp = 0.001 -- 10;
+injom = 5000;
+
+    %cov Q, R matrices
+%         %noise
+% Q = diag([0.0013 0.0013 5.0e-6 1.0e-10]);
+% R = diag([0.0006 0.0006]);
+%         %EKF
+Q = diag([0.1 0.1 0.1 0.001]);
+R = diag([0.05 0.05]);
+%         %test
+% Q = diag([0.1 0.1 0.1 10]);
+% R = diag([0.5 0.5]);
+
+%horizont
+T = 120000; 
+
+%reference 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);
+
+% [var_ome, var_th] = fpcrb2(model, control, amp, injom, Q, R, T, ref_profile, 100*eye(4));
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%   plot
+
+p0 = 1;
+
+%axis
+xax = 1:T-1;
+dt = 0.000125;
+timex = (xax)*dt;
+cmp = hsv(10);
+
+[var_ome, var_th] = fpcrb2(model, control, 10, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(1,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 1, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(2,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.1, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(3,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.02, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(4,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.01, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(5,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.005, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(6,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.003, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(7,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.002, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(8,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.0015, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(9,:));
+
+[var_ome, var_th] = fpcrb2(model, control, 0.001, injom, Q, R, T, ref_profile, p0*eye(4));
+figure(1);
+hold on;
+plot(timex, var_th(xax,1),'Color',cmp(10,:));
+legend('10','1','0.1','0.02','0.01','0.005','0.003','0.002','0.0015','0.001','Location','NorthEastOutside');
+
+% sound(chirp(0:0.001:2,0,1,150));
