siyuanshu.zip_DSP导航解算_二阶龙格库塔_四元数_四元数法c_龙格库塔

#include <math.h> #include <stdio.h> double Attdx_initial=0.0,Attdy_initial=0.0,Attdz_initial=0.0; long double Lati_initial=0.0,Longti_initial=0.0; double Height_initial=0.0; unsigned int Course_initial=0; double Heightgpstrue=0.0; double Distancetrue=0.0; double Heightgpsgps[3]; double Heightgps_t0; int i_Wibb=0,i_fb=0,i_Height=0,i_odometer=0; void Update_Height(); #define Re 6378137.0 #define e 0.00335281318 #define Wie 7.2921151e-5 #define step 0.01 #define PI 3.1415926535 #define n1 12000 #define n2 18000 #define eps 1E-17 #define g0 9.7803267714 #define Y0 500000 #define E2 0.00673852541468 #define K00 0.000000157046064172 #define K1 0.005051773759 #define K2 -0.000029837302 #define C 6399698.9018 #define C00 6367558.49686 #define C11 32005.79642 #define C22 133.86115 double Wibx,Wiby,Wibz; double fbx,fby,fbz; double Attdx,Attdy,Attdz; double Vx,Vy,Vz,Vb[3]; long double Lati,Longti,Height; double Cbt[9],Ctb[9],Cmb[9],Cmt[9],Cie[9],Cen[9]; double Cb_ib0[9],Cib0_b[9],Cib0_i[9]; long double q0,q1,q2,q3; long double qq00,qq11,qq22,qq33; double Rx,Ry,g; double fb[3],ft[3]; double Wibb[3],Wibb_old[3],Wiet[3],Wett[3],Wtbb[3]={0.0,0.0,0.0}; double fib0[3]={0.0,0.0,0.0},vib0_Tu[3]={0.0,0.0,0.0},vib0_Tv[3]={0.0,0.0,0.0},vi_Tu[3]={0.0,0.0,0.0},vi_Tv[3]={0.0,0.0,0.0}; double hB=20.0; double dH,dH_aa=0.0,dH_a=0.0,dH_v=0.0; double C1=0.03,C2=4.0e-4,C3=2.0e-6; double Shangxian,Xiaxian,delt_Sk; double Latigps,Longtigps,Heightgps; long double delt_L=0.0,delt_E=0.0; double Distancetrue_Lati=0.0; double Distancetrue_Longti=0.0; double dVx=0.0,dVy=0.0; double d_L=0.0,d_E=0.0; void Init_parameter() { int i; Lati=Lati_initial; Longti=Longti_initial; Height=Height_initial; PositionToXY(Longti,Lati,&x_out,&y_out,&zip); X_out=x_out; Y_out=y_out; Vx=0.0; Vy=0.0; Vz=0.0; Attdx=Attdx_initial; Attdy=Attdy_initial; Attdz=Attdz_initial; Distancetrue=Course_initial; Rx=Re*(1.0+e*sin(Lati)*sin(Lati))+Height; Ry=Re*(1.0-2.0*e+3.0*e*sin(Lati)*sin(Lati))+Height; g=g0*(1.0+0.00527094*sin(Lati)*sin(Lati)+0.0000232718*pow(sin(Lati),4))-0.000003086*Height; qq00=1.0; qq11=0.0; qq22=0.0; qq33=0.0; for(i=0;i<15;i++) Xk1[i]=0.0; vib0_Tu[0]=0.0; vib0_Tu[1]=0.0; vib0_Tu[2]=0.0; vib0_Tv[0]=0.0; vib0_Tv[1]=0.0; vib0_Tv[2]=0.0; } /******************更新四元数(龙格库塔法)*******************/ void Update_q() { long double b1,b2,b3,b4,b5,b6,b7,b8; long double qq; b1=(-q1*Wtbb0[0]-q2*Wtbb0[1]-q3*Wtbb0[2])/2.0; b2=(q0*Wtbb0[0]+q2*Wtbb0[2]-q3*Wtbb0[1])/2.0; b3=(q0*Wtbb0[1]-q1*Wtbb0[2]+q3*Wtbb0[0])/2.0; b4=(q0*Wtbb0[2]+q1*Wtbb0[1]-q2*Wtbb0[0])/2.0; b5=(-(q1+b2*step)*Wtbb[0]-(q2+b3*step)*Wtbb[1]-(q3+b4*step)*Wtbb[2])/2.0; b6=((q0+b1*step)*Wtbb[0]+(q2+b3*step)*Wtbb[2]-(q3+b4*step)*Wtbb[1])/2.0; b7=((q0+b1*step)*Wtbb[1]-(q1+b2*step)*Wtbb[2]+(q3+b4*step)*Wtbb[0])/2.0; b8=((q0+b1*step)*Wtbb[2]+(q1+b2*step)*Wtbb[1]-(q2+b3*step)*Wtbb[0])/2.0; q0=q0+(b1+b5)*step/2.0; q1=q1+(b2+b6)*step/2.0; q2=q2+(b3+b7)*step/2.0; q3=q3+(b4+b8)*step/2.0; //长时间的导航导致四元数非正交，需要对四元数正交处理 qq=sqrt(q0*q0+q1*q1+q2*q2+q3*q3); q0=q0/qq; q1=q1/qq; q2=q2/qq; q3=q3/qq; }