pos.cpp

利用广播星历计算卫星轨道坐标

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

//
void sat_pos(int& posk,double& tm,double& tr,sta_polar& sp,vector<nav_sat>& vn)  
{ 
	//vector<nav_sat> vn;
	double t2=vn[posk].TOE.num_sec+tm*60;
	int sec=vn[posk].TOE.num_sec/100,vs=0;
	sec=sec*100;
    vs=vn[posk].TOE.num_sec-sec;
    if(vs!=0)
	{
		t2=t2+100-vs;
		//cout<<100-vs<<endl;
	}
	
    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=xk;
	   sp.Ep=yk;
	   sp.Rp=zk;
}