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