function [x_k, P_k] = extKF(x, y, u, pre, A, C, P, Q, R)
    a = 0.9898;
    b = 0.0072;
    c = 0.0361;
    d = 1.0;
    e = 0.0149;
    dt = 0.000125;

    Pp = A*P*A' + Q;
    S = C*Pp*C' + R;
    K = Pp*C'/S;
    P_k = Pp - K*C*Pp;    
    
    xp = zeros(5,1);
    xp(1) = d*x(1) + e*(y(2)*cos(x(2)) - y(1)*sin(x(2)));
    xp(2) = x(2) + dt*x(1);
    xp(3) = pre(1);
    xp(4) = pre(2);
    xp(5) = pre(3);
    yp = zeros(2,1);
    yp(1) = a*y(1) + b*x(1)*sin(x(2)) + c*u(1);
    yp(2) = a*y(2) - b*x(1)*cos(x(2)) + c*u(2);
    
    x_k = xp + K*(y - yp);
    
    %max x(5) = pi^2/3 ... variance of uniform -pi,pi
%     if(x_k(5) > pi^2/3)
%         x_k(5) = pi^2/3;
%     end
end