2.cpp

惯性导航四位置寻北算法程序

#include <iostream>
#include <math.h>
#define pai 3.1415
#define Wie 15*pai/180
#define L 32*pai/180
#define l 10
//y轴敏感北向分量
 using namespace std;

void main()
{   
   
	FILE *w1,*w2,*w3,*w4;
	double sita;
	double a;
	double b;

	w1=fopen("11_0","r");
    w2=fopen("11_90","r");
	w3=fopen("11_180","r");
	w4=fopen("11_270","r");

	double Ngz0=0;
	double Ngz1=0;
	double Ngz2=0;
//	double w1[l][6];
	double Ng0,Ng1,Ng2;
    double Wibb1;
	int i,j;
    for(i=0;i<3;i++)
		for(j=0;j<=l;j++)
		{
		   Ngz0+=w1[j][0];
		   Ngz1+=w1[j][1];
           Ngz2+=w1[j][2];
		};
	Ng0=Ngz0/l; 
	Ng1=Ngz1/l;
    Ng2=Ngz2/l;

Wibb1=0.0043847099*((Ng0/2088555.75571429)-(-0.0000261934))+0.9999675911*((Ng1/2040682.52142857)-(-0.0001351865))+(-0.0051314378)*((Ng2/(-2037682.25428571))-(0.0000486546));



//	a=acos((Wibb1-Wibb3)/(2*Wie*cos(L)));
		cout<<Wibb1<<endl;
//	b=asin((Wibb4-Wibb2)/(2*Wie*cos(L)));
		//cout<<b<<endl;

	//sita=sin(a)*sin(a)*a+cos(b)*cos(b)*b;
   // cout<<sita<<endl;
    fclose(w1);
	fclose(w2);
	fclose(w3);
	fclose(w4);
};