jjj.cpp

利用伪距GPS卫星单点定位程序

#include<fstream>
#include<iostream>
#include<string>
#include<vector>
#include<cmath>
#include<iomanip>
#include"mystruct.h"
#include"readNfile.h"
#include"matrix.h"
//以下矩阵要用------------------
using namespace std;           //
#ifndef _NO_NAMESPACE          //
using namespace math;          //
#define STD std                //
#else                          //
#define STD                    //
#endif                         //
                               //
#ifndef _NO_TEMPLATE           //
typedef matrix<double> Matrix; // 
#else                          //
typedef matrix Matrix;         // 
#endif//-------------------------
time_gps GregToGps(time_calendar tc);//声明时间转换
//////////////////////--------------主函数开始-----------------
int main()
{	
//----------------------Read N File---------------------------------
    vector<nav_sat> vn;
	readNfile(vn);
	cout<<"N文件循环vn.size="<<vn.size()<<endl;
    cout<<"-------------n file is ok-----------"<<endl;
//----------------------Read O File----------------------------------
    vector<epoch_set> v;double apx,apy,apz;
	readOfile(v,apx,apy,apz);
	cout<<"v.size="<<v.size()<<endl;
    cout<<"-------------o file is ok-----------"<<endl;
//------------------------开始计算-------------------------------	   
double const pi=3.1415926535898;
double const c=2.99792458e8;//光速
int posk,epc=0,j=0;//posk最近信息位置，epc=第epc历元
double ts,tr;//tr卫星信号接收时刻,ts发射时刻
ofstream outfile("各个历元坐标.txt",ios::out);
ofstream outfile1("卫星坐标.txt",ios::out);
cout<<setiosflags(ios::fixed)<<setprecision(15);    
sta_polar sp;//(x,y,z)坐标,借用sta_polar结构的
vector<sta_polar> sps;

Matrix deltx(4,1);

deltx(3,0)=0;//卫星钟差
double sumx=0,sumy=0,sumz=0,ex,ey,ez;
for(epc=0;epc<v.size();epc++)//历元总数=v.size()
{       Matrix P(v[epc].num_sat,v[epc].num_sat);//权
        Matrix dT(v[epc].num_sat,1);//卫星种差
        Matrix x0(4,1),x1(4,1),Xx(4,1);//迭代要用的
        x1(0,0)=0;x1(1,0)=0;x1(2,0)=0;x1(3,0)=0;//初始化为地心坐标        
		tr=v[epc].gps_sat.num_sec;//第epc历元的观测时刻的秒	
		
		sps.clear();//清空容器，不然会一直存上一历元的卫星坐标	
		double dt0=0,dt1=0;
		//sumx=0;sumy=0;sumz=0;
		
	  do
	  {
          dt0=dt1;
		  tr+=dt0;//接收机种差改正
	   
	   for(j=0;j<v[epc].num_sat;j++ )//j是N文件的卫星号0,1,2..8颗;/////////////////
	   {                            //[epc].num_sat指本历元的卫星个数            // 
			 double t0=tr-0.075;//初始化
			 double tk,min;
		     for(int k=0;k<vn.size();k++)//先有第一次                            //
			 {
			     if(vn[k].PRN==v[epc].array_sat[j].sat_num)                      //
				 { tk=fabs(t0-vn[k].TOE.num_sec);min=tk;posk=k; 				  
			           break;                                                    //
				 }
			 }
		     for(k=0;k<vn.size();k++)//进行间隔比较                              //最小外推时间
			 {
			    if(vn[k].PRN==v[epc].array_sat[j].sat_num)
				{	
				   tk=fabs(t0-vn[k].TOE.num_sec);	                             //  
				        if(tk<min)
						{
						   min=tk;posk=k;
						}//获得间隔最小的卫星的位置                              //
				}
			 }                                                                   //
           //cout<<"第"<<v[epc].array_sat[j].sat_num<<"号卫星最近的轨道----";
           //cout<<"于N文件的第"<<posk<<"组,卫星号:"<<vn[posk].PRN<<endl;        //
			 for(int i=0;i<v[epc].num_sat;i++)
			 {
				 if(i==j) 
				 {
					 if (vn[posk].IODE==0) P(i,j)=1e6;
					 else P(i,j)=1000/vn[posk].IODE;}
			    else P(i,j)=0;
			 }
			 
		 double t2;
         do//这个do迭代卫星位置，求卫星发射时刻                                  //
		 {
		    t2=t0;	    
            sat_pos(posk,t2, tr, sp,vn) ;//计算位置
            ts=tr-sqrt((sp.Ap-apx)*(sp.Ap-apx)+(sp.Ep-apy)*(sp.Ep-apy)+(sp.Rp-apz)*(sp.Rp-apz))/c;
	        t0=ts;
		 }while(fabs(ts-t2)>1e-12);
		 sps.push_back(sp);//应当用完后清0                                       //
         dT(j,0)=vn[posk].pare_clock.a0+vn[posk].pare_clock.a1*(tr-vn[posk].pare_clock.TOC.num_sec)+vn[posk].pare_clock.a2*pow((tr-vn[posk].pare_clock.TOC.num_sec),2);		
		 //cout<<"第"<<epc+1<<"历元"<<"第"<<j+1<<"卫星,over"<<endl;
		 
	   }	//位置算完
	  
       ////////////////////////////////////////////////////////////////////////////						
       //下面组法方程
	    do
		{     
                x0=x1;
				
				Matrix A(v[epc].num_sat,4);//V=A*deltx+L
				Matrix R0(v[epc].num_sat,1);//
				Matrix l(v[epc].num_sat,1);
				Matrix m(v[epc].num_sat,1);
				Matrix n(v[epc].num_sat,1);
				Matrix L(v[epc].num_sat,1);
		
			
				

		        for(int i=0;i<v[epc].num_sat;i++)
				{
		   R0(i,0)=sqrt(pow((sps[i].Ap-x0(0,0)),2)+pow((sps[i].Ep-x0(1,0)),2)+pow((sps[i].Rp-x0(2,0)),2));
		   l(i,0)=(sps[i].Ap-x0(0,0))/R0(i,0);
		   m(i,0)=(sps[i].Ep-x0(1,0))/R0(i,0);
	   	   n(i,0)=(sps[i].Rp-x0(2,0))/R0(i,0);
		   A(i,0)=l(i,0);A(i,1)=m(i,0);A(i,2)=n(i,0);A(i,3)=-1;
		   L(i,0)=v[epc].array_sat[i].set_obs.pseudo_obs.value-R0(i,0)+c*dT(i,0);//+卫星钟差+大气延迟？
			
				}
				deltx=-(!((~A)*A))*((~A)*L);//解法方程,无权
	    	    //deltx=-(!((~A)*P*A))*((~A)*P*L);//解法方程,有权
					
		        Xx=x0+deltx;
		        x1=Xx;
		}while(fabs(deltx(0,0)*deltx(0,0)+deltx(1,0)*deltx(1,0)+deltx(2,0)*deltx(2,0))>1e-3);
		dt1=deltx(3,0)/c;
		//cout<<"epc="<<epc+1;
		//cout<<"dt1="<<dt1<<endl;
        sps.clear();
		//cout<<"dt1-dt0="<<fabs(deltx(3,0)/c-dt0)<<endl;
	
	   }while(fabs(dt1-dt0)>1e-9);
	  
   /////////////////////////////////
   outfile1.precision(15);
   for( j=0;j<v[epc].num_sat;j++)
   {
	   outfile1.precision(15);
	   outfile1<<"<"<<v[epc].array_sat[1].tgps.num_week<<","<<ts<<">";
	   outfile1<<tr<<"; "<<"第"<<v[epc].array_sat[j].sat_num<<"颗卫星的x="<<sps[j].Ap <<" y="<<sps[j].Ep<<" z="<<sps[j].Rp<<endl;      
   }
   outfile.precision(15);   
   outfile<<"第"<<epc+1<<"历元的坐标: "<<Xx(0,0)<<"; "<<Xx(1,0)<<"; "<<Xx(2,0)<<endl;
   outfile<<";"<<endl;
   sumx+=Xx(0,0);sumy+=Xx(1,0);sumz+=Xx(2,0);
}//epc
ex=sumx/epc;ey=sumy/epc;ez=sumz/epc;
outfile<<"********************************"<<endl;
outfile<<"平均="<<ex<<"; "<<ey<<"; "<<ez;
outfile.close();outfile1.close();
cout<<"与近似坐标的差："<<endl;
cout<<ex-apx<<";"<<ey-apy<<";"<<ez-apz<<endl;		
return 0;
}//main
//格里高历转GPS时间
time_gps GregToGps(time_calendar tc)
{
	double JD,h,yr,mth,WN,TOW;
	time_gps tg;
	if(tc.month>2) {yr=tc.year;mth=tc.month;}
	else {yr=tc.year-1;mth=tc.month+12;}
	if(yr>10) yr+=1900;
	else yr+=2000;
	h=tc.hour+tc.minute/60.0+tc.second/3600.0;	
	JD=floor(365.25*yr)+floor(30.6001*(mth+1))+tc.day+h/24.0+1720981.5;	
    WN=floor((JD-2444244.5)/7.0);
	TOW=(JD-2444244.5-7*WN)*86400.0;
	tg.num_week=WN;tg.num_sec=TOW;
	return tg;
}