www.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;