指北方位系统_捷联惯性导航系统_matlab模拟算法_imu

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %陀螺仪及加速度计信息提取 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% clc DATAW=textread('f:\data1\imu_ENU.txt'); %陀螺仪数据表格 DATAf=textread('f:\data1\ins_c.txt'); %加速度计数据表格 Wib1=DATAW(:,(5:7));%提取角速度 fb1=DATAf(:,(8:10));%提取加速度 sizeDATAW=size(DATAW); sizeDATAf=size(DATAf); clear DATA; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %基本初始化参数 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% wie=7.2722e-5;%地球自转角速度 Re=6378137.0;%地球半径 f=1.0/298.257;%地球扁率 g0=9.7803;%重力加速度 wie=7.292*10^-5; Wie=[0;0;wie];%Wiep初始化 Wep=[0;0;0];%Wep初始化 deg=180/pi; rad=pi/180; T=0.01;%采样周期 temp=1;%临时变量（下同） tempW=1; tempV=1; flag=1; i=1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %位置矩阵、四元数、姿态矩阵初始化 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% lambd(tempW,1)=DATAW(1,2); lambdm=lambd(tempW,1)*rad;%初始经度 L(tempW,1)=DATAW(1,3); Lm=L(tempW,1)*rad;%初始纬度 h(tempV,1)=DATAf(1,4);%初始高度 V=[DATAf(1,5);DATAf(1,6);DATAf(1,7)];%初始速度 psi(temp,1)=DATAf(1,11); psim=psi(temp,1)*rad;%偏航角 theta(temp,1)=DATAf(1,12); thetam=theta(temp,1)*rad;%俯仰角 gamma(temp,1)=DATAf(1,13); gammam=gamma(temp,1)*rad;%滚动角 C=[-sin(lambdm),cos(lambdm),0; -sin(Lm)*cos(lambdm),-sin(Lm)*sin(lambdm),cos(Lm); cos(Lm)*cos(lambdm),cos(Lm)*sin(lambdm),sin(Lm)];%初始位置矩阵 q=[cos(psim/2)*cos(thetam/2)*cos(gammam/2)-sin(psim/2)*sin(thetam/2)*sin(gammam/2); cos(psim/2)*sin(thetam/2)*cos(gammam/2)-sin(psim/2)*cos(thetam/2)*sin(gammam/2); cos(psim/2)*cos(thetam/2)*sin(gammam/2)+sin(psim/2)*sin(thetam/2)*cos(gammam/2); cos(psim/2)*sin(thetam/2)*sin(gammam/2)+sin(psim/2)*cos(thetam/2)*cos(gammam/2)];%初始四元数 Cbp=[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];%初始姿态矩阵 % % 陀螺的误差设置：陀螺的常值漂移delta_w=0.02度每小时,随机干扰w_randomerror=0.01度每小时 % delta_w=pi*0.1/180/3600; % w_randomerror=pi*0.01/180/3600*randn(3*data_num,1); % % 加速度计的误差设置：加速度计的偏置量delta_a=1e-4*g,随机干扰a_randomerror=(5e-5*randn(3*data_num,1))*g % delta_a=1e-4*g; % a_randomerror=(1e-5*randn(3*data_num,1))*g; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %导航参数更新 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% while i<=sizeDATAf(1,1)-1 Wib=Wib1(i,:)';%输入陀螺仪测得的角速度 Wpb=Wib-Cbp'*(Wep+Wie);%计算姿态矩阵速率 %g0=(g0+0.051799*C(3,3)^2); %四元数更新（四阶龙格-库塔） W=0.5*[0,-Wpb(1),-Wpb(2),-Wpb(3); Wpb(1),0,Wpb(3),-Wpb(2); Wpb(2),-Wpb(3),0,Wpb(1); Wpb(3),Wpb(2),-Wpb(1),0]; switch(flag) case(1) W1=W; flag=2; i=i+1; continue case(2) W3=W; flag=1; end W2=(W1+W3)/2; qn=q; k1=W1*q; q=qn+(T/2)*k1; k2=W2*q; q=qn+(T/2)*k2; k3=W2*q; q=qn+T*k3; k4=W3*q; q=qn+(T/6)*(k1+2*k2+2*k3+k4); q=q/sqrt((q(1)^2+q(2)^2+q(3)^2+q(4)^2));%四元数归一化 % %四元数更新（欧拉法） % q1=0.5*W*q; % q=q+T*q1; Cbp=[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];%姿态矩阵更新 %速率方程求解（二阶龙格-库塔） Speed(1,1)=0; VA=[0,2*Wie(3),-(2*Wie(2)+Wep(2)); -2*Wie(3),0,2*Wie(1)+Wep(1); 2*Wie(2)+Wep(2),-(2*Wie(1)+Wep(1)),0]; fb=fb1(i,:)';%采集加速度计数据 fp1=Cbp*fb; fb=fb1(i+1,:)'; fp2=Cbp*fb; Vn=V; k1=fp1-[0,0,g0]'+VA*V; V=Vn+k1*T; k2=fp2-[0,0,g0]'+VA*V; V=Vn+(T/2)*(k1+k2); Speed(tempV,1)=sqrt(V(1)^2+V(2)^2);%速度计算 tempV=tempV+1; h(tempV,1)=h(tempV-1,1)+0.5*(Vn(3)+V(3))*T; VM(:,tempV)=V; Wep=[-V(2)/Re;V(1)/Re;V(1)*tan(Lm)/Re];%位置速率计算 %位置矩阵微分方程求解（二阶龙格-库塔） CA=[0,Wep(3),-Wep(2); -Wep(3),0,Wep(1); Wep(2),-Wep(1),0]; Cn=C; k1=CA*C; C=Cn+k1*T; k2=CA*C; C=Cn+(k1+k2)*(T/2); thetam=asin(Cbp(3,2)); gammam=atan(-Cbp(3,1)/Cbp(3,3)); psim=atan(-Cbp(1,2)/Cbp(2,2)); %三个姿态角的主值 if Cbp(3,3)>0;%滚动角计算 gammam=gammam; elseif (Cbp(3,3)<0)&(gammam<0); gammam=gammam+pi; else (Cbp(3,3)<0)&(gammam>0); gammam=gammam-pi; end if (Cbp(2,2)>0)&(psim>0);%偏航角计算 psim=psim; elseif (Cbp(2,2)>0)&(psim<0); psi=psim+2*pi; else Cbp(2,2)<0; psim=psim+pi; end temp=temp+1; psi(temp,1)=psim*deg; theta(temp,1)=thetam*deg; gamma(temp,1)=gammam*deg; lambdm=atan(C(3,2)/C(3,1));%经纬度主值计算 Lm=asin(C(3,3)); if C(3,1)>0;%经度计算 lambdm=lambdm; elseif (C(3,1)<0)&(lambdm<0); lambdm=lambdm+pi; else (C(3,1)<0)&(lambdm>0); lambdm=lambdm-pi; end tempW=tempW+1; lambd(tempW,1)=lambdm*deg; L(tempW,1)=Lm*deg; i=i+1; end