Single-Pos_VCPP.rar_c++GPS单点定位_single_单点定位_单点定位c++_高精度GPS

#include <stdio.h> #include <string.h> #include <stdlib.h> #include <malloc.h> #include <math.h> #include "readobs.h" #include "readnav.h" //#include "svcal.h" #include "poscal.h" void timeToGPStime(GPSOBSREC *p1, GPSNAVMSGREC *p2); void gaussj(double A[4][4], double B[4][4]); void poscal(GPSOBSMSG gpsobsmsg1,GPSNAVMSG gpsnavmsg1)//,GPSSVMSG gpssvmsg1 { double c=299792458; double gm=398600500000000; double omegae=0.000072921151467; double n[4][4]; //定义法方程数组 double nf[4][4]; GPSPOSDAT gpsposdat; double dt; double dtm; double md; double na; // double deltare0; double deltrou0; double ns[7][4]; GPSSVPOS1 gpssvpos; double dsvx,dsvy,dsvz; double deltalfa; double dbl[4]; double dl; double ds; double navlweek,navlsecond; double navdClkBias,navdClkDrift,navdClkDriftRate; double dposx,dposy,dposz; double dME; double fz,fm; double pi=3.14159265358979; char *ca; int number; int num; int i,j,k; int count=0; int count1=0; FILE *fp5; if((fp5=fopen("输出数据/点坐标.txt","w"))==NULL) { printf("文件打开失败\n"); } //赋值 // gpssvmsg.gpssvpos = gpssvmsg1.head; // gpssvmsg.gpssvpos = gpssvmsg1.gpssvpos->next; // gpssvmsg.gpssvpos = (GPSSVPOS*)malloc(sizeof(GPSSVPOS)); //将观测历元全部转化成GPS时间 timeToGPStime(gpsobsmsg1.head,gpsnavmsg1.head); gpsobsmsg1.Record = gpsobsmsg1.head; printf("%d %20.20lf\n",gpsobsmsg1.Record->Hdr.lWeek,gpsobsmsg1.Record->Hdr.lSecond); gpsnavmsg1.Record = gpsnavmsg1.head; printf("%d %20lf\n",gpsnavmsg1.Record->lWeek,gpsnavmsg1.Record->lSecond); // printf("%5d%5d%5d%5d%5d%5d%10lf\n",gpsnavmsg1.Record->byPRN,gpsnavmsg1.Record->TOC.year,gpsnavmsg1.Record->TOC.mon,gpsnavmsg1.Record->TOC.day,gpsnavmsg1.Record->TOC.hour,gpsnavmsg1.Record->TOC.min,gpsnavmsg1.Record->TOC.sec); //提取C/A码 for(i = 0; i gpsobsmsg1.Header.byNumofobaType; i++) { ca = gpsobsmsg1.Header.pbyobstype[i].pbyObsTypeList; // printf("%s\n",gpsobsmsg1.Header.pbyobstype[i].pbyObsTypeList); if( strcmp(ca,"C1") == 0) { count = count + i; } } printf("\n%d\n",count); gpsobsmsg1.Record = gpsobsmsg1.head; // gpsobsmsg1.Record = gpsobsmsg1.Record->next; number = 0; while(gpsobsmsg1.Record != NULL) { //法方程赋初值 for(i=0;i<4;i++) { for(j=0;j<4;j++) { n[i][j] = 0.0; nf[i][j] = 0.0; } dbl[i] = 0; } //将地面点坐标和接收机钟改正数设为0 gpsposdat.dx = 0; gpsposdat.dy = 0; gpsposdat.dz = 0; gpsposdat.deltare = 0; //给改正数赋初始值 gpsposdat.deltax = gpsposdat.deltay = gpsposdat.deltaz = 10; while( (fabs(gpsposdat.deltay) > 0.0000001) ) { for(i=0;i<4;i++) { for(j=0;j<4;j++) { n[i][j] = 0.0; nf[i][j] = 0.0; } dbl[i] = 0; } // printf("成功\n"); for(num = 0; num gpsobsmsg1.Record->Hdr.bySatNum; num++) { //提取导航电文里的观测数值 gpsnavmsg1.Record = gpsnavmsg1.head; while(gpsnavmsg1.Record != NULL) { if(gpsobsmsg1.Record->Hdr.sv[num] == gpsnavmsg1.Record->byPRN) { na= (gpsobsmsg1.Record->Hdr.lWeek - gpsnavmsg1.Record->dGPSWeek) * 604800 + gpsobsmsg1.Record->Hdr.lSecond - gpsnavmsg1.Record->dTOE; if(na <3600 ) { navlweek = gpsnavmsg1.Record->lWeek; navlsecond = gpsnavmsg1.Record->lSecond; navdClkBias = gpsnavmsg1.Record->dClkBias; navdClkDrift = gpsnavmsg1.Record->dClkDrift; navdClkDriftRate = gpsnavmsg1.Record->dClkDriftRate; break; } } gpsnavmsg1.Record = gpsnavmsg1.Record->next; } dt = gpsobsmsg1.Record->Obs.dC1[num][count]/c + gpsposdat.deltare; dtm = 0; // printf("%20lf\n",gpsobsmsg1.Record->Obs.dC1[num][count]); while( fabs(dt - dtm) > 0.000000001) { dtm = dt; gpsnavmsg1.Record = gpsnavmsg1.head; while(gpsnavmsg1.Record != NULL) { if(gpsobsmsg1.Record->Hdr.sv[num] == gpsnavmsg1.Record->byPRN) { na= (gpsobsmsg1.Record->Hdr.lWeek - gpsnavmsg1.Record->dGPSWeek) * 604800 + gpsobsmsg1.Record->Hdr.lSecond - gpsnavmsg1.Record->dTOE; if(na <3600 ) { //计算卫星运动的平均角速度n gpssvpos.dA = gpsnavmsg1.Record->dSqrtA * gpsnavmsg1.Record->dSqrtA; gpssvpos.dn0 = sqrt(gm)/(pow(gpsnavmsg1.Record->dSqrtA,3)); gpssvpos.dn = gpssvpos.dn0 + gpsnavmsg1.Record->dDeltaN; //计算观测瞬间卫星的平近点角M gpssvpos.dtk = gpsobsmsg1.Record->Hdr.lSecond - dtm - gpsnavmsg1.Record->dTOE; if(gpssvpos.dtk > 302400) gpssvpos.dtk = gpssvpos.dtk - 604800; if(gpssvpos.dtk -302400) gpssvpos.dtk = gpssvpos.dtk + 604800; gpssvpos.dM = gpsnavmsg1.Record->dM0 + gpssvpos.dn * gpssvpos.dtk; //计算偏近点角E采取迭代运算 gpssvpos.dE = gpssvpos.dM; dME = 0.0; while(fabs(gpssvpos.dE - dME) > 0.00000000001) {