gusucode.com > 扩展卡尔曼滤波，粒子滤波，去偏卡尔曼滤波和循环增益尔曼滤波的源程序 > 扩展卡尔曼滤波/extend_kalman_filter.m

    function [x_filt,x_pre,p_new,k11,k21,h]=extend_kalman_filter(pre_data,z,p_old,ch,Q1,h,l,point_Q2,T)
%x_prev_new=ch*pre_filt_data;
pre_p=ch*p_old*(ch)'+l*Q1*(l)';
k=pre_p*(h)'*inv(h*pre_p*(h)'+point_Q2);
k11=k(1:4,1);
k21=k(1:4,2);
z11=sqrt(pre_data(3)^2+pre_data(1)^2);
z12=atan(pre_data(3)/pre_data(1));
z1=[z11;z12];
x_filt=pre_data+k*(z-z1);
x_pre=ch*x_filt;
p_new=(eye(4)-k*h)*pre_p;

y1=x_pre(1);
y2=x_pre(3);
y3=y1^2+y2^2;
h=[y1/sqrt(y3) 0 y2/sqrt(y3) 0;-y2/y3 0 y1/y3 0];