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

% clear ; % clc; % close all; %% %参数设定 nd = 2;%2表示x和y这个二维平面；3表示x和y这个二维平面 np = 2;%2表示cv模型，有位置和速度两个量，3表示ca模型，有位置、速度和加速度三个量 nx = nd*np; %状态模型维数 nf = 0.001;%噪声强度 nz = 2;%测量维数 N_Q=1;%目标个数（简化目标产生函数） N_R=2;%雷达个数 Ts = 1; N_step = 200; %仿真时长 N_sim = 150; radar_location=[-3000 0; 0 -3000]; ini_state(:,1) = [2000, 300, 1000, 100 ]'; R_base = [1000 0; 0 0.00001]; %对应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},radar_location(i,:));%其中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 = h_inv(z_real_every{i1,N_initial})+radar_location(i1,:)'; %减去对应的雷达位置 a2 = h_inv(z_real_every{i1,N_initial-1})+radar_location(i1,:)'; 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} = 10000*eye(4); %trans(X_real{1}(:,N_initial),R_cal(R_base,X_real{1}(:,N_initial),radar_location(i1,:)')); 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}] = UKF(x_filter{i1,i-1},z_real_every{i1,i},P{i1,i-1},R_mid,F,Q,radar_location(i1,:)); 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}) + P_F_inv ; X_f = (eye(nx)/P{j,i})*(x_filter{j,i}-X_1) + X_f; end P_F_inv = P_F_inv-((N_R-1)*(eye(nx)/P_1)); P_CI{i} = eye(nx)/P_F_inv; X_CI(:,i) = X_1 + P_CI{i}*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 for j =1:N_R x_filter{j,i} = X_CI(:,i); P{j,i} = P_CI{i}; 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); legend("1","2","3","4",'CI'); xlim([0,N_step]); ylim([0,100]); %自定义函数 % function error_output = trans(mes,R) % % r = sqrt(mes(1)^2+mes(2)^2); % theta = atan2(mes(2),mes(1)); % e11 = cos(theta)^2*R(1,1)+r^2*sin(theta)^2*R(2,2); % e22 = sin(theta)^2*R(1,1)+r^2*cos(theta)^2*R(2,2); % e12 = sin(theta)*cos(theta)*R(1,1)-r^2*sin(theta)*cos(theta)*R(2,2); % error_output = [e11 e12 % e12 e22]; % end