惯导代码,惯导是什么,matlab

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %% 指北方位捷联式惯性导航系统 %% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %初始化 clear load('jlfw'); %定义常量 Re=6378245; e=1/298.3; wie_scalar=7.292115147e-5; g0=9.7803267714; gk1=0.00193185138639; gk2=0.00669437999013; %变量赋初值 longitude=[116.344695283*pi/180 zeros(1,47999)]; latitude=[39.975172*pi/180 zeros(1,47999)]; h=30; f=zeros(3,48000); wnb=zeros(3,48000); theta=[0.120992605 zeros(1,47999)]*pi/180; gamma=[0.010445947 zeros(1,47999)]*pi/180; psi=[360-91.637207 zeros(1,47999)]*pi/180; Q=[cos(psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2)+sin(psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2); cos(psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2)+sin(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2); cos(psi(1)/2)*cos(theta(1)/2)*sin(gamma(1)/2)-sin(psi(1)/2)*sin(theta(1)/2)*cos(gamma(1)/2); cos(psi(1)/2)*sin(theta(1)/2)*sin(gamma(1)/2)-sin(psi(1)/2)*cos(theta(1)/2)*cos(gamma(1)/2)]; velocity=[0.000048637 0.000206947 0.007106781]; v=[zeros(1,48000)]; for i=2:48000 %计算所在地参数 %Rx为卯酉圈的曲率半径 Rx=Re/(1-e*sin(latitude(i-1))^2); %Ry为子午圈的曲率半径 Ry=Re/(1+2*e-3*e*sin(latitude(i-1))^2); %使用WGS-84模型计算该位置的重力加速度 g=g0*(1+gk1*sin(latitude(i-1))^2)*(1-2*h/Re)/sqrt(1-gk2*sin(latitude(i-1))^2); %计算姿态矩阵 Cnb=[Q(1)^2+Q(2)^2-Q(3)^2-Q(4)^2 2*(Q(2)*Q(3)-Q(1)*Q(4)) 2*(Q(2)*Q(4)+Q(1)*Q(3)); 2*(Q(2)*Q(3)+Q(1)*Q(4)) Q(1)^2-Q(2)^2+Q(3)^2-Q(4)^2 2*(Q(3)*Q(4)-Q(1)*Q(2)); 2*(Q(2)*Q(4)-Q(1)*Q(3)) 2*(Q(3)*Q(4)+Q(1)*Q(2)) Q(1)^2-Q(2)^2-Q(3)^2+Q(4)^2]; Cbn=[Q(1)^2+Q(2)^2-Q(3)^2-Q(4)^2 2*(Q(2)*Q(3)+Q(1)*Q(4)) 2*(Q(2)*Q(4)-Q(1)*Q(3)); 2*(Q(2)*Q(3)-Q(1)*Q(4)) Q(1)^2-Q(2)^2+Q(3)^2-Q(4)^2 2*(Q(3)*Q(4)+Q(1)*Q(2)); 2*(Q(2)*Q(4)+Q(1)*Q(3)) 2*(Q(3)*Q(4)-Q(1)*Q(2)) Q(1)^2-Q(2)^2-Q(3)^2+Q(4)^2]; %使用毕卡法（角增量法）求解四元数微分方程 win=[-velocity(2)/Ry;wie_scalar*cos(latitude(i-1))+velocity(1)/Rx; wie_scalar*sin(latitude(i-1))+velocity(1)/Rx*tan(latitude(i-1))]; wnb(:,i)=wib_INSc(:,i)-Cbn*win; delta_big_theta=[0 -wnb(1,i) -wnb(2,i) -wnb(3,i); wnb(1,i) 0 wnb(3,i) -wnb(2,i); wnb(2,i) -wnb(3,i) 0 wnb(1,i); wnb(3,i) wnb(2,i) -wnb(1,i) 0]/80; delta_theta=sqrt((wnb(1,i)/80)^2+(wnb(2,i)/80)^2+(wnb(3,i)/80)^2); Q=(eye(4)*(1-delta_theta^2/8+delta_theta^4/384)+(1/2-delta_theta^2/48)*delta_big_theta)*Q; %计算姿态角 theta(i)=asin(Cbn(2,3)); gamma(i)=atan(-Cbn(1,3)/Cbn(3,3)); psi(i)=atan(Cbn(2,1)/Cbn(2,2)); if Cbn(2,2)<0 psi(i)=psi(i)+pi; else if Cbn(2,1)<0 psi(i)=psi(i)+2*pi; end end if Cbn(3,3)<0 if gamma(i)>0 gamma(i)=gamma(i)-pi; else gamma(i)=gamma(i)+pi; end end %计算速度与经纬度 f(:,i)=Cnb*f_INSc(:,i); ax=f(1,i)+2*wie_scalar*sin(latitude(i-1))*velocity(2)+velocity(1)*velocity(2)*tan(latitude(i-1))/Rx; ay=f(2,i)-2*wie_scalar*sin(latitude(i-1))*velocity(1)-velocity(1)^2*tan(latitude(i-1))/Rx; average=velocity; velocity=velocity+[ax ay 0]/80; average=(average+velocity)/2; %速度 v(i)=sqrt(average(1)^2+average(2)^2); %经度longitude,纬度latitude longitude(i)=longitude(i-1)+average(1)/Rx*sec(latitude(i-1))/80; latitude(i)=latitude(i-1)+average(2)/Ry/80; end %将弧度转化为角度 longitude=longitude*180/pi; latitude=latitude*180/pi; theta=theta*180/pi; gamma=gamma*180/pi; psi=ones(1,48000)*360-psi*180/pi; %作图 %经纬度曲线 figure; title('位置曲线'); xlabel('经度'); ylabel('纬度 ','Rotation',0); hold on axis([1.163446e+002,1.163448e+002,39.9750,39.9752]); plot(longitude,latitude,'r','LineWidth',3); plot(longitude(20000),latitude(20000),'r>','MarkerFaceColor','r'); plot(longitude(45000),latitude(45000),'rv','MarkerFaceColor','r'); %速度曲线 figure; title('速度曲线'); xlabel('时间(s)'); ylabel('速度(m/s) ','Rotation',0); grid on hold on plot([0:1/80:600 - 1/80],v,'r','LineWidth',3); %姿态角曲线 figure; hold on subplot(311) ,plot([0:1/80:600 - 1/80],psi,'r','LineWidth',0.5);title('航向角曲线');ylabel(' 航向角(度) ','Rotation',0);grid on; subplot(312),plot([0:1/80:600 - 1/80],theta,'r','LineWidth',0.5);title('俯仰角曲线');ylabel('俯仰角(度) ','Rotation',0);grid on; subplot(313),plot([0:1/80:600 - 1/80],gamma,'r','LineWidth',0.5);title('滚转角曲线');ylabel(' 滚转角(度) ','Rotation',0);grid on; xlabel('时间(s)');