adjpro0504.cpp

GPS网平差计算

#include<iostream.h>
#include<fstream.h>
#define MAX 100
#define PI 3.14159265358979312
#define rou (180.0*60*60/PI)
#include<math.h>
#include<string.h>

//***************************矩阵求逆函数说明**************************************
int inverse(double C[][MAX],double B[][MAX],int n);
//define the function of inverseing;
//return 0 means mat can't be inversed or;
//return 1 means mat inversed correctly, and B is inversed mat;

//***************************矩阵乘积函数说明**************************************
void AXB(double A[][MAX],double B[][MAX],double C[][MAX],int m,int n,int k);
void AXB(double a,double A[][MAX], double aA[][MAX],int m,int n); 
void AXB(double A[][MAX],double B[][1],double C[][1],int m,int n);
//define the time fuction of mats or mat and a number

//***************************矩阵转置函数说明**************************************
void AT(double A[][MAX],double AH[][MAX],int m,int n);
void AT(double A[][1],double AH[][MAX],int m);
void AT(double A[][MAX],double AH[][1],int m);
//define the fuction to turn-over a mat 

//***************************平差计算相关函数**************************************
void ATPA(double A[][MAX],double P[][MAX],double ATPA[][MAX],int m,int n);
void ATPL(double A[][MAX],double P[][MAX],double L[][1],double ATPL[][1],int m,int n);
double VPV(double V[][1],double P[][MAX],int m);
//define initial fuctions of doadj()

//**************************矩阵显示***********************************************
void matdis(double A[][MAX],int n,int m);
void matdis(double A[][1],int n);
//define two fuctions to output a mat to screen  

//**********************通用平差相关结构与函数*************************************
struct adj;
void ksetadj(adj &a);     //input data from keyboard 
int fsetadj(adj &aa,char name[20]);  //input data from a file 
int  doadj(adj &a);       //adjusting a adj 
int rubust(adj &a);       //抗差估计
void adjdis(adj &aa);     //output a adj results to screen 
int foutadj(adj &aa, char name[20]); //outputdate to a file
//define adj and it related fuctions

//************************************************************************************************
void AXB(double A[][MAX],double B[][MAX],double C[][MAX],int m,int n,int k)
{
   for(int i=0;i<m;i++)
	   for(int j=0;j<k;j++)
	   {
		   C[i][j]=0;
          for(int l=0;l<n;l++)
		  C[i][j]+=A[i][l]*B[l][j];
	   }
}
//************************************************************************************************

void AXB(double A[][MAX],double B[][1],double C[][1],int m,int n)
{
for(int i=0;i<m;i++)
	   for(int j=0;j<1;j++)
	   {
		   C[i][j]=0;
          for(int l=0;l<n;l++)
		  C[i][j]+=A[i][l]*B[l][j];
	   }

}
//************************************************************************************************

void AXB(double a,double A[][MAX], double aA[][MAX],  int m,int n) //void  
{
     for(int i=0;i<m;i++)
	   for(int j=0;j<n;j++)
		   aA[i][j]=a*A[i][j];

}
//************************************************************************************************

void AT(double A[][MAX],double AH[][MAX],int m,int n)   // 矩阵转置
{
    for(int i=0;i<m;i++)
	   for(int j=0;j<n;j++)
		   AH[j][i]=A[i][j];
}
//************************************************************************************************

void AT(double A[][1],double AH[][MAX],int m)    //  列向量转置为行向量
{
    for(int i=0;i<m;i++)
	   	   AH[0][i]=A[i][0];
}
//************************************************************************************************

void AT(double A[][MAX],double AH[][1],int m)    //  行向量转置为列向量
{
    for(int i=0;i<m;i++)
	   	   AH[i][0]=A[0][i];
}
//************************************************************************************************


void ATPA(double A[][MAX],double P[][MAX],double ATPA[][MAX],int m,int n)
{double AH[MAX][MAX],ATP[MAX][MAX];
  AT(A,AH,m,n);  
  AXB(AH,P,ATP,n,m,m);
  AXB(ATP,A,ATPA,n,m,n);
}
//************************************************************************************************


double VPV(double V[][1],double P[][MAX],int m)
{double VH[1][MAX],VTP[1][MAX];
     for(int i=0;i<m;i++)  
		 VH[0][i]=V[i][0];
       for(i=0;i<m;i++)
	   {		   
		   VTP[0][i]=0;
	       for(int j=0;j<m;j++)
			   VTP[0][i]+=VH[0][j]*P[j][i];		   
	   }
		   
  double dd(0);
  for (i=0;i<m;i++)
	  dd+=VTP[0][i]*V[i][0];
  return dd;
}
//************************************************************************************************
void ATPL(double A[][MAX],double P[][MAX],double L[][1],double ATPL[][1],int m,int n)
{
  double AH[MAX][MAX],ATP[MAX][MAX];
  AT(A,AH,m,n);  
  AXB(AH,P,ATP,n,m,m);
  for(int i=0;i<n;i++)
  {
      ATPL[i][0]=0;
	  for(int j=0;j<m;j++)
		  ATPL[i][0]+=ATP[i][j]*L[j][0];
  }
}
//************************************************************************************************ 
void matdis(double A[][MAX],int n,int m) // 显示矩阵
{//1.set B[][] I;
   for(int i=0;i<n;i++)
   {  cout<<"     ";
	  for(int j=0;j<m;j++)
	  cout<<A[i][j]<<"  ";
		 cout<<endl;
   }
}
//************************************************************************************************
void matdis(double A[][1],int n)       //  显示列向量
{//1.set B[][] I;
   for(int i=0;i<n;i++)
      cout<<"     "<<A[i][0]<<endl;
 
}
//************************************************************************************************
int inverse(double C[][MAX],double B[][MAX],int n)
{//1.set B[][] I;
	double A[MAX][MAX],e;
   for(int i=0;i<n;i++)
	   for(int j=0;j<n;j++)
	   {
		   A[i][j]=C[i][j];
		   if(i==j) B[i][j]=1;
	       else B[i][j]=0;
	   
	   }
	
 //2. inverse and judge the Matrix inversable
     for(i=0;i<n;i++)
	 {
		 //对主元为零的处理
		 if(A[i][i]==0)
			 for(int j=i+1;j<n;j++)
			 {
				 if(A[j][i]!=0) 
				 for(int k=0;k<n;k++)
				 	 {
						 A[i][k]+=A[j][k];
						 B[i][k]+=B[j][k];
					 }
			    break;
			 }					
				 if(fabs(A[i][i])<0.000000000000001) 
				 {
					  for(int i=0;i<n;i++)
	                    for(int j=0;j<n;j++)
							B[i][j]=0;
				     return 0;
				 }//MAT can't be inversed
    	// line processing
		e=A[i][i];
		for(int j=0;j<n;j++)  
		 {
		   A[i][j]=A[i][j]/e;
		   B[i][j]=B[i][j]/e;
		 }
        // row processing
		for(j=0;j<n;j++)
		{
			e=A[j][i];
			for(int k=0;k<n;k++)
			 if(i!=j)
			 {
              A[j][k]+=-1*e*A[i][k];			 
			  B[j][k]+=-1*e*B[i][k];
			 }
		}
	 }
return 1;
}
//************************************************************************************************
double setf(double a, int t)
{
   double b=fabs(a);
   for(int i=0;i<t;i++)
	   b*=10;
   if(b-floor(b)>0.5) b=floor(b)+1;
   else b=floor(b);
   for(i=0;i<t;i++)
	   b/=10;
   if(a<0) b=-b;
    return b;
}
//************************************************************************************************
 struct adj{
// adjustment model :
//     	V=AX-L P 
//  	A[m][n], P[m][m], L[m][1]
char name[40];       //	V=AX-L P 
int m;               // the number of observations
int n;               // the number of unknown data
double A[MAX][MAX];  // paremater mat of unknown data 
double P[MAX][MAX];  // observation weight mat
double l[MAX][1];    // fix data mat
double X[MAX][1];    // unknown data mat
double QXX[MAX][MAX];// coherance date mat 
double m0;           // unit weight error  
double V[MAX][1];
int flag;            // flag=1 means adjustment successfully
}; 
//************************************************************************************************
void ksetadj(adj &a) //keyboard and screen input
{
  cout<<"  input the object  name:   ";
  cin>>a.name;cout<<endl;
  
  cout<<"  input observation number:   ";
  cin>>a.m;cout<<endl;
  
  cout<<"  input unknown data number:   ";
  cin>>a.n;cout<<endl;

  cout<<"  input data of mat A["<<a.m<<"]["<<a.n<<"]"<<endl;
  
  for(int i=0;i<a.m;i++)
	  for(int j=0;j<a.n;j++)
		  cin>>a.A[i][j];

cout<<"  input data of mat P["<<a.m<<"]["<<a.m<<"]"<<endl;
  
  for(i=0;i<a.m;i++)
	  for(int j=0;j<a.m;j++)
		  cin>>a.P[i][j];

cout<<"  input data of mat L["<<a.m<<"][1]"<<endl;
  
  for(i=0;i<a.m;i++)
	cin>>a.l[i][0];
 }
//************************************************************************************************
int fsetadj(adj &aa,char *name) //keyboard and screen input
{
  
	ifstream in(name,ios::nocreate);
	if(!in) return 0;
// 输入平差项目名    
	in>>aa.name;
  
// input observation number:  
	in>>aa.m;
  
// input unknown data number: 
    in>>aa.n;

// input data of mat A[m][n]:
  
  for(int i=0;i<aa.m;i++)
	  for(int j=0;j<aa.n;j++)
		  in>>aa.A[i][j];

// input data of mat P[m][m]:
  
  for(i=0;i<aa.m;i++)
	  for(int j=0;j<aa.m;j++)
		  in>>aa.P[i][j];

// input data of mat L[m][1]:
  
  for(i=0;i<aa.m;i++)
	in>>aa.l[i][0];
  in.close();
return 1;
}

//*******************************************************************************************

int  doadj(adj &a)            // 普通最小二乘平差
{
	double APA[MAX][MAX];
	ATPA(a.A,a.P,APA,a.m,a.n);
	int flag=inverse(APA,a.QXX,a.n);
	if(flag!=1) 
	{
		a.flag=0;
		return 0;
	}
	double AX[MAX][1];
	ATPL(a.A,a.P,a.l,AX,a.m,a.n);
	AXB(a.QXX,AX,a.X,a.n,a.n);
    AXB(a.A,a.X,AX,a.m,a.n);
	for(int i=0;i<a.m;i++)
		a.V[i][0]=AX[i][0]-a.l[i][0];
   double cc=VPV(a.V,a.P,a.m);
	a.m0=sqrt(cc/(a.m-a.n));
	a.flag=1;
	return 1;

}

//*******************************************************************************************
int doadj(adj &a,int known,int r)        // 极大权法最小二乘平差,known--控制点已知数据个数;
{                                        // r－固定数据个数＋测站数（平面网）
double APA[MAX][MAX];
	ATPA(a.A,a.P,APA,a.m,a.n);
	double add(0);
	for(int i=0;i<a.n;i++)
     if(add<=APA[i][i]) add=APA[i][i];
	add*=1000;
	for(i=0;i<known;i++)                 // 对已知点方法程系数阵的处理：
		APA[i][i]+=add;                  // 控制点点号对应矩阵主元加平均权的10000倍
	int flag=inverse(APA,a.QXX,a.n);
	if(flag!=1)                          // 不可求逆的判断及处理 
	{ a.flag=0;  return 0;}
	double AX[MAX][1];                   // 极大权平差过程
	ATPL(a.A,a.P,a.l,AX,a.m,a.n);
	AXB(a.QXX,AX,a.X,a.n,a.n);
    AXB(a.A,a.X,AX,a.m,a.n);
	for(i=0;i<a.m;i++)
		a.V[i][0]=AX[i][0]-a.l[i][0];
   double cc=VPV(a.V,a.P,a.m);           // VTPV计算
	a.m0=sqrt(cc/(a.m-a.n+known-r));     // 单位权中误差计算，必要观测数为（a.n-knownp）
	a.flag=1;                            // 平差完毕
	return 1;
}
//*******************************************************************************************

void adjdis(adj &aa)   
{
cout<<"   平差项目名 ："<<aa.name<<endl<<endl;
 
cout<<" 误差方程阵："<<endl;
  matdis(aa.A,aa.m,aa.n);
 
  cout<<endl<<endl<<" 观测值权矩阵："<<endl;
  matdis(aa.P,aa.m,aa.m);

  cout<<endl<<endl<<" 常数项："<<endl;
  matdis(aa.l,aa.m);