...
...
    Jh(1,1) = -dx/r;   Jh(1,2) = -dy/r;   Jh(1,3) = 0;
    Jh(2,1) = dy/(r^2);   Jh(2,2) = -dx/(r^2);   Jh(2,3) = -1;

    Jh(1,numStates-1) = 0;     Jh(1,numStates) = 0;
    Jh(2,numStates-1) = 0;     Jh(2,numStates) = 0;
...
...
    K = Pest*Jh'/(Jh*Pest*Jh'+R);
    xest(:,k) = xest(:,k)+K*innov;
    xest(3,k) = normalizeAngle(xest(3,k));
    I = eye(size(K*Jh));
    Pest = (I-K*Jh)*Pest;
...
...