root/applications/dual/vahala/kim/assembDeriv.m @ 1451

function [A_k, C_k, pre_k, A_l] = assembDeriv(ksi, iab, ksi0, Q, R, ref_ome, inddq)    
    a = 0.9898;
    b = 0.0072;
    c = 0.0361;    
    d = 1.0;
    e = 0.0149;
    dt = 0.000125;

    Rs = 0.28;
    Ls = 0.003465;
    psi = 0.1989;
    B = 0;    
    kp = 1.5;
    pp = 4.0;
    J = 0.04;
    Lq = 1.0*Ls;
    Ld = 0.9*Ls;
    kpp = kp*pp*pp;
    kppj = kpp/J;
    
    ome = ksi(1);
    the = ksi(2);
    P = [ksi(3), ksi(4); ksi(4), ksi(5)];
    ia = iab(1);
    ib = iab(2);
    A_k = zeros(5);
    A_l = zeros(6);
       
    
    %puvodni matice derivaci
    if(inddq == 0)
        %stejne indukcnosti
        A = [d, -e*(ia*cos(the)+ib*sin(the)); dt, 1.0];
    else
        %ruzne indukcnosti
        A = [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];
    end
    C = [b*sin(the), b*ome*cos(the); -b*cos(the), b*ome*sin(the)];
    
    %dalsi matice EKF
    Pp = A*P*A' + Q;
    S = C*Pp*C' + R;
    K = Pp*C'/S;
    Pnew = (eye(2) - K*C)*Pp;    
    
    %derivace zakladnich matic EKF stavu
%     dAdom = zeros(4);
    dAdth = [0, e*(ia*sin(the)-ib*cos(the)); 0, 0];
%     dAdP = zeros(4);
    dCdom = [0, b*cos(the); 0, b*sin(the)];
    dCdth = [b*cos(the), -b*ome*sin(the); b*sin(the), b*ome*cos(the)];
%     dCdP = zeros(4);
%     dPdomth = zeros(4);
    dPdPo = [1, 0; 0, 0];
    dPdPot = [0, 1; 1, 0];
    dPdPt = [0, 0; 0, 1];
    
    %derivace dalsich matic EKF stavu
%     dPpdom = zeros(4);
    dPpdth = dAdth*P*A' + A*P*dAdth';
    dPpdPo = A*dPdPo*A';
    dPpdPot = A*dPdPot*A';
    dPpdPt = A*dPdPt*A';
    
    dSdom = dCdom*Pp*C' + C*Pp*dCdom';
    dSdth = dCdth*Pp*C' + C*dPpdth*C' + C*Pp*dCdth';
    dSdPo = C*dPpdPo*C';
    dSdPot = C*dPpdPot*C';
    dSdPt = C*dPpdPt*C';
    
    dKdom = Pp*dCdom'/S - Pp*C'/S*dSdom/S;
    dKdth = dPpdth*C'/S + Pp*dCdth'/S - Pp*C'/S*dSdth/S;
    dKdPo = dPpdPo*C'/S - Pp*C'/S*dSdPo/S;
    dKdPot = dPpdPot*C'/S - Pp*C'/S*dSdPot/S;
    dKdPt = dPpdPt*C'/S - Pp*C'/S*dSdPt/S;
    
    dPnewdom = - dKdom*C*Pp - K*dCdom*Pp;
    dPnewdth = dPpdth - dKdth*C*Pp - K*dCdth*Pp - K*C*dPpdth; 
    dPnewdPo = dPpdPo - dKdPo*C*Pp - K*C*dPpdPo;
    dPnewdPot = dPpdPot - dKdPot*C*Pp - K*C*dPpdPot;
    dPnewdPt = dPpdPt - dKdPt*C*Pp - K*C*dPpdPt;
    
    A_k(1,1) = d;
    A_k(1,2) = -e*(ia*cos(the)+ib*sin(the));
%     A_k(1,3) = 0;
%     A_k(1,4) = 0;
%     A_k(1,5) = 0;
    
    A_k(2,1) = dt;
    A_k(2,2) = 1.0;
%     A_k(2,3) = 0;
%     A_k(2,4) = 0;
%     A_k(2,5) = 0;
    
    A_k(3,1) = dPnewdom(1,1);
    A_k(3,2) = dPnewdth(1,1);
    A_k(3,3) = dPnewdPo(1,1);
    A_k(3,4) = dPnewdPot(1,1);
    A_k(3,5) = dPnewdPt(1,1);
    
    A_k(4,1) = dPnewdom(1,2);
    A_k(4,2) = dPnewdth(1,2);
    A_k(4,3) = dPnewdPo(1,2);
    A_k(4,4) = dPnewdPot(1,2);
    A_k(4,5) = dPnewdPt(1,2);
    
    A_k(5,1) = dPnewdom(2,2);
    A_k(5,2) = dPnewdth(2,2);
    A_k(5,3) = dPnewdPo(2,2);
    A_k(5,4) = dPnewdPot(2,2);
    A_k(5,5) = dPnewdPt(2,2);
    
    phi = zeros(5,1);
    phi(1) = d*ksi0(1) + e*(ib*cos(ksi0(2)) - ia*sin(ksi0(2)));
    phi(2) = ksi0(2) + dt*ksi0(1);
    phi(3) = Pnew(1,1);
    phi(4) = Pnew(1,2);
    phi(5) = Pnew(2,2);
    
    A_l(1:5,1:5) = A_k;
    A_l(1:5,6) = phi - A_k*ksi0;
    A_l(6,6) = 1.0;
    
    %%%
    A_l(1,2) = 0; %maze clen e*(...), ktery se prevede do B
    A_l(1:2,6) = [0;dt*ref_ome]; %zmena korekce, protoze je to tam uz linearni, ale posun
    %%%
    
    C_k = zeros(2,5);
    C_k(1:2,1:2) = C;
    
    pre_k = zeros(3,1);    
    pre_k(1) = Pnew(1,1);
    pre_k(2) = Pnew(1,2);
    pre_k(3) = Pnew(2,2); 
    %max x(5) = pi^2/3 ... variance of uniform -pi,pi
    if(pre_k(3) > pi^2/3)
        pre_k(3) = pi^2/3;
    end
end