附有限制条件的间接平差类(+抗差估计)

附有限制条件的间接平差类(+抗差估计）

间接平差模型，有详细的注释，需要的自取
头文件是上个博客写的头文件
这是个 FX_adj.h文件文件

#if !defined(CHDADJ_FX_ADJ_H____INCLUDED_)               // FX_adj.h文件
#define CHDADJ_FX_ADJ_H____INCLUDED_            //VS:Version Code:2019.0
//**********************************************************************
#include"MAT.h"                           // 需要应用矩阵类 (lines: 336)
//**********************************************************************
class FX_adj                     // 附有限制条件的间接平差类(+抗差估计)
{
  public://平差模型：            V=AX-l  P  
	     //                      CX=W
         //  平差模型矩阵与阶数：A[m][n], P[m][m], l[m][1],C[s][n],W[s][1]
	     //  注：当限制条件数s=0时，简化为间接平差（+抗差估计）  	
//输入数据和矩阵（当限制条件数s>0时，需要输入限制条件矩阵C及常数项向量W）
	char name[40];               // 平差项目名
	int m;                       // 观测值数
    int n;                       // 未知参数X的个数
	int s;                       // 未知参数间的限制条件数
	MAT A;                       // 误差方程系数阵，阶数：m×n       
	MAT P;                       // 观测值权阵，阶数：m×m 
	MAT l;                       // 误差方程常数项向量，阶数：m×1 
   	MAT C;                       // 限制条件系数阵，阶数：s×n
	MAT W;                       // 限制条件常数项向量，阶数：s×1
//计算结果数据和矩阵
	MAT X;                       // 未知参数向量，阶数：n×1
	MAT V;                       // 观测值改正数向量，阶数：m×1
    MAT QXX;                     // 未知参数协因数阵，阶数：n×n 
	MAT DXX;                     // 未知参数协方差阵，阶数：n×n
	double m0;                   // 平差单位权中误差
//公有成员函数
	FX_adj();                    // 构造函数（不确定m、n和s的值）
	FX_adj(int mm,int nn,int ss=0);//对m、n、s赋值，设定相关矩阵阶数
	bool fsetadj(char *fname) ;  // 文件输入计算数据
	void ksetadj();              // 键盘输入计算数据
 	void Set_MATs_hl(int hang,int lie,int rr=0);  
	                             // 设置平差矩阵阶数 	
	void Setrobust(bool True_or_false);
                                 // 抗差估计开关（True_or_false=ture时抗差）
	bool Getrobust();            // 返回抗差开关当前状态
  	bool doadj();                // 进行平差计算（rbust=true：抗差估计）
	static void Set_Km(double pre); 
	                             // 设置抗差估计迭代条件(m0变化率限制条件)
	static double Get_Km();   // 得到抗差估计迭代条件(m0变化率限制条件)
    void adjdis();               // 屏幕输出平差计算结果
    bool foutadj(char *fname);   // 文件输出平差计算结果
  private:
	int P0;                      // 抗差估计之后，权为0的观测值数
    int t;                       // 多余观测数
	bool flag;                   // flag=true平差成功；=false失败/未平差
 	bool rbust;                  // 抗差估计开关：=true抗差；=false不抗差
    static double m1_m0;         // 抗差迭代终止条件:单位权中误差变化率
//私有成员函数（定义该类的后台计算功能函数）  
	bool dadj();                 // 经典平差计算（s=0时为间接平差）
	bool robust();               // 抗差估计，（要应用 dadj()函数）
};
double  FX_adj::m1_m0=0.05;                  // 缺省的单位权中误差变化率 
//**********************************************************************
inline FX_adj:: FX_adj()                                     // 构造函数
{
	m=n=s=t=P0=0;                                          // 数据初始化
	flag=false;                                            // 未平差计算
	rbust=false;                                           // 不抗差
}
//**********************************************************************
 FX_adj::FX_adj(int obser_n,int unknown_n,int rr)
{// 构造有观测值、未知数(与条件数)个数的平差模型
    Set_MATs_hl(obser_n,unknown_n,rr);               // 设置计算矩阵阶数
	rbust=false;                                     // 不进行抗差估计
	flag=false;                                      // 未进行平差
}
//**********************************************************************
inline void FX_adj::Setrobust(bool True_or_false) // 设置抗差/不抗差估计 
{
    rbust=True_or_false;                       //=true抗差；=false非抗差
}
//**********************************************************************
inline bool  FX_adj::Getrobust()                 // 返回抗差开关当前状态
{
    return rbust;
}
//**********************************************************************
void  FX_adj::Set_MATs_hl(int Obser_n,int Unknown_n,int rr) 
{                                         //设置平差模型中主要矩阵的阶数 
	m=Obser_n;                                   //m为观测值个数
	n=Unknown_n;                                 //n为未知参数个数
	s=rr;                                        //s为未知数间限制条件数
	A.SetRR(m,n);                                //误差方程系数阵A
	P.SetRR(m,m);                                //观测值权阵P
 	QXX.SetRR(n,n);                              //未知数协因数QXX
	X.SetRR(n,1);                                //未知参数向量X
	l.SetRR(m,1);                                //误差方程常数项L 
    V.SetRR(m,1);                                //观测值改正数V
	DXX.SetRR(n,n);                              //未知数协方差阵DXX
    C.SetRR(s,n);                                //限制条件矩阵C
	W.SetRR(s,1);                                //限制条件常数项W
	flag=false;	                                 //未成功平差标记
}
//**********************************************************************
inline void  FX_adj::Set_Km(double pre) 
{
    m1_m0=pre;                //设置抗差估计迭代条件：单位权中误差变化率
}
//**********************************************************************
inline double  FX_adj::Get_Km() 
{
    return m1_m0;
}
//**********************************************************************
bool  FX_adj::dadj()                                 // 最小二乘平差计算
{
	flag=false;
//1 计算法方程系数阵
	MAT AT,APA;                          // 定义A的转置矩阵、法方程系数阵
	AT=A.T();
    APA=AT*P*A;                                      // 法方程系数阵计算
	if(!APA.Is_Valid()) return false;            // 若APA无效，退出本函数
//2 法方程系数阵赋值
	MAT Ny(n+s),Wy(n+s,1);           // 定义总的法方程系数阵和常数项向量
  //2.1 ATPA赋值
	for(int i=0;i<n;i++)
	  for(int j=0;j<n;j++)
		  Ny(i,j)=APA(i,j);                     // 构造Ny矩阵（ATPA部分）  
  //2.2 C赋值
     for(int i=n;i<n+s;i++)
	  for(int j=0;j<n;j++)
		  Ny(i,j)=C(i-n,j);                        // 构造Ny矩阵（C部分）
  //2.3 CT赋值
     for(int i=n;i<n+s;i++)
	  for(int j=0;j<n;j++)
		  Ny(j,i)=Ny(i,j);                        // 构造Ny矩阵（CT部分）
//3 法方程阵常数项赋值    
	 MAT APL=A.T()*P*l;                              // 计算法方程常数项
      for(int i=0;i<n;i++)
		 Wy(i,0)=APL(i,0);                      // 构造Wy向量（ATPL部分）                     
	  for(int i=n;i<n+s;i++)
		 Wy(i,0)=W(i-n,0);                         // 构造Wy向量（W部分）
//4 平差计算
     MAT Ny_1=Ny.inverse();                    // 计算法方程系数阵的逆阵
	 if(!Ny_1.Is_Valid()) return false;           //若逆阵计算失败，退出
	 MAT Y=Ny_1*Wy;                                         //解算法方程
   //从Y中提取X
	 for(int i=0;i<n;i++)
		X(i,0)=Y(i,0);                             // 从法方程解Y中提取X
   //提取未知数协因数阵
	for(int i=0;i<n;i++)
	  for(int j=0;j<n;j++)
		  QXX(i,j)=Ny_1(i,j);                   // 提取QXX中前n×n的方阵
//5 计算单位权中误差
	MAT AX=A*X;
	V=AX-l;                                          // 计算观测值改正数
	MAT VPV=V.T()*P*V;                            // 计算VTPV（阶数1×1）
	if(!VPV.Is_Valid()) return false;                 // 判断VPV的有效性
   double cc=VPV(0,0);                                  // 得到VTPV的值
    P0=0;                                          // 权为零的观测值个数
 	for(int i=0;i<m;i++)
 	   if(P(i,i)==1E-12) P0++;       // 统计(抗差估计后)权为零的观测值数
    t=m-P0-(n-s);                    // 0权不真为0，为0可能导致平差失败
	if(t<1||cc<0) return false;                            
	this->m0=sqrt(cc/t);                             // 计算单位权中误差
//6 计算未知数协方差矩阵
	DXX=m0*m0*QXX;                                     // 未知数协方差阵
	return flag=true;                                    // 平差计算成功
}
//**********************************************************************
bool  FX_adj::robust()                                   // 抗差估计计算
{
// 1.最小二乘平差
   if(!dadj()) return false;           // 首先进行一次间接平差
   double m1;                          // m1为上次平差计算的单位权中误差
   double m0_V;                        // 相邻两次平差的单位权中误差变化率  
   double *K=new double[m];            // 定义权因子数组
   MAT P_old = P;                      // 原始权阵
   do
   {   m1=m0;                          // 保存上次平差的单位权中误差
// 2.等价权处理
       MAT DVV=P.inverse()-A*QXX*A.T();         // 计算改正数的协因数阵   
	   DVV=m0*m0*DVV;                           // 计算改正数的协方差阵
	   for(int i=0;i<m;i++)                     // 对所有观测值进行计算
       {
	      double fV=fabs(V(i,0));               // 观测值改正数
		  double dV=sqrt(DVV(i,i));             // 计算观测值中误差
		  if(fV>2.5*dV) K[i]=0;                 // 粗差观测值的权设为0
		  else if(fV<=2.5*dV && fV>1.5*dV) 
			 K[i]=1.0/fV*1.5*dV;                // 修改部分观测值的权
		  else  K[i]=1.0;                       // 改正数小的权不变
	   } 
	   for(int i=0;i<m;i++)
	   {
		   for (int j = 0; j < m; j++)
		   {
			   P(i, j) *= sqrt(K[i] * K[j]);    // 计算对应权阵
			   if (P(i, i) / P_old(i, i) < 1E-2)// 若某个权降低了100倍以上
				   P(i, i) = P(i, j) = P(j, i) = 0;
		   }
	       if(P(i,i)==0) 
			   P(i,i)=1E-12;                    // 0权处理（0可能平差失败）
	   }
       if(!dadj())return false;                 // 再次进行最小二乘平差       
       m0_V=fabs(m1-m0)/m0;                     // 计算单位权中误差变化率
   }while(m0_V>m1_m0);                          // 不满足结束迭代条件
   delete []K;  K=NULL;                         // 释放权因子数组内存空间 
 //3 计算未知数协方差矩阵 
   DXX=m0*m0*QXX;                               // 更新未知数协方差阵
   return flag=true;
}
//**********************************************************************
 bool  FX_adj::doadj()                                       // 平差计算
 {
    if(rbust)
	{
	    cout<<"    ... 抗差估计计算:|m0-m1|/m1<="<<m1_m0<<" ..."<<endl;
		return robust();                         // 若抗差，进行抗差估计
	}
	cout<<"    ...... 最小二乘平差计算 ......"<<endl;
	return dadj();                               // 否则，做最小二乘平差
 }
 //**********************************************************************
void  FX_adj::ksetadj()                            //键盘输入平差数据函数
{
    cout<<"  本程序平差计算模型为：V=AX-L  P"<<endl;
    cout<<"  平差项目名:   ";
    cin>>name;
    cout<<endl;
    cout<<"  观测值个数:   ";
    cin>>this->m;
    cout<<endl;
    cout<<"  未知数个数:   ";
    cin>>n;
    cout<<"  限制条件数:   ";
    cin>>s;
    cout<<endl;
    Set_MATs_hl(m,n,s);
    cout<<"  误差方程系数阵A["<<m<<"]["<<n<<"]：  "<<endl;
 	cin>>A;
    cout<<"  观测值权阵 P["<<m<<"]["<<m<<"]："<<endl;
  	cin>>P;
	cout<<"  误差方程常数项L["<<m<<"]："<<endl;
   	cin>>l;
	if(s>0)
	{
	    cout<<"  限制条件矩阵 C["<<s<<"]["<<n<<"]："<<endl;
 	    cin>>C;	  
	    cout<<"  限制条件常数项 W["<<s<<"]["<<1<<"]："<<endl;
	    cin>>W;
	}
}
//**********************************************************************
bool  FX_adj::fsetadj(char *filename)            // 文件输入平差项目数据
{
//0 建立文件流，打开数据文件name
	ifstream in(filename,ios::_Nocreate);
	if(!in) return false;
//1 输入平差项目名    
	in>>name;
//2 输入观测值个数  
	in>>m;
//3 输入未知数个数 
    in>>n;
//4 输入限制条件个数 
	in>>s;
//5 设置相关矩阵行列数    
	Set_MATs_hl(m,n,s);
//6 输入误差方程系数阵A[m][n]
 	in>>A;
//7 输入误差方程权阵P[m][m]
 	in>>P;	
//8 输入误差方程常数项L[m][1]
    in>>l;
//9 输入限制条件矩阵C[s][n]
 	in>>C;	
//10 输入误差方程常数项L[m][1]
    in>>W; 
    in.close();   
    return true;
}
//**********************************************************************
 void  FX_adj::adjdis()                              // 平幕显示平差项目
{
    cout<<endl<<"1、平差计算输入数据："<<endl;
    cout<<"   1、1 平差项目名 ："<<this->name<<endl<<endl;
    cout<<"   1、2 误差方程系数矩阵A："<<endl;
    cout<<A;
    cout<<endl<<"   1、3 观测值权矩阵P："<<endl;
    cout<<P;
    cout<<endl<<"   1、4 常数项L："<<endl;
    cout<<l;
    if(s>0)
    {
       cout<<endl<<"   1、5 限制条件数："<<s<<endl;
       cout<<endl<<"   1、6 限制条件矩阵C："<<endl;
       cout<<C;
	   cout<<endl<<"   1、7 限制条件常数项向量W："<<endl;
       cout<<W;
    }
    cout<<endl<<"2、平差计算结果："<<endl;
    cout<<endl<<endl<<"   2、1 观测值改正数V："<<endl;
    cout<<V;
    cout<<endl<<"    2、2 未知数解X："<<endl;
    cout<<X;
    cout<<endl<<"   2、3 单位权中误差 m0：+-"<<m0<<endl;
    cout<<endl<<"   2、4 未知数协方差阵DXX："<<endl;
    cout<<DXX;
}
//**********************************************************************
bool  FX_adj::foutadj(char *name)                  // 向文件输出平差项目
{
	 ofstream on(name,ios::out);        // 建立文件流，打开/建立name文件        
	 if(!on) return false;
     on.precision(10);                                //保留10位有效数字
     on<<endl<<"1、平差计算输入数据："<<endl;
     on<<"   1、1 平差项目名 ："<<this->name<<endl<<endl;
     on<<"   1、2 误差方程系数矩阵A："<<endl;
     on<<A;
     on<<endl<<"   1、3 观测值权矩阵P："<<endl;
     on<<P;
     on<<endl<<"   1、4 常数项L："<<endl;
     on<<l;
     if(s>0)
     {
         on<<endl<<"   1、5 限制条件数："<<s<<endl;
         on<<endl<<"   1、6 限制条件矩阵C："<<endl;
         on<<C;
	     on<<endl<<"   1、7 限制条件常数项向量W："<<endl;
         on<<W;
     }
     on<<endl<<"2、平差计算结果："<<endl;
     on<<endl<<endl<<"   2、1 观测值改正数V："<<endl;
     on<<V;
     on<<endl<<"    2、2 未知数解X："<<endl;
     on<<X;
     on<<endl<<"   2、3 单位权中误差：+-"<<m0<<endl;
     on<<endl<<"   2、4 未知数协方差阵DXX："<<endl;
     on<<DXX;
     on.close();                         // 关闭输出文件
     return true;
}
//**********************************************************************
#endif