声明：本程序未经过工程实际测试，仅用于测绘程序的学习交流，望您留下对本程序宝贵的意见，谢谢！

适用于：水准网平差  三角网平差  水准网和三角网联合平差

#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 
void matout(double A[][MAX],int n,int m,ofstream out); // 向文件输出矩阵

//**********************通用平差相关结构与函数*************************************
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;
// cout.precision(12);
   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;
  ofstream ou("ATPA.txt");
  matout(APA,a.n,a.n,ou);
  ou.close();
 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*=100;
 for(i=0;i<known;i++)                 // 对已知点方法程系数阵的处理：
  APA[i][i]+=add;                  // 控制点点号对应矩阵主元加平均权的100倍
 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-r)-(a.n-known)));     // 单位权中误差计算
                                             // （a.m-r）为观测数-固定数据个数
                                          // （a.n-known）为必要观测数
 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);
  cout<<endl<<endl;

  cout<<endl<<endl<<"  未知数解："<<endl;
  matdis(aa.X,aa.n);
 
  cout<<endl<<endl<<" 未知数协因数阵："<<endl;
  matdis(aa.QXX,aa.n,aa.n);

  cout<<endl<<endl<<" 改正数："<<endl;
  matdis(aa.V,aa.m);

  cout<<endl<<endl<<" 单位权中误差：+-"<<aa.m0<<endl;

}
//************************************************************************************************
int foutadj(adj &aa, char *name)
{
ofstream on(name);
 if(!on) return 0;
// 输出平差项目名   
 on<<aa.name<<endl<<endl;
 
// output observation number: 
 on<<aa.m<<endl;
 
// output unknown data number:
    on<<aa.n<<endl<<endl;

//out unknown data results
  for(int i=0;i<aa.n;i++)
   on<<aa.X[i][0]<<endl;
      on<<endl;
// 未知数协因数阵："<<endl;
   for(i=0;i<aa.n;i++)
   {
    for(int j=0;j<aa.n;j++)
   on<<aa.QXX[i][j]<<"  ";
    on<<endl;
   }
 
   on<<endl;
//output 改正数：;
  for(i=0;i<aa.m;i++)
   on<<aa.V[i][0]<<endl;
      on<<endl;

  on<<aa.m0<<endl;
  on.close();
return 1;

}
//************************************************************************************************
int rubust(adj &a)  // 定义抗差估计
{
// 1.最小二成平差
  if(!doadj(a)) return 0;
double *xold,*xnew,small(0),P[MAX],n0(0);
xold=new double[a.n];
 int num(0);
 xnew=new double[a.n];
for(int i=0;i<a.m;i++)
  P[i]=a.P[i][i];
 do{
  small=0;
 for(i=0;i<a.n;i++) *(xold+i)=a.X[i][0];
// 2.等权处理
  for(i=0;i<a.m;i++)
  {
   if(fabs(a.V[i][0])>2.5*a.m0) { a.P[i][i]=0;n0++;}
  else if(fabs(a.V[i][0])<1.8*a.m0*sqrt(1/a.P[i][i])) ;
   else a.P[i][i]=a.P[i][i]/1.8/a.m0;
  }
  cout<<"a.m0="<<a.m0<<endl;
  matdis(a.P,a.m,a.m);
  doadj(a);
  for(i=0;i<a.n;i++)
  { *(xnew+i)=a.X[i][0]-*(xold+i);
    if(small<fabs(*(xnew+i))) small=fabs(*(xnew+i));
  }
  num++;
 }while(small>0.0000000001);
if(n0) a.m0*=sqrt((a.m-a.n)/(a.m-a.n-n0));
delete [] xold;
delete [] xnew;
return 1;
}