【目标定位】循环增益卡尔曼滤波目标定位仿真【含Matlab源码 145期】.zip

clc; clear; N1=100; thita=1*pi/4; speed=20; T=2.5; point_Q2=[400 0;0 0.64*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; object_init_state=[2 speed*cos(thita) 3 speed*sin(thita)]; for i=1:N1 [real_data,view_data,real_v,Q1]=new_data_get(object_init_state,N,T,l); %前条件：目标的初始状态，目标跟踪次数已知 %后条件：得到目标的观测矩阵，目标的真实值矩阵,真实速度 [filter_data,k1,k2,d,count]=data_kalman_filter(view_data,N,object_init_state,Q1,point_Q2,T,l); %前条件：目标的观测矩阵已知 %后条件：得到目标的滤波矩阵和目标的各次增益 filt_k1(i,:,:)=k1; filt_k2(i,:,:)=k2; all_d(i,:)=d; numberofreset(i)=count; [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); %前条件：得到增益的三维矩阵 %后条件：求出平均增益 final_d=get_d(all_d); [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,N1,real_data,view_data,filter_data,real_v,final_d,numberofreset); %前条件：目标的平均观测误差矩阵，平均滤波误差矩阵已知 %后条件：作出目标的平均观测误差矩阵，平均滤波误差矩阵的仿真图