function [x_k, P_k] = extKF4(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(14,1);
    xp(1) = a*x(1) + b*x(3)*sin(x(4)) + c*u(1);
    xp(2) = a*x(2) - b*x(3)*cos(x(4)) + c*u(2);
    xp(3) = d*x(3) + e*(x(2)*cos(x(4)) - x(1)*sin(x(4)));
    xp(4) = x(4) + dt*x(3);
    xp(5:14) = pre;
        
    x_k = xp + K*(y - xp(1:2));
    
    %max x(14) = pi^2/3 ... variance of uniform -pi,pi
%     if(x_k(14) > pi^2/3)
%         x_k(14) = pi^2/3;
%     end
end