附有限制条件的间接平差类(+抗差估计)
间接平差模型,有详细的注释,需要的自取
头文件是上个博客写的头文件
这是个 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