jd=0;
wd=0;
g=9.8;
%ka=[0.001,0,0;0,0.001,0;0,0,0.001];
%kg=[0.001,0,0;0,0.001,0;0,0,0.001];
ka=[0,0,0;0,0,0;0,0,0];
kg=[0,0,0;0,0,0;0,0,0];

%piaoyi=[(0.1*pi/180)/3600;(0.1*pi/180)/3600;(0.1*pi/180)/3600];
piaoyi=[(0.1*pi/180)/3600;0;0];
%lingpiao=[0.0001*g;0.0001*g;0.0001*g];
lingpiao=[0;0;0];
ve=0;
vn=0;
vz=0;

h=30;
wie=2*pi/(24*3600);
fb=(eye(3)+ka)*[0;0;9.8]+lingpiao;

wit=[0;wie*cos(wd);sin(wd)];
wibb=(eye(3)+kg)*wit+piaoyi;

cbt=eye(3);
R=6378245;
qj=[1;0;0;0];
for k=1:86400/h
   ft=cbt*fb;
   witt=[-vn/R;wie*cos(wd)+ve/R;wie*sin(wd)+ve*tan(wd)/R];
   ctb=inv(cbt);
   wtbb=wibb-ctb*witt;
   
   
   jzl=h*wtbb;   
   xx=jzl(1);
   xy=jzl(2);
   xz=jzl(3);
   xa=sqrt(xx*xx+xy*xy+xz*xz);
   
   c=cos(xa/2);
   s=sin(xa/2)/xa;
   
   qx=[c;xx*s;xy*s;xz*s];
   qj=quaterion(qj,qx);
   daxiao=sqrt(qj(1).^2+qj(2).^2+qj(3).^2+qj(4).^2);
   qj=1/daxiao*qj;
   erq0=qj(1);
   erq1=qj(2);
   erq2=qj(3);
   erq3=qj(4);
   
   t12=2*(erq1*erq2-erq0*erq3);
   t22=1-2*(erq1*erq1+erq3*erq3);
   t32=2*(erq2*erq3+erq0*erq1);
   t31=2*(erq1*erq3-erq0*erq2);
   t33=1-2*(erq1*erq1+erq2*erq2);
   t11=1-2*(erq2*erq2+erq3*erq3);
   t21=2*(erq1*erq2+erq0*erq3);
   t13=2*(erq1*erq3+erq0*erq2);
   t23=2*(erq2*erq3-erq0*erq1);
%   t31=2*(erq1*erq3-erq0*erq2);
   cbt=[t11,t12,t13;t21,t22,t23;t31,t32,t33];

   hang(k)=atan(cbt(1,2)/cbt(2,2));
   zong(k)=asin(t32);
   heng(k)=atan(-t31/t33);
   
   if(k==1)
      erve(k)=ve;
      ervn(k)=vn;
      ervz(k)=vz;
      erjd(k)=jd;
      erwd(k)=wd;
   end
   erve(k+1)=ve+ft(1)*h+(2*wie*sin(wd)+ve*tan(wd)/R)*vn*h-(2*wie*cos(wd)+ve/R)*vz*h;
   ervn(k+1)=vn+ft(2)*h-(2*wie*sin(wd)+ve*tan(wd)/R)*ve*h-vn/R*vz*h;
   ervz(k+1)=vz+ft(3)*h+(2*wie*cos(wd)+ve/R)*ve*h+vn*vn/R*h-g*h;
   erjd(k+1)=jd+h*(ve/cos(wd))/R;
   erwd(k+1)=wd+h*(vn/R);
   
   ve=erve(k+1);
   vn=ervn(k+1);
   vz=ervz(k+1);
   jd=erjd(k+1);
   wd=erwd(k+1);
end
k=1:1:86400/h;
%plot(k,heng(k))
%plot(k,heng(k))
%plot(k,zong(k))

%plot(k,erjd(k))
%plot(k,erwd(k))


%plot(k,erve(k))
plot(k,ervn(k))