www.gusucode.com > 扩展卡尔曼滤波,粒子滤波,去偏卡尔曼滤波和循环增益尔曼滤波的源程序 > 循环增益卡尔曼滤波/data_kalman_filter.m
function [filter_data,k1,k2,d,count]=data_kalman_filter(view_data,N,object_init_state,Q1,point_Q2,T,l) %定义初试滤波 filter_data0=object_init_state'; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% ch=[1 T 0 0;0 1 0 0;0 0 1 T;0 0 0 1]; %定义状态转移矩阵 h=[1 0 0 0;0 0 1 0]; %观测转移矩阵 %观测误差是随时间变化的所以需计算观测误差 cos_2_v_thita=(object_init_state(1,1)^2)/(object_init_state(1,1)^2+object_init_state(1,3)^2); sin_2_v_thita=1-cos_2_v_thita; R_2=(object_init_state(1,1)^2+object_init_state(1,3)^2); Q2=zeros(2,2); Q2(1,1)=point_Q2(1,1)*cos_2_v_thita+R_2*sin_2_v_thita*point_Q2(2,2); %Q2(1,2)=sqrt(cos_2_v_thita)*sqrt(sin_2_v_thita)*(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_thita+R_2*cos_2_v_thita*point_Q2(2,2); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% pre_data(:,1)=ch*filter_data0; p_init=100*eye(4); %初始协方差定义 p_old=p_init; Q0=[0.09 0;0 0.09]; %最大机动漂移假设 count=0; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %根据初始预测计算滤波值 %通过输入观测值进行卡尔曼滤波 [filter_data(:,1),pre_data(:,2),p_new,k11,k21,Q2,v,d(1)]=kalman_filter(pre_data(:,1),view_data(:,1),p_old,ch,Q1,Q2,h,l,point_Q2); p_old=p_new; k1(:,1)=k11; k2(:,1)=k21; for i=2:N if d(i-1)<2.3 k=[k11 k21]; [filter_data(:,i),pre_data(:,i+1),p_new,k11,k21,Q2,v,d(i)]=constant_gain_filter(pre_data(:,i),view_data(:,i),p_old,ch,Q1,Q2,h,l,point_Q2,k); p_old=p_new; else [filter_data(:,i),pre_data(:,i+1),p_new,k11,k21,Q2,v,d(i)]=kalman_filter(pre_data(:,i),view_data(:,i),p_old,ch,Q1,Q2,h,l,point_Q2); p_old=p_new; count=count+1; v1(:,i)=v; if(i>=4) signv(:,1)=sign(v1(:,i-3)); signv(:,2)=sign(v1(:,i-2)); signv(:,3)=sign(v1(:,i-1)); signv(:,4)=sign(v1(:,i)); m=abs(diag(signv(:,1)+signv(:,2)+signv(:,3)+signv(:,4))); Q1=Q1+m.*Q0/4; end end k1(:,i)=k11; k2(:,i)=k21; end