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

    function [x_filt,x_pre,p_new,k11,k21,Q2,v,d]=constant_gain_filter(pre_data,z,p_old,ch,Q1,Q2,h,l,point_Q2,k)

pre_p=ch*p_old*(ch)'+l*Q1*(l)';
s=inv(h*pre_p*(h)'+Q2);
k11=k(1:4,1);
k21=k(1:4,2);
v=(z-h*pre_data);
x_filt=pre_data+k*v;
x_pre=ch*x_filt;
%p_new=(eye(4)-k*h)*pre_p*(eye(4)-k*h)'+k*Q2*k';
p_new=p_old;
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);
d=v'*s*v;