observefile.cpp

一个ｇｐｓ单点定位的程序

// ObserveFile.cpp: implementation of the CObserveFile class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include "GPS.h"
#include "ObserveFile.h"
#include "math.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////

int i;
CObserveFile::CObserveFile()
{
	for(i=0;i<20;i++)
	{
		L1[i]=0;
		L2[i]=0;
		P1[i]=0;
		P2[i]=0;
		C1[i]=0;
		PRNS[i]=0;
	}
	isCaculated=false;
	X_O=0;
	Y_O=0;
	Z_O=0;
	ZC=0;
	
	HDOP=0;
	VDOP=0;
	PDOP=0;
	TDOP=0;
	GDOP=0;

}

CObserveFile::~CObserveFile()
{

}
CObserveFile* CObserveFile::operator = (CObserveFile &src)
{
	if(this!=&src)
	{
		surveyTime=src.surveyTime ;
		surveyCount=src.surveyCount ;
	    isCaculated=src.isCaculated ;
		for(int i=0;i<20;i++)
		{
			C1[i]=src.C1[i];
			L1[i]=src.L1[i];
			L2[i]=src.L2[i];
			P1[i]=src.P1[i];
			P2[i]=src.P2[i];
   
			PRNS[i]=src.PRNS[i];

			Salite[i]=src.Salite [i];
		}
		X_O=src.X_O;
		Y_O=src.Y_O;
		Z_O=src.Z_O;
		ZC=src.ZC ;
	}
	return this;

}

void CObserveFile::SetSurveyCount(int count)
{
	surveyCount=count;
}

int CObserveFile::GetSurveyCount()
{
	return surveyCount;
}

void CObserveFile::SetL1(double data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			L1[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}

void CObserveFile::SetL2(double data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			L2[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}

void CObserveFile::SetP1(double data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			P1[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}

void CObserveFile::SetP2(double data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			P2[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}

void CObserveFile::SetC1(double data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			C1[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}
void CObserveFile::SetPRNS(int data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			PRNS[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}

void CObserveFile::SetSalite(CNavigationFile data[], int size)
{
	if(size<=20)
	{
		for(i=0;i<size;i++)
		{
			Salite[i]=data[i];
		}
	}
	else
		AfxMessageBox("程序运行错误：数组长度不能超过20");


}
void CObserveFile::SetSurveyTime(CTime time)
{
	surveyTime=time;
}

void CObserveFile::CaculateCor()
{
	int j,k;
	//计算观测历元卫星序列坐标
	for(i=0;i<surveyCount;i++)
	{
		Salite[i].SetTime(surveyTime.GetYear(),
						surveyTime.GetMonth(),
						surveyTime.GetDay(),
						surveyTime.GetHour(),
						surveyTime.GetMinute(),
						surveyTime.GetSecond());

		Salite[i].CaculateCOR();
		
	}
	//给定测站坐标初值
    double X0=1000;
	double Y0=1000;
	double Z0=1000;
    double ZC0=0;

	double* A=new double[surveyCount*4]; //误差方程系数阵
	double* L=new double [surveyCount];  //误差方程常数项
	double* V=new double [surveyCount];  //误差方程观测误差
	double C[4][4];                      //Na
	double W[4];                         //U
	double Q[4][4];  //权逆阵
	double Result[4];
  
	const double CL=2.99792458e+8;   //光速
    
	bool isOK=false;
	int count=0;
	//迭代求解
	while(!isOK)
	{
		//组成系数阵
		for(i=0;i<surveyCount;i++)
		{
			double Xs,Ys,Zs ,detT;//卫星坐标
			double p0;
			Xs=Salite[i].GetX();
			Ys=Salite[i].GetY();
			Zs=Salite[i].GetZ();
			detT=Salite[i].GetDetT();

			p0=sqrt((Xs-X0)*(Xs-X0)+(Ys-Y0)*(Ys-Y0)+(Zs-Z0)*(Zs-Z0));
			
			A[4*i]=(X0-Xs)/p0;
			A[4*i+1]=(Y0-Ys)/p0;
			A[4*i+2]=(Z0-Zs)/p0;
			A[4*i+3]=1;
			
			L[i]=C1[i]-p0+CL*detT;
			
			V[i]=0;
		}
		
		//组成法方程
		
		for(i=0;i<4;i++)    ///////////////////////// C
		{
			for(j=0;j<4;j++)
			{
				C[i][j]=0;
				for(k=0;k<surveyCount;k++)
				{
					C[i][j]=C[i][j]+A[k*4+i]*A[k*4+j];
				}
				
			}
		}
		
		for(i=0;i<4;i++)   ///////////////////////// W
		{
			W[i]=0;
			for(j=0;j<surveyCount;j++)
			{
				W[i]=W[i]+A[j*4+i]*L[j];
			}
		}
		
		//解算法方程
		for(i=0;i<4;i++)
		{
			for(j=0;j<4;j++)
			{
				Q[i][j]=C[i][j];
			}
		}
		int isSuccess=Dinvert(&Q[0][0],4,4);
		if(isSuccess!=0)
		{
			AfxMessageBox("无法解算法方程：矩阵求逆失败");
			isCaculated=false;                    //标记解算失败
			return;
		}
		//dx dy dz
		for(i=0;i<4;i++)
		{
			Result[i]=0;
			for(j=0;j<4;j++)
			{
				Result[i]=Result[i]+Q[i][j]*W[j];
			}
		}
		/*//计算改正数
        for(i=0;i<surveyCount;i++)
		{
			V[i]=0;
			for(j=0;j<4;j++)
			{
				V[i]=V[i]+A[4*i+j]*Result[j];
			}
			V[i]=V[i]-L[i];
		}*/
		X0=X0+Result[0];
		Y0=Y0+Result[1];
		Z0=Z0+Result[2];
        ZC0=ZC0+Result[3];
         
		if(++count==10)
			break;
	}
	this->X_O=X0+Result[0];
	this->Y_O=Y0+Result[1];
	this->Z_O=Z0+Result[2];
	this->ZC=(ZC0+Result[3])/CL;

	//释放指针
	A=NULL;
	delete A;

	L=NULL;
	delete L;

	V=NULL;
	delete V;
}
//////////////////////////////////////////////////////////////////////////
//Function: Dinvert
//SUBROUTINE:      dinvert
//     INVERT BY CHOLESKY DECOMP
//
//     RETURN:   0 - success
//               1 - matrix not positive definite
//               2 - Singularity
//               3 - Can't sqrt a neg. number
//               4 - Can't divide by zero
//     WARNING: you must check return value
//
//////////////////////////////////////////////////////////////////////////

int CObserveFile::Dinvert(double *mat, int n, int ln)
{
	int i,j,k;
	double sum;
	//int ln=n;
	for (i=0;i<n;i++)
	{
		if (mat[i*ln+i]<0.0)	return 1;

		if (fabs(mat[i*ln+i])<0.000000000001) return 2;
	}
	if (n==1) 
	{
		mat[0]=1.0/mat[0];
		return 0;
	}
	if (n==2)
	{
		sum=mat[0];
		mat[0]=mat[ln+1];
		mat[ln+1]=sum;
		sum=mat[0]*mat[ln+1]-mat[1]*mat[ln];
		if (fabs(sum)<0.000000000001) return 4;
		mat[0]/=sum;
		mat[1]/=-sum;
		mat[ln]/=-sum;
		mat[ln+1]/=sum;
		return 0;
	}
	
	
	for (j=0;j<n;j++)
	{
		for (k=0;k<j;k++)
			mat[j*ln+j]-=mat[j*ln+k]*mat[j*ln+k];
		if (mat[j*ln+j]<0.0)
			return 3;
		mat[j*ln+j]=sqrt(mat[j*ln+j]);
		
		for (i=j+1;i<n;i++)
		{
			for (k=0;k<j;k++)
			{
				mat[i*ln+j]-=mat[i*ln+k]*mat[j*ln+k];
			}
			
			if (fabs(mat[j*ln+j])<0.000000000001) return 4;
			mat[i*ln+j]/=mat[j*ln+j];
		}
	}
	
	for (j=0;j<n;j++)
	{
		mat[j*ln+j]=1.0/mat[j*ln+j];
		for (i=j+1;i<n;i++)
		{
			mat[i*ln+j]=-mat[i*ln+j]*mat[j*ln+j]/mat[i*ln+i];
			for (k=j+1;k<i;k++)
				mat[i*ln+j]-=mat[i*ln+k]*mat[k*ln+j]/mat[i*ln+i];
		}
	}
	
	for (j=0;j<n;j++)
		for (i=j;i<n;i++)
		{
			mat[i*ln+j]*=mat[i*ln+i];
			for (k=i+1;k<n;k++)
				mat[i*ln+j]+=mat[k*ln+i]*mat[k*ln+j];
		}
		
	for (i=1;i<n;i++)
		for (j=0;j<i;j++)
			mat[j*ln+i]=mat[i*ln+j];
	return 0;


}

double CObserveFile::GetX_O()
{
	return X_O;
}

double CObserveFile::GetY_O()
{
	return Y_O;

}

double CObserveFile::GetZ_O()
{
	return Z_O;

}

double CObserveFile::GetZC()
{
	return ZC;

}

CString CObserveFile::GetSurveyTime()
{
	CString result;
	CString str1,str2,str3;
    
	int year=surveyTime.GetYear();
	int month=surveyTime.GetMonth();
	int day=surveyTime.GetDay();
	int hour=surveyTime.GetHour();
	int minute=surveyTime.GetMinute();
	int second=surveyTime.GetSecond();

	str1.Format("%d",hour);
	if(hour<10)
	{
		str1="0" + str1;
	}

	str2.Format("%d",minute);
	if(minute<10)
	{
		str2="0" + str2;
	}
	str3.Format("%d",second);
	if(second<10)
	{
		str3="0" + str3;
	}

	result.Format ("%d %d.%d %s:%s:%s",year,month,day,str1,str2,str3);

	return result;

}