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

    function [filter_data,k1,k2]=data_kalman_filter(view_data,point_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;
x1=pre_data(1,1);
x2=pre_data(3,1);
x3=x1^2+x2^2;
h=[180*x2/(x3*pi) 0 180*x1/(x3*pi) 0;x1/sqrt(x3) 0 x2/sqrt(x3) 0];
p_init=100*eye(4);                                                   %初始协方差定义 
p_old=p_init;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%根据初始预测计算滤波值
%通过输入观测值进行卡尔曼滤波
for i=1:N
    [filter_data(:,i),pre_data(:,i+1),p_new,k11,k21,h]=extend_kalman_filter(pre_data(:,i),point_view_data(:,i),p_old,ch,Q1,h,l,point_Q2,T);
    p_old=p_new;
    k1(:,i)=k11;
    k2(:,i)=k21;
end