sat_pos.cpp

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

#include<fstream>
#include<iostream>
#include<string>
#include<vector>
#include<cmath>
#include<iomanip>
using namespace std;
#include"mystruct.h"
 

//
void sat_pos(int& posk,double& t2,double& tr,sta_polar& sp,vector<nav_sat>& vn)  
{ 
	//vector<nav_sat> vn;
    double tk=t2-vn[posk].TOE.num_sec;
	double const GM=3.9860047e14;
	double const we=7.2921151467e-5;//地球自转角速度//曾经把它抄错了成7.792..
	double const pi=3.1415926535898;
	double const c=2.99792458e8;//光速
	double n0=sqrt(GM)/pow((vn[posk].sqrtA),3);
	double e_n=n0+vn[posk].delta_n;//rt=right
		if(tk>302400) tk=tk-604800;//rt
		if(tk<-302400) tk=tk+604800;
	double M=vn[posk].M0+e_n*tk;//????? t is gps time leave sat? M平近点角rt
	    //cout<<"M平近点角="<<M<<endl;
	double E,E0,Ek;
	   //迭代偏近点角
	E0=M;
	   do
	   {   
		   Ek=E0;
		   E=M+vn[posk].e*sin(Ek);
		   E0=E;
	   }while(fabs(E-Ek)>1e-12);//rt
	   //cout<<"E偏近点角="<<E<<"--"<<E-Ek<<endl;
	double vk=atan2(sqrt(1.0-vn[posk].e*vn[posk].e)*sin(E),cos(E)-vn[posk].e);//rt
	   //cout<<"真近点角vk="<<vk<<endl;
	double faik=vn[posk].w+vk;//升交距角//rt
	   //cout<<"升交距角bigfaik="<<faik<<endl;
	   //2阶调和改正数 
	double duk=vn[posk].Cus*sin(2*faik)+vn[posk].Cuc*cos(2*faik);
	double drk=vn[posk].Crs*sin(2*faik)+vn[posk].Crc*cos(2*faik);
	double dik=vn[posk].Cis*sin(2*faik)+vn[posk].Cic*cos(2*faik);
	  //cout<<"duk="<<duk<<" drk="<<drk<<" dik="<<dik<<endl;
        //改正后升交距角
	double uk=faik+duk;
	double rk=vn[posk].sqrtA*vn[posk].sqrtA*(1.0-vn[posk].e*cos(E))+drk;
	double ik=vn[posk].i0+dik+vn[posk].i_DOT*tk;//???????????????????
	  //cout<<"uk="<<uk<<" rk="<<rk<<" ik="<<ik<<endl;
	    //卫星在轨道平面上的位置
	double xkd=rk*cos(uk);
	double ykd=rk*sin(uk);//rt
	    //改正后升交点经度
	double omgak=vn[posk].OMEGA+vn[posk].OMEGA_DOT*tk-we*t2;//rt
	//double o1=vn[posk].OMEGA+(vn[posk].OMEGA_DOT-we)*tk-we*vn[posk].TOE.num_sec;
	//cout<<"xk="<<xkd<<" yk="<<ykd<<" omgak"<<omgak<<endl;
	    //cout<<"omgak="<<omgak<<"--"<<"o2="<<o2<<endl;
		//vn[posk].OMEGA+(vn[posk].OMEGA_DOT-we)*tk-we*vn[posk].TOE.num_sec;//?????????
	    //地固坐标系下的位置
	double xk=xkd*cos(omgak)-ykd*cos(ik)*sin(omgak);
	double yk=xkd*sin(omgak)+ykd*cos(ik)*cos(omgak);
	double zk=ykd*sin(ik);
	//自转改正	
    double om=pi*2.0/(24.0*3600.0);
	double tau=tr-t2;//??.0?????????????
	     //cout<<"tau="<<tau<<endl;
    double x=cos(om*tau)*xk+yk*sin(om*tau);
	double y=cos(om*tau)*yk-xk*sin(om*tau);
	double z=zk;  	
	sp.Ap=x;
	sp.Ep=y;
	sp.Rp=z;
}