基于粒子群的双星定位算法.zip

clc;clear;close all; load Positioning global Mx My Mz Nx Ny Nz Vmx Vmy Vmz Vnx Vny Vnz DF DT; %% 参数设定 nvars = 3; % 个体的变量数目 Re = 6378245; VarRange = 1.2*[-Re-10,0;0,Re+10;-Re-10,Re+10]; mv = 0.2*(VarRange(:,2) - VarRange(:,1));%最大速度取值范围 D = 3; PSOparams = [10 2000 60 2 2 0.9 0.4 1500 1e-100 600 NaN 0 0];%个体数目20，进化次数300 minmax = 0; %% FDOA滤波 figure plot(FDOA(:,2)); lev = 5;%分解层数 FDOA(:,2)=wden(FDOA(:,2),'minimaxi','s','mln',lev,'sym6'); hold on plot(FDOA(:,2)); %% TDOA滤波 figure plot(TDOA(:,2)); lev = 5;%分解层数 TDOA(:,2) = wden(TDOA(:,2),'minimaxi','s','mln',lev,'sym6'); hold on plot(TDOA(:,2)); %% 方程求解 for i = 1:289 DT = TDOA(i,2); DF = FDOA(i,2); Mx = MS(i,2);My = MS(i,3);Mz = MS(i,4); Vmx = MS(i,5);Vmy = MS(i,6);Vmz = MS(i,7); Nx = NS(i,2);Ny = NS(i,3);Nz = NS(i,4); Vnx = NS(i,5);Vny = NS(i,6);Vnz = NS(i,7); [I0,tr,te] = pso_Trelea_vectorized('fitness_up',D,mv,VarRange,minmax,PSOparams,''); II(i,:) = I0(1:3)'; hold on end %% 所求解三维显示 plot3(II(:,1),II(:,2),II(:,3),'*'); error = II(:,1).^2+II(:,2).^2+II(:,3).^2 - (Re+10)^2 %% 坐标变换，WGS84 lla = ecef2lla( II, 'WGS84' ) %大地坐标转地理坐标 ecef = lla2ecef(lla,'WGS84') %地理坐标转大地坐标 save data5