jielian_guandao_chengxu.m

此程序用语捷联惯性导航系统中姿态

% 此为基于四元素法，角增量法的捷连惯导系统程序算法
% 飞行器飞行过程中飞行高度不变
% 航向角以逆时针为正
% 以地理系为导航坐标系
% 运行程序时需导入比力信息及陀螺议角速率信息
clear clc
Wie=7.292115147e-5;                                                                             % 地球自传角速度
Re=6378245;                                                                                     % 地球椭球长半径
h=30;                                                                                           % 飞行高度
e=1/298.3;
% 初始经纬度
Lamda(1)=116.344695283*pi/180;                                                                  % 初始经度（弧度）
L(1)=39.975172*pi/180;                                                                          % 初始纬度（弧度）
% 初始姿态角
Seita(1)=0.120992605*pi/180;                                                                    % 俯仰角（弧度）
Gama(1)=0.010445947*pi/180;                                                                     % 横滚角（弧度）
Ksai(1)=91.637207*pi/180;                                                                       % 航向角（弧度）
% 初始速度
Vx(1)=0.000048637;                                                                              % x通道速度
Vy(1)=0.000206947;                                                                              % y通道速度
Vz(1)=0.007106781;                                                                              % z通道速度
% 重力加速度计算参数
g0=9.7803267714;
gk1=0.00193185138639;
gk2=0.00669437999013;
Vx=zeros(1,48001);Vy=zeros(1,48001);Vz=zeros(1,48001);
Lamda=zeros(1,48001);L=zeros(1,48001);Seita=zeros(1,48001);Gama=zeros(1,48001);Ksai=zeros(1,48001);
% 四元素初始值
e0=cos(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1));
e1=-cos(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1));
e2=-cos(0.5*Ksai(1))*cos(0.5*Seita(1))*sin(0.5*Gama(1))-sin(0.5*Ksai(1))*sin(0.5*Seita(1))*cos(0.5*Gama(1));
e3=cos(0.5*Ksai(1))*sin(0.5*Seita(1))*sin(0.5*Gama(1))+sin(0.5*Ksai(1))*cos(0.5*Seita(1))*cos(0.5*Gama(1));
Ctb=[e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);                                       % 用四元素表示得姿态矩阵
     2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);
     2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];
E=[e0 e1 e2 e3]';% 四元素的四个元素值
for i=1:48000
    Ry(i)=Re*(1-2*e+3*e*(sin(L(i)))^2);                                                         % 计算子午圈主曲率半径
    Rx(i)=Re*(1+e*(sin(L(i)))^2);                                                               % 计算卯酉圈主曲率半径
    g=g0*(1+gk1*(sin(L(i)))^2)*(1-2*h/Re)/sqrt(1-gk2*(sin(L(i)))^2);                            % 重力加速度计算
    Cbt=Ctb';
    f_t=Cbt*f_INSc;                                                                             % 将体轴系中的比例转化到地理系
    Vx(i+1)=(f_t(1,i)+2*Wie*sin(L(i))*Vy(i)+Vx(i)*Vy(i)*tan(L(i))/Rx(i))/80+Vx(i);              % x通道速度计算
    Vy(i+1)=(f_t(2,i)-2*Wie*sin(L(i))*Vx(i)-Vx(i)*Vx(i)*tan(L(i))/Rx(i))/80+Vy(i);              % y通道速度计算
    Vz(i+1)=(f_t(3,i)+2*Wie*cos(L(i))*Vx(i)+Vx(i)*Vx(i)/Rx(i)+Vy(i)*Vy(i)/Ry(i)-g)/80+Vz(i);
    Lamda(i+1)=Vx(i)/cos(L(i))/Rx(i)/80+Lamda(i);                                               % 经度计算
    if Lamda(i+1)>pi
        Lamda(i+1)=Lamda(i+1)-2*pi;                       %经度在-180度(西经)到180(东经)范围
    end
    L(i+1)=Vy(i)/Ry(i)/80+L(i);                                                                 % 纬度计算
    if L(i+1)>(pi/2)
        L(i+1)=pi-L(i+1);                                 %纬度小于90度(北纬)
    end
    Wetx_t(i)=-Vy(i)/Ry(i);Wety_t(i)=Vx(i)/Rx(i);Wetz_t(i)=Vx(i)*tan(L(i))/Rx(i);               % 在地理坐标系的位移角速率
    Wet_t=[Wetx_t(i) Wety_t(i) Wetz_t(i)]';                                                     % 在地理坐标系的位移角速率
    Wib_b=[wib_INSc(1,i) wib_INSc(2,i) wib_INSc(3,i)]';                                         % 陀螺仪测的角速率值
    Wie_t=[0 Wie*cos(L(i)) Wie*sin(L(i))]';                                                     % 在地理坐标系的地球角速率
    Wtb_b=Wib_b-Ctb*(Wie_t+Wet_t);                                                              % 姿态矩阵角速率
    % 用角增量法计算四元素姿态矩阵
    Mwtb=[0 -Wtb_b(1) -Wtb_b(2) -Wtb_b(3);
        Wtb_b(1) 0 Wtb_b(3) -Wtb_b(2);
        Wtb_b(2) -Wtb_b(3) 0 Wtb_b(1);
        Wtb_b(3) Wtb_b(2) -Wtb_b(1) 0]/80;
    derta=sqrt((Mwtb(1,2))^2+(Mwtb(1,3))^2+(Mwtb(1,4))^2);
    E=[eye(4)*(1-derta^2/8+derta^4/384)+(1/2-derta^2/48)*Mwtb]*E;% E=(cos(0.5*derta)*eye(4)+Mwtb*sin(0.5*derta)/derta)*E，采用四阶近似算法
    e0=E(1);e1=E(2);e2=E(3);e3=E(4);
    Ctb=[e0^2+e1^2-e2^2-e3^2 2*(e1*e2+e0*e3) 2*(e1*e3-e0*e2);                                  % 用四元素表示得姿态矩阵
     2*(e1*e2-e0*e3) e0^2-e1^2+e2^2-e3^2 2*(e2*e3+e0*e1);
     2*(e1*e3+e0*e2) 2*(e2*e3-e0*e1) e0^2-e1^2-e2^2+e3^2];
    % 姿态角计算
    Seita(i+1)=asin(Ctb(2,3));                                                                  % 俯仰角计算
    Gama(i+1)=atan(-Ctb(1,3)/Ctb(3,3));                                                         % 横滚角计算
    if abs(Ctb(3,3))>eps
        Gama(i+1)=atan(-Ctb(1,3)/Ctb(3,3));
        if Ctb(3,3)>0
            Gama(i+1)=Gama(i+1);
        elseif -Ctb(1,3)> 0
                Gama(i+1)=Gama(i+1)+pi;
            else Gama(i+1)=Gama(i+1)-pi;
            end
    elseif -Ctb(1,3)> 0
                Gama(i+1)=pi/2;
         else Gama(i+1)=-pi/2;
    end
    Ksai(i+1)=atan(Ctb(2,1)/Ctb(2,2));                                                          % 航向角计算
    if abs(Ctb(2,2))>eps
       Ksai(i+1)=atan(Ctb(2,1)/Ctb(2,2));
       if Ctb(2,2)>0
           Ksai(i+1)=Ksai(i+1);
       elseif Ctb(2,1)> 0
               Ksai(i+1)=Ksai(i+1)+pi;
            else  Ksai(i+1)=Ksai(i+1)-pi;
            end
    elseif Ctb(2,1)>0  
               Ksai(i+1)=pi/2;
         else Ksai(i+1)=-pi/2;
    end
end
% 将弧度换算为角度
Seita=Seita*180/pi;Gama=Gama*180/pi;Ksai=Ksai*180/pi;
L=L*180/pi;Lamda=Lamda*180/pi;
t=0:1/80:600;
% 绘制曲线图
figure(1);
plot(t,Lamda)                                                                                   % 绘制经度变化曲线图
grid on
Xlabel('时间/秒');Ylabel('经度Lamda/度');title('经度变化曲线图');
figure(2);
plot(t,L)                                                                                       % 绘制纬度变化曲线图
grid on
Xlabel('时间/秒');Ylabel('纬度L/度');title('纬度变化曲线图');
figure(3);
plot(Lamda,L)                                                                                   % 绘制经-纬度变化曲线图
grid on 
Xlabel('经度Lamda/度');Ylabel('纬度L/度');title('经—纬度坐标曲线图');
figure(4);
plot(t,Seita)                                                                                   % 绘制俯仰角变化曲线图
grid on
Xlabel('时间/秒');Ylabel('俯仰角Seita/度');title('俯仰角变化曲线图');
figure(5);
plot(t,Gama)                                                                                    % 绘制横滚角变化曲线图
grid on
Xlabel('时间/秒');Ylabel('横滚角Gama/度');title('横滚角变化曲线图');
figure(6);
plot(t,Ksai)                                                                                    % 绘制航向角变化曲线
grid on
Xlabel('时间/秒');Ylabel('航向角Ksai/度');title('航向角变化曲线图');
figure(7);
plot(t,Vx)                                                                                      % 绘制东向速度变化曲线
grid on
Xlabel('时间/秒');Ylabel('东向速度Vx  米/秒');title('东向速度变化曲线图');
figure(8);
plot(t,Vy)                                                                                      % 绘制北向速度变化曲线
grid on
Xlabel('时间/秒');Ylabel('北向速度Vy  米/秒');title('北向速度变化曲线图');
figure(9);
plot(t,Vz)                                                                                      % 绘制垂直速度变化曲线
grid on
Xlabel('时间/秒');Ylabel('垂直速度Vz  米/秒');title('垂直速度变化曲线图');