function F = EKF(p,A,C,Q,REKFinv)

P = [p(1) p(2); p(2) p(3)];

RiccatiEKF = A*P + P*transpose(A) - P*transpose(C)*REKFinv*C*P + Q;

F = [RiccatiEKF(1,1);
     RiccatiEKF(1,2);
     RiccatiEKF(2,2)];

% F = [2*p(2) - REKFinv*p(1)^2 + Q(1,1);
%        p(3) - REKFinv*p(1)*p(2);
%             - REKFinv*p(2)^2 + Q(2,2)];