function [xnew, Pnew] = update_nonlin(xpred, Ppred, z, measure_func, X, R)

[zhat, dh] = feval(measure_func, xpred, X);
nu = zhat - z                   %% innovation
S = R + dh*Ppred*dh'                 %% innovation convariance
K = Ppred*dh'*inv(S);   %% Kalman gain
xnew = xpred + K*nu; %% new state
K*S*K'
Pnew = Ppred - K*S*K'              %% new covariance



