📄 dutto.m

📁 matlab 惯性导航基本算法程序没有扩展功能和接口控制程序
💻 M
字号:
clear
clc
glvs
w_earth= 7.2921151467e-5;
RofEarth= 6378245;
a=1/298.257;
e2=2*a-a^2;
VE=0;
VN=0;
InLong = 0.0;
InLat = 0.0;
InVe = 0.0;
InVn = 0.0;
OutLong = 0.0;
OutLat = 0.0;
Roll = 0.0;
pitch = 0.0;
Yaw = 0.0;
OutTheta = 0.0;
OutGamma = 0.0;
OutKappa = 0.0;
OutVE = 0.0;
OutVN = 0.0;
InLat=30.3142;
InLong=114.2125;
g= 9.8;
fid=fopen('sx1.txt');
for i=1:8
    tline=fgetl(fid);
end
B=dms2rad(B*100);

    tline = fgets(fid);
    if ~ischar(tline), break, end
    s=str2num(tline);
	 VEPre=VE;
	 VNPre=VN;
	 latitude=dms2rad(InLat);
	 longitude=dms2rad(InLong);
	 InitYaw=dms2rad(Yaw);
	 Initpitch=dms2rad(pitch);
	 InitRoll=dms2rad(Roll);	
	 WE=0; WN=0; WZ=0;
	 T=0.1;%采样间隔
	OmegaIee=zeros(3);  %地理系相对惯性系旋转角速度矩阵
	OmegaIbb=zeros(3);  %陀螺仪提供的旋转角速度矩阵
	K1=zeros(3);	K2=zeros(3);	K3=zeros(3);	K4=zeros(3);%龙哥库塔中间变量
	Cbe=zeros(3);
	V=zeros(3,1);VPre=zeros(3,1); fe=zeros(3,1); OmegaIeE1=zeros(3,1); OmegaEee=zeros(3,1);
	Cbe(1,1)=cos(InitYaw)*cos(Initpitch);
	Cbe(2,1)=sin(InitYaw)*cos(Initpitch);
	Cbe(3,1)=-sin(Initpitch);
	Cbe(1,2)=cos(InitYaw)*sin(Initpitch)*sin(InitRoll)-sin(InitYaw)*cos(InitRoll);
	Cbe(2,2)=sin(InitYaw)*sin(Initpitch)*sin(InitRoll)+cos(InitYaw)*cos(InitRoll);
	Cbe(3,2)=cos(Initpitch)*sin(InitRoll);
	Cbe(1,3)=cos(InitYaw)*sin(Initpitch)*cos(InitRoll)+sin(InitYaw)*sin(InitRoll);
	Cbe(2,3)=sin(InitYaw)*sin(Initpitch)*cos(InitRoll)-cos(InitYaw)*sin(InitRoll);
	Cbe(3,3)=cos(Initpitch)*cos(InitRoll);
	CbeOrigin=Cbe;
	CbePre1=Cbe;
	CbePre2=Cbe;
	%time,Roll,pitch,Rollrate,pitchrate,Yawrate,XAccel,YAccel,ZAccel,Temperature,Timercounts;
	%time1,Roll1,pitch1,Rollrate1,pitchrate1,Yawrate1,XAccel1,YAccel1,ZAccel1,Temperature1,Timercounts1;
	%time2,Roll2,pitch2,Rollrate2,pitchrate2,Yawrate2,XAccel2,YAccel2,ZAccel2,Temperature2,Timercounts2;
	time1=s(1);
    Roll1=s(2);
    pitch1=s(3);
    Rollrate1=s(4);
    pitchrate1=s(5);
    Yawrate1=s(6);
    XAccel1=s(7);
    YAccel1=s(8);
    ZAccel1=s(9);
    tline = fgets(fid);
    s=str2num(tline);
	time2=s(1);
    Roll2=s(2);
    pitch2=s(3);
    Rollrate2=s(4);
    pitchrate2=s(5);
    Yawrate2=s(6);
    XAccel2=s(7);
    YAccel2=s(8);
    ZAccel2=s(9);
	tline = fgets(fid);
    i=0;
	aa=0;
while 1          %读取一行数据 开始循环
    tline = fgets(fid);
    if ~ischar(tline), break, end
    s=str2num(tline);
    i=i+1;
	%截取数据
	if (i>1)
		Roll1=Roll2;pitch1=pitch2;Rollrate1=Rollrate2;pitchrate1=pitchrate2;Yawrate1=Yawrate2;XAccel1=XAccel2;YAccel1=YAccel2;ZAccel1=ZAccel2;	
		Roll2=Roll;pitch2=pitch;Rollrate2=Rollrate;pitchrate2=pitchrate;Yawrate2=Yawrate;XAccel2=XAccel;YAccel2=YAccel;ZAccel2=ZAccel;
    end
		%读取文本文件中的数据	
	time=s(1);
    Roll=s(2);
    pitch=s(3);
    Rollrate=s(4);
    pitchrate=s(5);
    Yawrate=s(6);
    XAccel=s(7);
    YAccel=s(8);
    ZAccel=s(9);
    %计算方向余旋矩阵,四阶数值积分
	
    WE=-VN/(RofEarth*cos(latitude));
    WN=VE/RofEarth+(w_earth)*cos(latitude);
    WZ=VE*tan(latitude)/RofEarth+(w_earth)*sin(latitude);
	OmegaIee(1,2)=-WZ;OmegaIee(1,3)=WN;OmegaIee(2,1)=WZ;OmegaIee(2,3)=-WE;OmegaIee(3,1)=-WN;OmegaIee(3,2)=WE;	
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	OmegaIbb(1,2)=-deg2rad(Yawrate1);OmegaIbb(1,3)=deg2rad(pitchrate1);
	OmegaIbb(2,1)=deg2rad(Yawrate1);OmegaIbb(2,3)=-deg2rad(Rollrate1);
	OmegaIbb(3,1)=-deg2rad(pitchrate1);OmegaIbb(3,2)=deg2rad(Rollrate1);
	K1=Cbe*OmegaIbb-OmegaIee*Cbe;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	OmegaIbb(1,2)=-deg2rad(Yawrate2);OmegaIbb(1,3)=deg2rad(pitchrate2);
    OmegaIbb(2,1)=deg2rad(Yawrate2);OmegaIbb(2,3)=-deg2rad(Rollrate2);
    OmegaIbb(3,1)=-deg2rad(pitchrate2);OmegaIbb(3,2)=deg2rad(Rollrate2);
    K2=(Cbe+K1*T)*OmegaIbb-OmegaIee*(Cbe+K1*T);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    K3=(Cbe+K2*T)*OmegaIbb-OmegaIee*(Cbe+K2*T);
    %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    OmegaIbb(1,2)=-deg2rad(Yawrate);OmegaIbb(1,3)=deg2rad(pitchrate);
	OmegaIbb(2,1)=deg2rad(Yawrate);OmegaIbb(2,3)=-deg2rad(Rollrate);
	OmegaIbb(3,1)=-deg2rad(pitchrate);OmegaIbb(3,2)=deg2rad(Rollrate);
	K4=(Cbe+K3*2.*T)*OmegaIbb-OmegaIee*(Cbe+K3*2.*T);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
	if(i<3)
		Cbe=CbeOrigin+T/3.0*(K1+2.0*K2+2.0*K3+K4);
	else
		Cbe=CbePre1+T/3.0*(K1+2.0*K2+2.0*K3+K4);    %数值积分
    end


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%	
		%计算速度和位置
		if(i==1)
			Accel1=zeros(3,1);
			VEPre=VE;
			VNPre=VN;
			Accel1(1,1)=g*XAccel1;Accel1(2,1)=g*YAccel1;Accel1(3,1)=g*ZAccel1;
			Accel1=CbeOrigin*Accel1; %对加速度进行坐标变换
			VE=VE+(Accel1(1,1)+(2.0*w_earth*sin(latitude)+VE*tan(latitude)/RofEarth)*VN)*T;
			VN=VN+(Accel1(2,1)-(2.0*w_earth*sin(latitude)+VE*tan(latitude)/RofEarth)*VE)*T;
			RM=RofEarth/(1-e2)*(1-e2*sin(latitude)^2)^1.5;
            RN=RofEarth/sqrt(1-e2*sin(latitude)^2);
            latitude=latitude+(VNPre+VN)*T/(2.0*RM);
			longitude=longitude+(VEPre+VE)*T/( cos(latitude)*(2.0*RN) );
            
			
			VEPre=VE;
			VNPre=VN;
			Accel1(1,1)=g*XAccel2;Accel1(2,1)=g*YAccel2;Accel1(3,1)=g*ZAccel2;
			Accel1=CbeOrigin*Accel1; %对加速度进行坐标变换
			VE=VE+(Accel1(1,1)+(2.0*w_earth*sin(latitude)+VE*tan(latitude)/RofEarth)*VN)*T;
			VN=VN+(Accel1(2,1)-(2.0*w_earth*sin(latitude)+VE*tan(latitude)/RofEarth)*VE)*T;
			RM=RofEarth/(1-e2)*(1-e2*sin(latitude)^2)^1.5;
            RN=RofEarth/sqrt(1-e2*sin(latitude)^2);
            latitude=latitude+(VNPre+VN)*T/(2.0*RM);
			longitude=longitude+(VEPre+VE)*T/( cos(latitude)*(2.0*RN) );
            subplot(2,1,1),plot(s(1),latitude,'b-'),plot(s(1),longitude,'r-'),hold on
            
		
		else
		
			Accel1=zeros(3,1);
			Accel1(1,1)=g*XAccel;Accel1(2,1)=g*YAccel;Accel1(3,1)=g*ZAccel;
			Accel1=Cbe*Accel1; %对加速度进行坐标变换
			VE=VE+(Accel1(1,1)+(2.0*w_earth*sin(latitude)+VE*tan(latitude)/RofEarth)*VN)*T;
			VN=VN+(Accel1(2,1)-(2.0*w_earth*sin(latitude)+VE*tan(latitude)/RofEarth)*VE)*T;
			RM=RofEarth/(1-e2)*(1-e2*sin(latitude)^2)^1.5;
            RN=RofEarth/sqrt(1-e2*sin(latitude)^2);
            latitude=latitude+(VNPre+VN)*T/(2.0*RM);
			longitude=longitude+(VEPre+VE)*T/( cos(latitude)*(2.0*RN) );
            subplot(2,1,1),plot(s(1),latitude,'b-'),plot(s(1),longitude,'r-'),hold on
        end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
		CbePre1=CbePre2;
		CbePre2=Cbe;
		VEPre=VE;
		VNPre=VN;	 %保存变量


	Cbe=ctranspose(Cbe);
    OutLat=rad2dms(latitude);
    OutLong=rad2dms(longitude);
OutVE=VE;
OutVN=VN;
OutTheta=rad2dms(-asin(Cbe(1,3)));
    t11=Cbe(1,1);	t12=Cbe(1,2);	t13=Cbe(1,3);	t23=Cbe(2,3);	t33=Cbe(3,3);
	OutGamma=atan(t23/t33);
	OutKappa=atan(t12/t11);
	if(t33<0||t23>0),
        OutGamma=OutGamma+pi;
    end
    if(t33<0||t23<0),
        OutGamma=OutGamma-pi;
    end
    if(t11>0||t12<0),
        OutKappa=OutKappa+2*pi;
    end
    if(t11<0),
        OutKappa=OutKappa+pi;
    end 
    subplot(2,1,2),plot(s(1),OutGamma,'b-'),plot(s(1),OutKappa,'r-'),hold on
end
fclose(fid);
💿 文件大小 59 K
👤 上传用户 black001
📂 所属分类 matlab例程
🏷️ 相关标签

#matlab #程序 #惯性导航 #算法
⌨️ 快捷键说明

复制代码 Ctrl + C
搜索代码 Ctrl + F
全屏模式 F11
切换主题 Ctrl + Shift + D
显示快捷键 ?
增大字号 Ctrl + =
减小字号 Ctrl + -