GPS.zip_GPS-INS_INS_ins gps_short1st_sunlight6ew

function [Xw Xa AA] = Data_Genetate(Simu_Mode,Simu_Time,SampleFreq,Coordinate,Para) Xw = []; Xa = []; %%%%%%%%%%%%%%%%%%% load parameter G = Para.G; WIE = Para.WIE; Pos = Para.Pos; %%%%%%%%%%%%%%%%%%%%% N = Simu_Time*SampleFreq; % sample number AA = []; WW = []; switch Simu_Mode case 1 %%%%%%%%%% simulate quite data %添加常值零偏 Xa = ones(N,3)*Para.Acc_Bias*diag([1,1,1])*1 + randn(N,3)*Para.Acc_rand_Bias*diag([1,1,1])*1; Xw = ones(N,3)*Para.Gyro_Bias*diag([1,1,1])*1+ randn(N,3)*Para.Gyro_rand_Bias*diag([1,1,1])*1; AA = zeros(3,N); switch Coordinate case 1 Xa(:,3) = Xa(:,3)-G; %北东地 Xw(:,1) = Xw(:,1) + WIE*cos(Pos(1)); %北向分量 Xw(:,3) = Xw(:,3) - WIE*sin(Pos(1)) ; %地向分量 case 2 Xa(:,3) = Xa(:,3)+G; %东北天（东向没有地球自转角速率分量） Xw(:,2) = Xw(:,2) + WIE*cos(Pos(1)); %北向分量 Xw(:,3) = Xw(:,3) + WIE*sin(Pos(1)) ; %天向分量 otherwise disp('Please choose a valid Coordinate:(1,NED)or(2,ENUJ)') end; case 2 %%%%%%%%%%%%%5 motive simulate W = 2*pi./Para.Periods; %%%%%%%%% omiga； t = 1/SampleFreq:1/SampleFreq:Simu_Time; for i = 1:3 WW = [WW;Para.Swings(i)*W(i)*cos(W(i)*t)]; AA = [AA;Para.Swings(i)*sin(W(i)*t)]; % WW = -[WW;Para.Swings(i)*W(i)*sin(W(i)*t)]; % AA = [AA;Para.Swings(i)*cos(W(i)*t)]; end Lat = Para.Pos(1); Winn = [0;0;0]; Win_b = []; switch Coordinate case 1 %%%%%%%%%%% NED R = AA(1,:); % X roll P = AA(2,:); % Y pitch H = AA(3,:); % Z heading %%%%%%%%%%%%%%5 房建成P169 NED coordinate Wien = Para.WIE*[cos(Lat);0; -sin(Lat)]; %%%%%%%% NED for i = 1:N WW_W = [1 0 -sin(P(i)) ; 0 cos(R(i)) sin(R(i))*cos(P(i)); 0 -sin(R(i)) cos(P(i))*cos(R(i))]; Wnb_b = WW_W*[WW(1,i);WW(2,i);WW(3,i)]; %%%%%%%%%5 房建成P169 Swinb = Cn_b(Swien + SWinn) %%%%%%%%%%5 Cb_n 张天光P28 Cb_n = [cos(P(i))*cos(H(i)) -cos(R(i))*sin(H(i))+sin(R(i))*sin(P(i))*cos(H(i)) sin(R(i))*sin(H(i))+cos(R(i))*sin(P(i))*cos(H(i)); cos(P(i))*sin(H(i)) cos(R(i))*cos(H(i))+sin(R(i))*sin(P(i))*sin(H(i)) -sin(R(i))*cos(H(i))+cos(R(i))*sin(P(i))*sin(H(i)) ; -sin(P(i)) sin(R(i))*cos(P(i)) cos(R(i))*cos(P(i))]; Cn_b = Cb_n'; F_b = Cn_b* [0;0;-G]; Win_b = Cn_b*(Wien + Winn); Xw = [Xw Wnb_b + Win_b]; % Xw = [Xw Wnb_b]; Xa = [Xa F_b]; end case 2 %%%%%%%%% ENU P = AA(1,:); % X pitch R = AA(2,:); % Y roll H = AA(3,:); % Z heading %%%%%%%%%%%%%%5 房建成P169 NED coordinate Wien = Para.WIE*[0;cos(Lat); sin(Lat)]; %%%%%%%% ENU for i = 1:N WW_W = [cos(R(i)) 0 -sin(R(i))*cos(P(i)) ; 0 1 sin(P(i)); sin(R(i)) 0 cos(P(i))*cos(R(i))]; Wnb_b = WW_W*[WW(1,i);WW(2,i);WW(3,i)]; %%%%%%%%%5 房建成P169 Swinb = Cn_b(Swien + SWinn) %%%%%%%%%%5 Cb_n 张天光P28 T = [ cos((R(i)))*cos(H(i))-sin(R(i))*sin(P(i))*sin(H(i)) -cos(P(i))*sin(H(i)) sin(R(i))*cos(H(i))+cos(R(i))*sin(P(i))*sin(H(i)); cos(R(i))*sin(H(i))+sin(R(i))*sin(P(i))*cos(H(i)) cos(P(i))*cos(H(i)) sin(R(i))*sin(H(i))-cos(R(i))*sin(P(i))*cos(H(i)); -sin(R(i))*cos(P(i)) sin(P(i)) cos(R(i))*cos(P(i)) ]; Cn_b = Cb_n'; F_b = Cn_b* [0;0;-G]; end; Win_b = Cn_b*(Wien + Winn); Xw = [Xw Wnb_b + Win_b]; % Xw = [Xw Wnb_b]; Xa = [Xa F_b]; end; Xw = Xw'; Xa = Xa'; case 3 [Xw, Xa] = V2AG('jl.dat','jl.ini',0); % % Xw = [Xw(:,2) Xw(:,1) Xw(:,3)]; % Xw = [Xa(:,2) Xa(:,1) Xa(:,3)]; end; %%%%%%%%%%% longekuta, add two dot Xw = [Xw(1:2,:);Xw]; Xa = [Xa(1:2,:);Xa];