用c++自编的GPS单点定位程序（源码）

// 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