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

    function [x_filt,x_pre,p_new,k11,k21,Q2]=kalman_filter(pre_data,z,point_z,p_old,ch,Q1,Q2,h,l,point_Q2)
%x_prev_new=ch*pre_filt_data;
e=exp(-point_Q2(2,2))-exp(-point_Q2(2,2)/2);
r=point_z(1);
thita=point_z(2);
z1=z-[r*cos(thita)*e;r*sin(thita)*e];
pre_p=ch*p_old*(ch)'+l*Q1*(l)';
k=pre_p*(h)'*inv(h*pre_p*(h)'+Q2);
k11=k(1:4,1);
k21=k(1:4,2);
x_filt=pre_data+k*(z1-h*pre_data);
x_pre=ch*x_filt;
p_new=(eye(4)-k*h)*pre_p;
cos_2_v=x_filt(1,1)^2/(x_filt(1,1)^2+x_filt(3,1)^2);
sin_2_v=1-cos_2_v;
R_2=x_filt(1,1)^2+x_filt(3,1)^2;
Q2(1,1)=point_Q2(1,1)*cos_2_v+R_2*sin_2_v*point_Q2(2,2);
%Q2(1,2)=sqrt(cos_2_v)*sqrt(sin_2_v)*(point_Q2(1,1)-R_2*point_Q2(2,2));
Q2(1,2)=0;
Q2(2,1)=Q2(1,2);
Q2(2,2)=point_Q2(1,1)*sin_2_v+R_2*cos_2_v*point_Q2(2,2);