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

    clc;
clear;
N1=100;
thita=1*pi/4;
speed=20;
T=2.5;
point_Q2=[400 0;0 1*pi^2/180^2];
%Q1=[0.36 0;0 0];     %添加机动情形
%Q1=[0 0;0 0];        %不添加机动
l=[1/2*T^2 0;T 0;0 1/2*T^2;0 T];
N=200;
r=1000;
r_thita=1/3*pi;
object_init_state=[r*cos(r_thita) speed*cos(thita) r*sin(r_thita) speed*sin(thita)];   
for i=1:N1
    [real_data,view_data,real_v,Q1,point_view_data]=new_data_get(object_init_state,N,T,l);                   
    %前条件：目标的初始状态，目标跟踪次数已知
    %后条件：得到目标的观测矩阵，目标的真实值矩阵,真实速度
    [filter_data,k1,k2]=data_kalman_filter(view_data,point_view_data,N,object_init_state,Q1,point_Q2,T,l);
    %前条件：目标的观测矩阵已知
    %后条件：得到目标的滤波矩阵和目标的各次增益
    filt_k1(i,:,:)=k1;
    filt_k2(i,:,:)=k2;
    [ME_temp_view_err_x(i,1:N),ME_temp_filter_err_x(i,1:N),ME_temp_view_err_y(i,1:N),ME_temp_filter_err_y(i,1:N)]=temp_err_save(real_data,...
    filter_data,view_data);
    %前条件：目标的真实值矩阵，目标的滤波矩阵，目标的观测矩阵已知
    %后条件：本次目标的观测误差，滤波误差得到存储
end
[final_filt_k1,final_filt_k2]=filt_count(filt_k1,filt_k2,N1);
%前条件：得到增益的三维矩阵
%后条件：求出平均增益
[ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y]=ME_err_count(ME_temp_view_err_x,ME_temp_filter_err_x,...
ME_temp_view_err_y,ME_temp_filter_err_y);
%前条件：目标的观测误差矩阵，目标的滤波误差矩阵已知
%后条件：得到目标的平均观测误差矩阵，平均滤波误差矩阵
[RMSE_final_view_err_x,RMSE_final_filter_err_x,RMSE_final_view_err_y,RMSE_final_filter_err_y]=RMSE_err_count(ME_temp_view_err_x,...
ME_temp_filter_err_x,ME_temp_view_err_y,ME_temp_filter_err_y,ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y);
%后条件：得到目标的均方观测误差矩阵，均方滤波误差矩阵
%show(ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y,final_filt_k1,final_filt_k2,N,real_data,view_data,filter_data,real_v);
show(ME_final_view_err_x,ME_final_filter_err_x,ME_final_view_err_y,ME_final_filter_err_y,RMSE_final_view_err_x,RMSE_final_filter_err_x,...
RMSE_final_view_err_y,RMSE_final_filter_err_y,final_filt_k1,final_filt_k2,N,real_data,view_data,filter_data,real_v);
%前条件：目标的平均观测误差矩阵，平均滤波误差矩阵已知
%后条件：作出目标的平均观测误差矩阵，平均滤波误差矩阵的仿真图