惯性导航、gps导航工具箱

T=0.02;%步长 a=0;b=60*10;%起始时间与截止时间 t=a:T:b;%仿真时间 num=length(t);%四元数，四元数值0.02秒更新一次 t1=a:T/2:b; num2=length(t1);%存储更新频率为0.01秒的数据 %俯仰角、偏航角、滚转角初始化,0.02秒更新一次 f=zeros(1,num);f(1)=43*pi/180;%在这里设置了俯仰角、滚转角、偏航角的初值 g=zeros(1,num);g(1)=5*pi/180; p=zeros(1,num);p(1)=3*pi/180; %纬度、经度、高度的初始化，0.02秒更新一次 lat=zeros(1,num);lat(1)=0.6*pi/180;%纬度、经度、高度初值的设定 long=zeros(1,num);long(1)=0.6*pi/180; h=zeros(1,num);h(1)=1000; %速度初始化，0.02秒更新一次 vnenx=zeros(1,num);vnenx(1)=0;%初始速度设定 vneny=zeros(1,num);vneny(1)=0; vnenz=zeros(1,num);vnenz(1)=0; %四元数初始化及初值的确定 q0=zeros(1,num); q1=zeros(1,num); q2=zeros(1,num); q3=zeros(1,num); q0(1)=cos(p(1)/2)*cos(f(1)/2)*cos(g(1)/2)-sin(p(1)/2)*sin(f(1)/2)*sin(g(1)/2); q1(1)=cos(p(1)/2)*sin(f(1)/2)*cos(g(1)/2)-sin(p(1)/2)*cos(f(1)/2)*sin(g(1)/2); q2(1)=cos(p(1)/2)*cos(f(1)/2)*sin(g(1)/2)+sin(p(1)/2)*sin(f(1)/2)*cos(g(1)/2); q3(1)=cos(p(1)/2)*sin(f(1)/2)*sin(g(1)/2)+sin(p(1)/2)*cos(f(1)/2)*cos(g(1)/2); Q=sqrt(q0(1)^2+q1(1)^2+q2(1)^2+q3(1)^2); q0(1)=q0(1)/Q;%四元数初值的归一化处理 q1(1)=q1(1)/Q; q2(1)=q2(1)/Q; q3(1)=q3(1)/Q; %wnie,wnen,wbnb初值的确定 wie=7.292115e-005; data1=load('C:\gyro.txt');%读入陀螺仪数据 data1=data1'; wbibx=data1(1,:);wbiby=data1(2,:);wbibz=data1(3,:);%陀螺仪的各个方向的输出 wbnbx=zeros(1,num2);wbnby=zeros(1,num2);wbnbz=zeros(1,num2); wniex=zeros(1,num2);wniey=zeros(1,num2);wniez=zeros(1,num2);%wniex始终为0 wnenx=zeros(1,num2);wneny=zeros(1,num2);wnenz=zeros(1,num2); wninx=zeros(1,num2);wniny=zeros(1,num2);wninz=zeros(1,num2); wniey(1)=wie*cos(lat(1));wniez(1)=wie*sin(lat(1));%wnie初值的确定 wniey(2)=wniey(1);wniez(2)=wniez(1);%纬度更新较慢 wniey(3)=wniey(2);wniez(3)=wniez(2); wnenx(1)=0;wneny(1)=0;wnenz(1)=0;%wnen初值的确定 wnenx(2)=wnenx(1);wneny(2)=wneny(1);wnenz(2)=wnenz(1);%速度更新较慢 wnenx(3)=wnenx(2);wneny(3)=wneny(2);wnenz(3)=wnenz(2); wninx(1)=wniex(1)+wnenx(1);wniny(1)=wniey(1)+wneny(1);wninz(1)=wniez(1)+wnenz(1); wninx(2)=wniex(2)+wnenx(2);wniny(2)=wniey(2)+wneny(2);wninz(2)=wniez(2)+wnenz(2); wninx(3)=wniex(3)+wnenx(3);wniny(3)=wniey(3)+wneny(3);wninz(3)=wniez(3)+wnenz(3); c00=q0(1)^2+q1(1)^2-q2(1)^2-q3(1)^2;%由四元数初值确定导航系到弹体系的转换矩阵的初值 c01=2*(q1(1)*q2(1)+q0(1)*q3(1)); c02=2*(q1(1)*q3(1)-q0(1)*q2(1)); c10=2*(q1(1)*q2(1)-q0(1)*q3(1)); c11=q0(1)^2+q2(1)^2-q1(1)^2-q3(1)^2; c12=2*(q2(1)*q3(1)+q0(1)*q1(1)); c20=2*(q1(1)*q3(1)+q0(1)*q2(1)); c21=2*(q2(1)*q3(1)-q0(1)*q1(1)); c22=q0(1)^2+q3(1)^2-q1(1)^2-q2(1)^2; wbinx(1)=c00*wninx(1)+c01*wniny(1)+c02*wninz(1); wbiny(1)=c10*wninx(1)+c11*wniny(1)+c12*wninz(1); wbinz(1)=c20*wninx(1)+c21*wniny(1)+c22*wninz(1); wbnbx(1)=wbibx(1)-wbinx(1); wbnby(1)=wbiby(1)-wbiny(1); wbnbz(1)=wbibz(1)-wbinz(1); wbnbx(2)=wbibx(2)-wbinx(1);%wbib更新快于wbin更新速度 wbnby(2)=wbiby(2)-wbiny(1); wbnbz(2)=wbibz(2)-wbinz(1); wbnbx(3)=wbibx(3)-wbinx(1); wbnby(3)=wbiby(3)-wbiny(1); wbnbz(3)=wbibz(3)-wbinz(1); %fnib初值的确定 data2=load('C:\accelerometer.txt');%读入加速度计的数据 data2=data2'; fbibx=data2(1,:);fbiby=data2(2,:);fbibz=data2(3,:);%加速度计的输出 fnibx=zeros(1,num2);fniby=zeros(1,num2);fnibz=zeros(1,num2); T00=c00;T01=c10;T02=c20;T10=c01;T11=c11;T12=c21;T20=c02;T21=c12;T22=c22; fnibx(1)=c00*fbibx(1)+c10*fbiby(1)+c20*fbibz(1); fniby(1)=c01*fbibx(1)+c11*fbiby(1)+c21*fbibz(1); fnibz(1)=c02*fbibx(1)+c12*fbiby(1)+c22*fbibz(1); %g、Re确定，在这里认为g为常值 g0=9.78049; Re=6378137; for k=1:num-1 %t(k)即为当前时刻 %解四元数微分方程 K11=-0.5*wbnbx(2*k-1)*q1(k)-0.5*wbnby(2*k-1)*q2(k)-0.5*wbnbz(2*k-1)*q3(k); K21=0.5*wbnbx(2*k-1)*q0(k)+0.5*wbnbz(2*k-1)*q2(k)-0.5*wbnby(2*k-1)*q3(k); K31=0.5*wbnby(2*k-1)*q0(k)-0.5*wbnbz(2*k-1)*q1(k)+0.5*wbnbx(2*k-1)*q3(k); K41=0.5*wbnbz(2*k-1)*q0(k)+0.5*wbnby(2*k-1)*q1(k)-0.5*wbnbx(2*k-1)*q2(k); K12=-0.5*wbnbx(2*k)*(q1(k)+T*K21/2)-0.5*wbnby(2*k)*(q2(k)+T*K31/2)-0.5*wbnbz(2*k)*(q3(k)+T*K41/2); K22=0.5*wbnbx(2*k)*(q0(k)+T*K11/2)+0.5*wbnbz(2*k)*(q2(k)+T*K31/2)-0.5*wbnby(2*k)*(q3(k)+T*K41/2); K32=0.5*wbnby(2*k)*(q0(k)+T*K11/2)-0.5*wbnbz(2*k)*(q1(k)+T*K21/2)+0.5*wbnbx(2*k)*(q3(k)+T*K41/2); K42=0.5*wbnbz(2*k)*(q0(k)+T*K11/2)+0.5*wbnby(2*k)*(q1(k)+T*K21/2)-0.5*wbnbx(2*k)*(q2(k)+T*K31/2); K13=-0.5*wbnbx(2*k)*(q1(k)+T*K22/2)-0.5*wbnby(2*k)*(q2(k)+T*K32/2)-0.5*wbnbz(2*k)*(q3(k)+T*K42/2); K23=0.5*wbnbx(2*k)*(q0(k)+T*K12/2)+0.5*wbnbz(2*k)*(q2(k)+T*K32/2)-0.5*wbnby(2*k)*(q3(k)+T*K42/2); K33=0.5*wbnby(2*k)*(q0(k)+T*K12/2)-0.5*wbnbz(2*k)*(q1(k)+T*K22/2)+0.5*wbnbx(2*k)*(q3(k)+T*K42/2); K43=0.5*wbnbz(2*k)*(q0(k)+T*K12/2)+0.5*wbnby(2*k)*(q1(k)+T*K22/2)-0.5*wbnbx(2*k)*(q2(k)+T*K32/2); K14=-0.5*wbnbx(2*k+1)*(q1(k)+T*K23)-0.5*wbnby(2*k+1)*(q2(k)+T*K33)-0.5*wbnbz(2*k+1)*(q3(k)+T*K43); K24=0.5*wbnbx(2*k+1)*(q0(k)+T*K13)+0.5*wbnbz(2*k+1)*(q2(k)+T*K33)-0.5*wbnby(2*k+1)*(q3(k)+T*K43); K34=0.5*wbnby(2*k+1)*(q0(k)+T*K13)-0.5*wbnbz(2*k+1)*(q1(k)+T*K23)+0.5*wbnbx(2*k+1)*(q3(k)+T*K43); K44=0.5*wbnbz(2*k+1)*(q0(k)+T*K13)+0.5*wbnby(2*k+1)*(q1(k)+T*K23)-0.5*wbnbx(2*k+1)*(q2(k)+T*K33); q0(k+1)=q0(k)+(K11+2*K12+2*K13+K14)*T/6; q1(k+1)=q1(k)+(K21+2*K22+2*K23+K24)*T/6; q2(k+1)=q2(k)+(K31+2*K32+2*K33+K34)*T/6; q3(k+1)=q3(k)+(K41+2*K42+2*K43+K44)*T/6; Q=sqrt(q0(k+1)^2+q1(k+1)^2+q2(k+1)^2+q3(k+1)^2); q0(k+1)=q0(k+1)/Q; q1(k+1)=q1(k+1)/Q; q2(k+1)=q2(k+1)/Q; q3(k+1)=q3(k+1)/Q;%四元数的归一化处理 %矩阵T的更新，每0.02秒更新一次 BT00=T00;BT01=T01;BT02=T02;%T更新前先保存上一时刻的值 BT10=T10;BT11=T11;BT12=T12; BT20=T20;BT21=T21;BT22=T22; T00=q0(k+1)^2+q1(k+1)^2-q2(k+1)^2-q3(k+1)^2; T01=2*(q1(k+1)*q2(k+1)-q0(k+1)*q3(k+1)); T02=2*(q1(k+1)*q3(k+1)+q0(k+1)*q2(k+1)); T10=2*(q1(k+1)*q2(k+1)+q0(k+1)*q3(k+1)); T11=q0(k+1)^2+q2(k+1)^2-q1(k+1)^2-q3(k+1)^2; T12=2*(q2(k+1)*q3(k+1)-q0(k+1)*q1(k+1)); T20=2*(q1(k+1)*q3(k+1)-q0(k+1)*q2(k+1)); T21=2*(q2(k+1)*q3(k+1)+q0(k+1)*q1(k+1)); T22=q0(k+1)^2+q3(k+1)^2-q1(k+1)^2-q2(k+1)^2; %姿态角的解算，每0.02秒更新一次 f(k+1)=asin(T21); if(T22>0) g(k+1)=atan(-T20/T22); else if(T20<0) g(k+1)=atan(-T20/T22)+pi; else g(k+1)=atan(-T20/T22)-pi; end end if(T11>0) p(k+1)=atan(-T01/T11); else if(T01>0) p(k+1)=atan(-T01/T11)+pi; else p(k+1)=atan(-T01/T11)-pi; end end %fnib的更新 fnibx(2*k)=(BT00+T00)/2*fbibx(2*k)+(BT01+T01)/2*fbiby(2*k)+(BT02+T02)/2*fbibz(2*k); fniby(2*k)=(BT10+T10)/2*fbibx(2*k)+(BT11+T11)/2*fbiby(2*k)+(BT12+T12)/2*fbibz(2*k); fnibz(2*k)=(BT20+T20)/2*fbibx(2*k)+(BT21+T21)/2*fbiby(2*k)+(BT22+T22)/2*fbibz(2*k); fnibx(2*k+1)=T00*fbibx(2*k+1)+T01*fbiby(2*k+1)+T02*fbibz(2*k+1); fniby(2*k+1)=T10*fbibx(2*k+1)+T11*fbiby(2*k+1)+T12*fbibz(2*k+1); fnibz(2*k+1)=T20*fbibx(2*k+1)+T21*fbiby(2*k+1)+T22*fbibz(2*k+1); %速度更新，0.02秒更新一次,速度微分方程为非线性微分方程 M11=fnibx(2*k-1)+(2*wie*sin(lat(k))+vnenx(k)*tan(lat(k))/(Re+h(k)))*vneny(k)-(2*wie*cos(lat(k))+vnenx(k)/(Re+h(k)))*vnenz(k); M21=fniby(2*k-1)-(2*wie*sin(lat(k))+vnenx(k)*tan(lat(k))/(Re+h(k)))*vnenx(k)-vneny(k)*vnenz(k)/(Re+h(k)); M31=fnibz(2*k-1)+(2*wie*cos(lat(k))+vnenx(k)/(Re+h(k)))*vnenx(k)+vneny(k)^2/(Re+h(k))-g0; M12=fnibx(2*k)+(2*wie*sin(lat(k))+(vnenx(k)+T*M11/2)*tan(lat(k))/(Re+h(k)))*(vneny(k)+T*M21/2)-(2*wie*cos(lat(k))+(vnenx(k)+T*M11/2)/(Re+h(k)))*(vnenz(k)+T*M31/2); M22=fniby(2*k)-(2*wie*sin(lat(k))+(vnenx(k)+T*M11/2)*tan(lat(k))/(Re+h(k)))*(vnenx(k)+T*M11/2)-(vneny(k)+T*M21/2)*(vnenz(k)+T*M31/2)/(Re+h(k)); M32=fnibz(2*k)+(2*wie*cos(lat(k))+(vnenx(k)+T*M11/2)/(Re+h(k)))*(vnenx(k)+T*M11/2)+(vneny(k)+T*M21/2)^2/(Re+h(k))-g0; M13=fnibx(2*k)+(2*wie*sin(lat(k))+(vnenx(k)+T*M12/2)*tan(lat(k))/(Re+h(k)))*(vneny(k)+T*M22/2)-(2*wie*cos(lat(k))+(vnenx(k)+T*M12/2)/(Re+h(k)))*(vnenz(k)+T*M32/2); M23=fniby(2*k)-(2*wie*sin(lat(k))+(vnenx(k)+T*M12/2)*tan(lat(k))/(Re+h(k)))*(vnenx(k)+T*M12/2)-(vneny(k)+T*M22/2)*(vnenz(k)+T*M32/2)/(Re+h(k)); M33=fnibz(2*k)+(2*wie*cos(lat(k))+(vnenx(k)+T*M12/2)/(Re+h(k)))*(vnenx(k)+T*M12/2)+(vneny(k)+T*M22/