// try.cpp : Defines the entry point for the console application.
//
#include "stdafx.h"
#include "help.h"
#include "GPSTime.h"
#include <windows.h>
#include "matrix.h"
#include "rinex.h"
#include <assert.h>
#include <math.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <string>
#include <iomanip>
#include <algorithm>
using namespace std;
const double c=3.0e+8;
void getDouble(string input, double &output)
{
char *p;
output = strtod( input.c_str(), &p );
}
void getLong(string input, long &output)
{
char *p;
output = strtol( input.c_str(), &p, 10 );
}
void makeRecordLength80(string &inputRec)
{
string blank(" ");
size_t slen;
if(inputRec.length()<80)
{
slen=80-inputRec.length();
inputRec.append(blank,0,slen);
}
}
bool blankstring(string inputStr)
{
for(int i=0;i<(unsigned short)inputStr.length();i++)
{
if(inputStr[i]!=' ')
{
return false;
}
}
return true;
}
void coordinatorcompute(ObsatEpoch &num,ObsCountForPRN *sat,YMDHMS &time)
{
double X=0.0 ,Y=0.0 ,Z=0.0;
double p0,dX,dY,dZ,tempX,tempY,tempZ,tempL;
int count=0,j,i;
for(i=0;i<num.numPRN;i++)
if(sat[i].getID()==0)
count++;
int newcount=num.numPRN-count;
GPSTime ofile(num),nfile(time);
do
{
double **B=new double*[newcount];
assert(B!=NULL);
for(i=0;i<newcount;i++)
{
B[i]=new double[4];
assert(B[i]!=NULL);
}
double **l=new double*[newcount];
assert(l!=NULL);
for(i=0;i<newcount;i++)
{
l[i]=new double[1];
assert(l[i]!=NULL);
}
for(i=0,j=0;i<newcount&&j<num.numPRN;i++,j++)
{
while(sat[j].getID()==0)
{
j++;
}
dX=sat[j].getX()-X;
dY=sat[j].getY()-Y;
dZ=sat[j].getZ()-Z;
p0=sqrt(pow(dX,2)+pow(dY,2)+pow(dZ,2));
tempL=sat[j].getP()-p0-c*(sat[j].geta0()+sat[j].geta1()*(ofile.getsecsOfWeek()-nfile.getsecsOfWeek())+
sat[j].geta2()*pow(ofile.getsecsOfWeek()-nfile.getsecsOfWeek(),2));
B[i][0]=-dX/p0;
B[i][1]=-dY/p0;
B[i][2]=-dZ/p0;
B[i][3]=-1;
l[i][0]=tempL;
}
CMatrix matrixB(newcount,4);
matrixB.set(B);
CMatrix matrixl(newcount,1);
matrixl.set(l);
CMatrix matrixBT=matrixB.Transpose();
CMatrix Nb=matrixBT*matrixB;
CMatrix W=matrixBT*matrixl;
CMatrix antNb=Nb.Contrary();
CMatrix dx=antNb*W;
tempX=X;
tempY=Y;
tempZ=Z;
X+=dx.get(0,0);
Y+=dx.get(1,0);
Z+=dx.get(2,0);
dx.Free();
antNb.Free();
W.Free();
Nb.Free();
matrixBT.Free();
matrixB.Free();
matrixl.Free();
for(i=0;i<newcount;i++)
{
delete []B[i];
B[i]=NULL;
delete []l[i];
l[i]=NULL;
}
B=NULL;
l=NULL;
}while(fabs(X-tempX)>1e-6||fabs(Y-tempY)>1e-6||fabs(Z-tempZ)>1e-6);
ofstream out("h:\\站坐标.txt",ios::app);
out.setf(ios::fixed,ios::floatfield);
out.setf(ios::showpoint);
out<<"X="<<setw(15)<<setprecision(3)<<X<<" ,Y="<<setw(15)<<setprecision(3)
<<Y<<" ,Z="<<setw(15)<<setprecision(3)<<Z<<endl;
out.close();
cout.setf(ios::fixed,ios::floatfield);
cout.setf(ios::showpoint);
cout<<"X="<<setw(15)<<setprecision(3)<<X<<" ,Y="<<setw(15)<<setprecision(3)
<<Y<<" ,Z="<<setw(15)<<setprecision(3)<<Z<<endl;
ofstream out1("h:\\卫星坐标.txt",ios::app);
out1.setf(ios::fixed,ios::floatfield);
out1.setf(ios::showpoint);
for(i=0;i<num.numPRN;i++)
{
if(sat[i].getID()==0)
continue;
out1<<num.Year<<" "<<num.Month<<" "<<num.Day<<" "<<num.Hour<<" "<<num.minute<<" "<<num.Sec<<endl;
out1<<setw(2)<<sat[i].getsatNum()<<" X="<<setw(15)<<setprecision(3)<<sat[i].getX()
<<" Y="<<setw(15)<<setprecision(3)<<sat[i].getY()<<" Z="<<setw(15)<<setprecision(3)<<sat[i].getZ()<<endl;
}
out1.close();
sat=NULL;
}
int main(int argc, char* argv[])
{
int p1num,p2num,numoftype;
int key;
string temp;
string str;
long tempL;
double tempD;
YMDHMS time;
//char filenameandpath[100];
//cout<<"输入观测文件名及路径:\n";
//cin>>filenameandpath;
ifstream inputStream("h:\\algo0080.08o\\algo0080.08o");
while(!inputStream.eof())
{
getline(inputStream,str,'\n');
if(str.find("# / TYPES OF OBSERV")==60) break;
}
string obsType;
temp=str.substr(6,54);
istringstream nextObs(temp);
int i=0;
while(nextObs>>obsType)
{
if(obsType.find("P1")!=string::npos)
{
p1num=i;
}
if(obsType.find("P2")!=string::npos)
{
p2num=i;
}
i++;
}
numoftype=i;
while(!inputStream.eof())
{
getline(inputStream,str,'\n');
if(str.find("END OF HEADER")==60) break;
}
while(!inputStream.eof())
{
getline(inputStream,str,'\n');
ObsatEpoch a;
temp=str.substr(1,2);
getLong(temp,tempL);
a.Year=static_cast<unsigned short> (tempL);
if(a.Year>=80&&a.Year<=99) a.Year+=1900;
if(a.Year>=0&&a.Year<=79) a.Year+=2000;
temp=str.substr(4,2);
getLong(temp,tempL);
a.Month=static_cast<unsigned short> (tempL);
temp=str.substr(7,2);
getLong(temp,tempL);
a.Day=static_cast<unsigned short> (tempL);
temp=str.substr(10,2);
getLong(temp,tempL);
a.Hour=static_cast<unsigned short> (tempL);
temp=str.substr(13,2);
getLong(temp,tempL);
a.minute=static_cast<unsigned short> (tempL);
temp=str.substr(15,11);
getDouble(temp,tempD);
a.Sec=tempD;
temp=str.substr(29,3);
getLong(temp,tempL);
a.numPRN=static_cast<unsigned short> (tempL);
ObsCountForPRN *PRN=new ObsCountForPRN[a.numPRN];
for(i=0;i<a.numPRN;i++)
{
temp=str.substr((32+i*3),1);
PRN[i].setsatCode(temp[0]);
temp=str.substr((32+(i*3)+1),2);
getLong(temp,tempL);
PRN[i].setsatNum(static_cast<unsigned short> (tempL));
}
for(int j=0;j<a.numPRN;j++)
{
getline(inputStream,str,'\n');
makeRecordLength80(str);
if(numoftype>5&&numoftype<11)
{
getline(inputStream,temp,'\n');
str.append(temp);
}
else if(numoftype==11)
{
getline(inputStream,str,'\n');
str.append(temp);
getline(inputStream,str,'\n');
str.append(temp);
}
temp=str.substr((p1num*16),14);
getDouble(temp,tempD);
PRN[j].setP1(tempD);
temp=str.substr((p2num*16),14);
getDouble(temp,tempD);
PRN[j].setP2(tempD);
PRN[j].setP(2.54573*PRN[j].getP1()-1.54573*PRN[j].getP2());
}
ifstream inputStream1("h:\\algo0080.08n\\algo0080.08n");
while(!inputStream1.eof())
{
getline(inputStream1,str,'\n');
if(str.find("END OF HEADER")==60)
break;
}
key=0;
while(!inputStream1.eof())
{
//YMDHMS time;
getline(inputStream1,str,'\n');
if(blankstring(str)) break;
replace(str.begin(),str.end(),'D','E');
temp=str.substr(0,2);
getLong(temp,tempL);
time.PRNnum=static_cast<unsigned short> (tempL);
temp=str.substr(3,2);
getLong(temp,tempL);
time.year=static_cast<unsigned short> (tempL);
if(time.year>=80&&time.year<=99)
time.year+=1900;
if(time.year>=0&&time.year<=79)
time.year+=2000;
temp=str.substr(6,2);
getLong(temp,tempL);
time.month=static_cast<unsigned short> (tempL);
temp=str.substr(9,2);
getLong(temp,tempL);
time.day=static_cast<unsigned short> (tempL);
temp=str.substr(12,2);
getLong(temp,tempL);
time.hour=static_cast<unsigned short> (tempL);
temp=str.substr(15,2);
getLong(temp,tempL);
time.min=static_cast<unsigned short> (tempL);
temp=str.substr(17,5);
getDouble(temp,tempD);
time.sec=tempD;
GPSTime ofile(a);
GPSTime nfile(time);
if(ofile.getGPSWeek()==nfile.getGPSWeek()
&&fabs(ofile.getsecsOfWeek()-nfile.getsecsOfWeek())<=3600)
{
for(j=0;j<a.numPRN;j++)
{
if(time.PRNnum==PRN[j].getsatNum()&&PRN[j].getID()==0)
{
key++;
PRNBlock prnBlock;
prnBlock.setSatellitePRN(time.PRNnum);
prnBlock.setTocYear(time.year);
prnBlock.setTocMonth(time.month);
prnBlock.setTocDay(time.day
评论0
最新资源