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