79419109ins-gps.zip

clc close all clear all format long %% 数据读取 load('IMU.dat'); load('GPS.dat'); f_INSc=IMU(1:end-1,6:8)'; wib_INSc=IMU(1:end-1,3:5)'*pi/180/3600; P_GPS=GPS(2:end,3:5)'; V_GPS=GPS(2:end,6:8)'; n=length(IMU);N=length(GPS);%数据个数 %% 初值设定 L=34.6521606250000*pi/180; %初始纬度 J=109.249623717000*pi/180; %初始经度 h=362.269000000000; %初始高度(m) Wie=7.292115147e-5; %地球自转角速度(rad/s) hangxiang=303.10881*pi/180; %初始航向角 fuyang=0.25516*pi/180; %初始俯仰角 henggun=1.76037*pi/180; %初始横滚角 T=0.01;%采样周期(s) Re=6378245 ; e=1/298.3 ; Vx=0; %初始东向水平速度(m/s) Vy=0.00900000000000000; %初始北向水平速度(m/s) Vz=0.0350000000000000; %初始天向速度(m/s) g0=9.7803267714; gk1=0.00193185138639; gk2=0.00669437999013; W=zeros(3,n-1); %用于记录实时三轴指令角速度 V=zeros(3,n-1); %用于记录实时东北天速度 attitude=zeros(3,n-1); %姿态角 Alt=zeros(3,N-1); pos=zeros(3,N-1); vel=zeros(3,N-1); q=zeros(4,1); %四元数 q(1)=cos(hangxiang/2)*cos(fuyang/2)*cos(henggun/2)-sin(hangxiang/2)*sin(fuyang/2)*sin(henggun/2); q(2)=cos(hangxiang/2)*sin(fuyang/2)*cos(henggun/2)-sin(hangxiang/2)*cos(fuyang/2)*sin(henggun/2); q(3)=cos(hangxiang/2)*cos(fuyang/2)*sin(henggun/2)+sin(hangxiang/2)*sin(fuyang/2)*cos(henggun/2); q(4)=cos(hangxiang/2)*sin(fuyang/2)*sin(henggun/2)+sin(hangxiang/2)*cos(fuyang/2)*cos(henggun/2); PP=zeros(15,N-1); X0=zeros(15,1); P0=eye(15,15); X=zeros(15,N-1); %% 捷联结算 for i=1:N-1 for t=(i-1)*5+1:(i-1)*5+5; 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;]; g=g0*(1+gk1*sin(L)^2)*(1-2*h/Re)/sqrt(1-gk2*sin(L)^2); ft=Cnb'*f_INSc(:,t)*g; %本体系下的比力转换到导航系 Rxt=(1-e*(sin(L)^2))/Re; Ryt=(1+2*e-3*e*(sin(L)^2))/Re; Dvx=ft(1)+2*Wie*sin(L)*Vy+Vx*Vy*Rxt*tan(L)-(2*Wie*cos(L)+Vx*Rxt)*Vz; %东向加速度 Dvy=ft(2)-2*Wie*sin(L)*Vx-Vx^2*Rxt*tan(L)-Vy*Ryt*Vz; %北向加速度 Dvz=ft(3)+(2*Wie*cos(L)+Vx*Rxt)*Vx+Vy*Ryt*Vy-g; %天向加速度 Vx=Vx+Dvx*T; %东向速度 Vy=Vy+Dvy*T; %北向速度 Vz=Vz+Dvz*T; %北向速度 V(:,t)=[Vx,Vy,Vz]'; %实时速度记录 L=Vy*Ryt*T+L; %地理纬度 J=Vx*Rxt*sec(L)*T+J; %地理经度 h=h+Vz*T; W(1,t)=-Vy*Ryt; %平台三轴指令角速度实时计算 W(2,t)=Wie*cos(L)+Vx*Rxt; W(3,t)=Wie*sin(L)+Vx*Rxt*tan(L); Witb=Cnb*W(:,t); %指令角速度转换到本体系 w=wib_INSc(:,t)-Witb; %本体系相对于导航系的角速度 dst=w*T; %角增量法解四元数微分方程 dst0=norm(dst); Dst=[ 0 -dst(1) -dst(2) -dst(3); dst(1) 0 dst(3) -dst(2); dst(2) -dst(3) 0 dst(1); dst(3) dst(2) -dst(1) 0;]; q=((1-dst0^2/8-dst0^4/348)*eye(4,4)+(.5-dst0^2/48)*Dst)*q; attitude(1,t)=asin(Cnb(2,3)); attitude(2,t)=atan(-Cnb(1,3)/Cnb(3,3)); attitude(3,t)=atan(-Cnb(2,1)/Cnb(2,2)); %% 多值处理 if(abs(Cnb(2,2))<1e-8) %航向角多值处理 if(Cnb(2,1)>0) attitude(3,t)=-pi/2; else attitude(3,t)=pi/2; end else if(Cnb(2,2)>0) if(attitude(3,t)<0) attitude(3,t)=attitude(3,t)+2*pi; end else attitude(3,t)=attitude(3,t)+pi; end end if(abs(Cnb(3,3))<1e-8) %横滚角多值处理 if(Cnb(1,3)>0) attitude(2,t)=-pi/2; else attitude(2,t)=pi/2; end else if(Cnb(3,3)<0) if(attitude(2,t)>0) attitude(2,t)=attitude(2,t)-pi; else attitude(2,t)=attitude(2,t)+pi; end end end end %% 滤波处理 Rn=1/Rxt+h; Rm=1/Ryt+h; Q=diag([(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(0.1*pi/180/3600)^2,(5e-5*g)^2,(5e-5*g)^2,(5e-5*g)^2,0,0,0,0,0,0,0,0,0]); R=diag([(0.01)^2,(0.01)^2,(0.01)^2,(0.1/Rm)^2,(0.1/(Rn*cos(L)))^2,0.15^2]); Fn=[ 0,Wie*sin(L)+Vx*tan(L)/Rn,-(Wie*cos(L)+Vx/Rn),0,-1/Rm,0,0,0,Vy/Rm^2; -(Wie*sin(L)+Vx*tan(L)/Rn),0,-Vy/Rm,1/Rn,0,0,-Wie*sin(L),0,-Vx/Rn^2; Wie*cos(L)+Vx/Rn,Vy/Rm,0,tan(L)/Rn,0,0,(Wie*cos(L)+Vx*(sec(L))^2/Rn),0,-Vx*tan(L)/Rn^2; 0,-ft(3),ft(2),(Vx*tan(L)-Vz)/Rn,(2*Wie*sin(L)+Vx*tan(L)/Rn),-(2*Wie*cos(L)+Vx/Rn),(2*Wie*Vy*cos(L)+Vx*Vy*(sec(L))^2/Rn+2*Wie*Vz*sin(L)),0,(Vx*Vz-Vx*Vy*tan(L))/Rn^2; ft(3),0,-ft(1),-2*(Wie*sin(L)+Vx*tan(L)/Rn),-Vz/Rm,-Vy/Rm,-(2*Wie*cos(L)+Vx*(sec(L))^2/Rn)*Vx,0,(Vy*Vz+Vx*Vx*tan(L))/Rn^2; -ft(2),ft(1),0,2*(Wie*cos(L)+Vx/Rn),2*Vy/Rm,0,-2*Wie*Vx*sin(L),0,-(Vx*Vx+Vy*Vy)/Rn^2; 0,0,0,0,1/Rm,0,0,0,Vy/Rm^2; 0,0,0,sec(L)/Rn,0,0,Vx*sec(L)*tan(L)/Rn,0,-Vx*sec(L)/Rn^2; 0,0,0,0,0,1,0,0,0;]; Fs=[Cnb',zeros(3,3); zeros(3,3),Cnb'; zeros(3,3),zeros(3,3);]; f=[Fn,Fs;zeros(6,15)]; F=eye(15,15)+f*0.05+f^2*0.05^2/2; Z=[Vx,Vy,Vz,L,J,h]'-[V_GPS(:,i);P_GPS(1,i)*pi/180;P_GPS(2,i)*pi/180;P_GPS(3,i);]; H=[zeros(3,3),eye(3,3),zeros(3,9); zeros(3,6),eye(3,3),zeros(3,6);]; X10=F*X0; P10=F*P0*F'+Q; Kk=P10*H'/(H*P10*H'+R); X0=X10+Kk*(Z-H*X10); P0=(eye(15,15)-Kk*H)*P10; Ctn=[ 1 X0(3) -X0(2); -X0(3) 1 X0(1); X0(2) -X0(1) 1 ;]; Cnb = Cnb*Ctn; Vx=Vx-X0(4); Vy=Vy-X0(5); Vz=Vz-X0(6); L=L-X0(7); J=J-X0(8); h=h-X0(9); pos(:,i)=[L*180/pi;J*180/pi;h]; vel(:,i)=[Vx;Vy;Vz]; Alt(1,i)=asin(Cnb(2,3)); Alt(2,i)=atan(-Cnb(1,3)/Cnb(3,3)); Alt(3,i)=atan(-Cnb(2,1)/Cnb(2,2)); if(abs(Cnb(2,2))<1e-8) %航向角多值处理 if(Cnb(2,1)>0) Alt(3,i)=-pi/2; else Alt(3,i)=pi/2; end else if(Cnb(2,2)>0) if(Alt(3,i)<0) Alt(3,i)=Alt(3,i)+2*pi; end else Alt(3,i)=Alt(3,i)+pi; end end if(abs(Cnb(3,3))<1e-8) %横滚角多值处理 if(Cnb(1,3)>0) Alt(2,i)=-pi/2; else Alt(2,i)=pi/2; end else if(Cnb(3,3)<0) if(Alt(2,i)>0) Alt(2,i)=Alt(2,i)-pi; else Alt(2,i)=Alt(2,i)+pi; end end end PP(1,i)=sqrt(P0(1,1)); PP(2,i)=sqrt(P0(2,2)); PP(3,i)=sqrt(P0(3,3)); PP(4,i)=sqrt(P0(4,4)); PP(5,i)=sqrt(P0(5,5)); PP(6,i)=sqrt(P0(6,6)); PP(7,i)=sqrt(P0(7,7)); PP(8,i)=sqrt(P0(8,8)); PP(9,i)=sqrt(P0(9,9)); PP(10,i)=sqrt(P0(10,10)); PP(11,i)=sqrt(P0(11,11));PP(12,i)=sqrt(P0(12,12));PP(13,i)=sqrt(P0(13,13));PP(14,i)=sqrt(P0(14,14));PP(15,i)=sqrt(P0(15,15)); X(:,i)=X0; end %% 绘制图形 t=(1:N-1)*0.05;