卡尔曼滤波（KF）+无反馈最优分布式融合

% clear ; % clc; % close all; %% %参数设定 nd = 2;%2表示x和y这个二维平面；3表示x和y这个二维平面 np = 2;%2表示cv模型，有位置和速度两个量，3表示ca模型，有位置、速度和加速度三个量 nx = nd*np; %状态模型维数 nf = 0.1;%噪声强度 nz = 2;%测量维数 N_Q=1;%目标个数（简化目标产生函数） N_R=2;%雷达个数 Ts = 1; N_step = 200; %仿真时长 N_sim = 150; radar_location=[-3e5 0; 0 -3e5; 0 3e5; 3e5 0; 0 0]; H = [1 0 0 0 0 0 1 0]; ini_state(:,1) = [2000, 300, 1000, 100 ]'; R_base = [1000 0; 0 1000]; %对应100000米时候的误差 %% RMSE_sum = 0; RMSE_CI = zeros(1,N_step); RMSE_CI_sum = 0; for k = 1:N_sim % 目标运动设定 [F,Q]= state_tran(Ts,nd,nf,np); X_real= cell(N_R,1); X_real(:) = {zeros(nx,N_step+1)}; for i = 1:N_Q X_real{i} = track(ini_state(:,i),nx,N_step,F,Q); end %% %量测信息更新 Z_real = cell(N_R,N_Q); for i = 1:N_R for j = 1:N_Q Z_real{i,j}= H*X_real{j};%其中h为量测更新函数，为N_R*N_Q的元组 end end %%滤波过程 x_filter = cell(N_R,N_step); x_filter(:) = {zeros(nx,1)}; z_real_every = cell(N_R,N_step); z_real_every(:) = {zeros(nz,1)}; P = cell(N_R,N_step); P(:) = {zeros(nx,nx)}; S = cell(N_R,N_step); S(:) = {zeros(nx,nx)}; RMSE = zeros(N_R,N_step); N_initial = 2;%起始的时刻，cv模型用两个量测值就可以起始，CA模型要用三个时刻 P_CI = cell(1,N_step); P_CI(:) = {zeros(nx,nx)}; X_CI = zeros(nx,N_step); for i1 = 1:N_R z_real_every{i1,N_initial} = Z_real{i1,1}(:,N_initial) + R_cal(R_base,X_real{1}(:,N_initial),radar_location(i1,:))*randn(nz,1); z_real_every{i1,N_initial-1} = Z_real{i1,1}(:,N_initial-1) + R_cal(R_base,X_real{1}(:,N_initial-1),radar_location(i1,:))*randn(nz,1); a1 = z_real_every{i1,N_initial}; %减去对应的雷达位置 a2 = z_real_every{i1,N_initial-1}; x_filter{i1,N_initial}(1,1) = a1(1,1); x_filter{i1,N_initial}(2,1) = (a1(1,1)-a2(1,1))/Ts; x_filter{i1,N_initial}(3,1) = a1(2,1); x_filter{i1,N_initial}(4,1) = (a1(2,1)-a2(2,1))/Ts; x_filter{i1,N_initial-1}(1,1) = a2(1,1); x_filter{i1,N_initial-1}(3,1) = a2(2,1); P{i1,N_initial} = 500*eye(4); end for i = N_initial+1 : N_step for i1 = 1:N_R R_mid = R_cal(R_base,X_real{1}(:,i),radar_location(i1,:)'); z_real_every{i1,i} = Z_real{i1,1}(:,i) + R_cal(R_base,X_real{1}(:,i),radar_location(i1,:))*randn(nz,1); [x_filter{i1,i},P{i1,i}] = KF(x_filter{i1,i-1},z_real_every{i1,i},P{i1,i-1},R_mid,F,Q,H); RMSE(i1,i) = (x_filter{i1,i}(1,1)-X_real{1}(1,i))^2 + (x_filter{i1,i}(3,1)-X_real{1}(3,i))^2; end % 信息融合部分 if i == N_initial+1 P_F_inv = 0; X_c = 0; tr_sum = zeros(1,N_R); for j = 1:N_R tr_sum(j) = trace(P{j,i}); end tr_sum_inv = 1./tr_sum; w = tr_sum_inv/sum(tr_sum_inv,2); for j = 1:N_R P_F_inv = w(j)*(eye(nx)/P{j,i}) + P_F_inv ; X_c = w(j)*(eye(nx)/P{j,i})* x_filter{j,i} + X_c; end P_CI{i} = eye(nx)/P_F_inv; X_CI(:,i) =(eye(nx)/P_F_inv)*X_c; RMSE_CI(1,i) = (X_CI(1,i)-X_real{1}(1,i))^2 + (X_CI(3,i)-X_real{1}(3,i))^2; else P_F_inv = 0; X_f = 0; X_1 = F*X_CI(:,i-1); P_1 = F*P_CI{i-1}*F'+Q; for j = 1:N_R P_F_inv = (eye(nx)/P{j,i}-(eye(nx)/(F*P{j,i-1}*F'+Q))) + P_F_inv ; X_f = ((eye(nx)/P{j,i}*x_filter{j,i})-(eye(nx)/(F*P{j,i-1}*F'+Q)*(F*x_filter{j,i-1}))) + X_f; end P_F_inv = P_F_inv+(eye(nx)/P_1); P_CI{i} = eye(nx)/P_F_inv; X_CI(:,i) =P_CI{i}*(eye(nx)/P_1*X_1+X_f); RMSE_CI(1,i) = (X_CI(1,i)-X_real{1}(1,i))^2 + (X_CI(3,i)-X_real{1}(3,i))^2; end end RMSE_sum = RMSE + RMSE_sum; RMSE_CI_sum =RMSE_CI_sum + RMSE_CI; end RMSE_sum = sqrt(RMSE_sum/N_sim); RMSE_CI_sum = sqrt(RMSE_CI_sum/N_sim); %% %绘图部分 figure; for i = 1:N_R xx1 = cell2mat(x_filter(i,:)); scatter(radar_location(i,1),radar_location(i,2),25,'ro'); hold on; plot(xx1(1,:),xx1(3,:)); hold on; end plot(X_real{1}(1,:),X_real{1}(3,:)); figure plot(RMSE_sum'); hold on ; plot(RMSE_CI_sum'); xlim([N_initial+1,N_step]); legend("1","2",'fusion');