guiji.rar_INS Accelerometer_惯导_惯导 速度_舰船_轨迹

#include <stdio.h> #include <stdlib.h> #include <math.h> # define W 7.2915e-5 //地球自转角速度 # define G 9.80147 //地球重力加速度 # define PI 3.1415926535 // # define Tn 5e-3 # define Tm 5e-6 # define L 60*200 FILE *fp1; //定义全局指针变量fp void main() { double yaw=0,yaw_0=0,roll=0,pitch=0,yaw_d=0,roll_d=0,pitch_d=0,yaw_w=0,roll_w=0,pitch_w=0; double A_y=10/180.0*PI,A_r=15/180.0*PI,A_p=20/180.0*PI,W_y=2*PI/10.0,W_r=2*PI/15.0,W_p=2*PI/20.0; double v0=10,v[3]={0},a[3]={0},f[3]={0},v_w[3]={0}; double lat=28.2/180.0*PI,lon=116.15326/180.0*PI,lat_s=0,lon_s=0; double t11,t12,t13,t21,t22,t23,t31,t32,t33; double Wien[3]={0},Wenn[3]={0},Winn[3]={0},Winb[3]={0},Wnbb[3]={0},temp[3]={0}; double gyro[3]={0},acceler[3]={0}; int i,j,k; double T1=0,T2=0; double R=6378254,Rm=0,Rn=0,e=1/298.3; if((fp1=fopen("D:\\学业资料\\学习\\程序\\data\\error_propagation\\swing.dat","w"))==NULL) { printf("can't open the file 16_pos.dat\n"); exit(0); } for(i=1;i<=L;i++) { Rm=R*(1-2*e+3*e*pow(sin(lat),2)); Rn=R*(1+e*pow(sin(lat),2)); for(k=0;k<1000;k++) { T1=(i-1)*Tn+k*Tm; yaw=A_y*sin(T1*W_y)+yaw_0; roll=A_r*sin(T1*W_r); pitch=A_p*sin(T1*W_p); yaw_d=A_y*W_y*cos(T1*W_y); roll_d=A_r*W_r*cos(T1*W_r); pitch_d=A_p*W_p*cos(T1*W_p); t11=cos(pitch)*cos(yaw); t12=sin(pitch); t13=-cos(pitch)*sin(yaw); t21=-cos(roll)*sin(pitch)*cos(yaw)+sin(roll)*sin(yaw); t22=cos(roll)*cos(pitch); t23=cos(roll)*sin(pitch)*sin(yaw)+sin(roll)*cos(yaw); t31=sin(roll)*sin(pitch)*cos(yaw)+cos(roll)*sin(yaw); t32=-sin(roll)*cos(pitch); t33=-sin(roll)*sin(pitch)*sin(yaw)+cos(roll)*cos(yaw); v[0]=v0*cos(yaw);v[1]=0;v[2]=-v0*sin(yaw); a[0]=-v0*sin(yaw)*yaw_d;a[1]=0;a[2]=-v0*cos(yaw)*yaw_d; Wien[0]=W*cos(lat);Wien[1]=W*sin(lat);Wien[2]=0; Wenn[0]=v[2]/Rn;Wenn[1]=v[2]*tan(lat)/Rn;Wenn[2]=-v[0]/Rm; Winn[0]=Wien[0]+Wenn[0]; Winn[1]=Wien[1]+Wenn[1]; Winn[2]=Wien[2]+Wenn[2]; Winb[0]=t11*Winn[0]+t12*Winn[1]+t13*Winn[2]; Winb[1]=t21*Winn[0]+t22*Winn[1]+t23*Winn[2]; Winb[2]=t31*Winn[0]+t32*Winn[1]+t33*Winn[2]; Wnbb[0]=sin(pitch)*yaw_d+roll_d; Wnbb[1]=cos(roll)*cos(pitch)*yaw_d+sin(roll)*pitch_d; Wnbb[2]=-sin(roll)*cos(pitch)*yaw_d+cos(roll)*pitch_d; gyro[0]+=(Winb[0]+Wnbb[0])*Tm; gyro[1]+=(Winb[1]+Wnbb[1])*Tm; gyro[2]+=(Winb[2]+Wnbb[2])*Tm; temp[0]=Wien[0]+Winn[0]; temp[1]=Wien[1]+Winn[1]; temp[2]=Wien[2]+Winn[2]; f[0]=-temp[2]*v[1]+temp[1]*v[2]+a[0]; f[1]=temp[2]*v[0]-temp[0]*v[2]+a[1]+G; f[2]=-temp[1]*v[0]+temp[0]*v[1]+a[2]; acceler[0]+=f[0]*Tm; acceler[1]+=f[1]*Tm; acceler[2]+=f[2]*Tm; lat_s+=v[0]*Tm; lon_s+=v[2]*Tm; } T2=i*Tn; yaw_w=A_y*sin(T2*W_y)+yaw_0; roll_w=A_r*sin(T2*W_r); pitch_w=A_p*sin(T2*W_p); v_w[0]=v0*cos(yaw_w);v_w[1]=0;v_w[2]=-v0*sin(yaw_w); lon+=lon_s/(Rn*cos(lat)); lat+=lat_s/Rm; fprintf(fp1,"%.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e %.15e\n",gyro[0],gyro[1],gyro[2],acceler[0],acceler[1],acceler[2],yaw_w,roll_w,pitch_w,v_w[0],v_w[1],v_w[2],lat,lon); gyro[0]=0;gyro[1]=0;gyro[2]=0; acceler[0]=0;acceler[1]=0;acceler[2]=0; lat_s=0;lon_s=0; } fclose(fp1); }