?? jjj.cpp
字號:
#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);//聲明時間轉(zhuǎn)換
//////////////////////--------------主函數(shù)開始-----------------
int main()
{
//----------------------Read N File---------------------------------
vector<nav_sat> vn;
readNfile(vn);
cout<<"N文件循環(huá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衛(wèi)星信號接收時刻,ts發(fā)射時刻
ofstream outfile("各個歷元坐標.txt",ios::out);
ofstream outfile1("衛(wèi)星坐標.txt",ios::out);
cout<<setiosflags(ios::fixed)<<setprecision(15);
sta_polar sp;//(x,y,z)坐標,借用sta_polar結(jié)構(gòu)的
vector<sta_polar> sps;
Matrix deltx(4,1);
deltx(3,0)=0;//衛(wèi)星鐘差
double sumx=0,sumy=0,sumz=0,ex,ey,ez;
for(epc=0;epc<v.size();epc++)//歷元總數(shù)=v.size()
{ Matrix P(v[epc].num_sat,v[epc].num_sat);//權(quán)
Matrix dT(v[epc].num_sat,1);//衛(wèi)星種差
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();//清空容器，不然會一直存上一歷元的衛(wèi)星坐標
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文件的衛(wèi)星號0,1,2..8顆;/////////////////
{ //[epc].num_sat指本歷元的衛(wèi)星個數(shù) //
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;
}//獲得間隔最小的衛(wèi)星的位置 //
}
} //
//cout<<"第"<<v[epc].array_sat[j].sat_num<<"號衛(wèi)星最近的軌道----";
//cout<<"于N文件的第"<<posk<<"組,衛(wèi)星號:"<<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迭代衛(wèi)星位置，求衛(wèi)星發(fā)射時刻 //
{
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);//應(yīng)當用完后清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<<"衛(wèi)星,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);//+衛(wèi)星鐘差+大氣延遲？
}
deltx=-(!((~A)*A))*((~A)*L);//解法方程,無權(quán)
//deltx=-(!((~A)*P*A))*((~A)*P*L);//解法方程,有權(quán)
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<<"顆衛(wèi)星的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
//格里高歷轉(zhuǎn)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;
}
?? 快捷鍵說明
復(fù)制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -