角速度与加速度仿真,角速度角加速度,matlab

clear clc w_ie=7.2921151467e-5; %地球自转角速度 %% 一、变量区 %% %psi：航向角Ψ，theta：俯仰角θ，gamma：横滚角γ，单位：° psi_deg0=30; %航向30° psi_T=10;psi_range=5; %航向角周期10s,幅度±5° th_T=8;th_range=8; %俯仰角周期10s,幅度±8° ga_T=6;ga_range=12; %横滚角周期6s,幅度±12° L_deg0=30.0058; %初始纬度：30.0058°N lamda_deg0=114.14; %初始经度：114.14°E interT=0.01; %数据时间间隔0.01s T=0:interT:3600; %总仿真时间：1小时=3600s Acc=1;T_acc=10; %加速度1m/s^2,持续10s %% 二、速度与姿态角赋值区 %% n=length(T); L_rad0=L_deg0*(pi/180); %初始纬度（单位：弧度） lamda_rad0=lamda_deg0*(pi/180); %初始经度（单位：弧度） detVT=zeros(1,n); detVT(T<T_acc)=Acc; %T<T_acc(s)时加速度为Acc (m/s^2) VTmax=Acc*T_acc; %达到的最大速度 VT=ones(1,n).*VTmax; %VT:速度向量 VT(T<=T_acc)=0:interT*Acc:VTmax; Ve=VT.*sin(psi_deg0*pi/180); Vn=VT.*cos(psi_deg0*pi/180); Vu=zeros(1,n); V_acc=[detVT.*sin(psi_deg0*pi/180);%平台坐标系内VT矢量变化率E向分量 detVT.*cos(psi_deg0*pi/180);%平台坐标系内VT矢量变化率N向分量 zeros(1,n)]; %平台坐标系内VT矢量变化率U向分量 psi_deg=psi_range.*sin((2*pi/psi_T).*T)+psi_deg0;%航向角Ψ，单位：° th_deg=th_range.*sin((2*pi/th_T).*T); %俯仰角θ，单位：° ga_deg=ga_range.*sin((2*pi/ga_T).*T); %横滚角γ，单位：° diff_psi=(psi_range*pi*pi/psi_T/90).*cos((2*pi/psi_T).*T);%单位：rad/s,2*pi/psi_T*psi_range*pi/180=psi_range*pi*pi/psi_T/90 diff_th=(th_range*pi*pi/th_T/90).*cos((2*pi/th_T).*T); %单位：rad/s diff_ga=(ga_range*pi*pi/ga_T/90).*cos((2*pi/ga_T).*T); %单位：rad/s %% 三、求fb和Wibb %% Wnbb = diffAtt2Wnbb([th_deg;ga_deg],[diff_psi;diff_th;diff_ga]); Wibb=zeros(3,n); L_rad=zeros(1,n); lamda_rad=zeros(1,n); Rn=zeros(1,n); Rm=zeros(1,n); g_L=zeros(1,n); fT=zeros(3,n); fb=zeros(3,n); for i=1:n if i~=1 [ Rn(i),Rm(i),g_L(i),L_rad(i),lamda_rad(i)] = update_geoinf(interT,L_rad(i-1),lamda_rad(i-1),Vn(i-1:i),Ve(i-1:i),Rn(i-1),Rm(i-1) ); else [ Rn(1),Rm(1),g_L(1),L_rad(1),lamda_rad(1)] = update_geoinf(interT,L_rad0,lamda_rad0); end Cbn =AttitudeAngle2Cbn( psi_deg(i),th_deg(i),ga_deg(i)); WieT=[0;w_ie*cos(L_rad(i));w_ie*sin(L_rad(i))]; WeTT=[-Vn(i)/Rm(i);Ve(i)/Rn(i);Ve(i)/Rn(i)*tan(L_rad(i))]; Wibb(:,i)=Wnbb(:,i)+Cbn'*(WieT+WeTT); WeT=2*WieT+WeTT; % 方法二，调用Wnbb2Wibb函数求Wibb %Wibb(:,i)=Wnbb2Wibb( Wnbb(:,i),Cbn,Vn(i),Ve(i),L_rad(i)*180/pi); %WeT=[-Vn(i)/Rm(i);2*w_ie*cos(L_rad(i))+Ve(i)/Rn(i);2*w_ie*sin(L_rad(i))+Ve(i)/Rn(i)*tan(L_rad(i))];%2WieT+WeTT fT(:,i)=V_acc(:,i)+cross(WeT,[Ve(i);Vn(i);Vu(i)])+[0;0;g_L(i)];%有害加速度：(2WieT+WeTT)×VeTT=cross(WeT,[Ve(i);Vn(i);Vu(i)]) fb(:,i)=Cbn'*fT(:,i); end %% save Data_fb_Wibb fb Wibb T;