ekf_ex1.m

ekf扩展卡尔曼滤波,效果比较好extended kalman filter

function [newx,newp] = EKF_ex1(x,p,y,Q,R)
% 
prex = 0.5*x+25*x/(1+x*x)+8*cos(1.2*x);
fk = 0.5+25*(1-x*x)/(1+x*x)*(1+x*x)-9.6*sin(1.2*x);
prep = Q+fk*p*fk;
hk = prex*0.1;
sk = hk*prep*hk+R;
kk = prep*hk/sk;
newx = prex+kk*(y-prex*prex/20);
newp = prep-kk*hk*prep;