📄 gps网平差.cpp
字号:
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;
}
//************************************************************************************************
void matout(double A[][1],int n,ofstream out) // 向文件输出矩阵
{
for(int i=0;i<n;i++)
out<<" "<<A[i][0]<<endl;
}
//************************************************************************************************
void matout(double A[][MAX],int n,int m,ofstream out) // 向文件输出矩阵
{//1.set B[][] I;
for(int i=0;i<n;i++)
{ out<<" ";
for(int j=0;j<m;j++)
out<<A[i][j]<<" ";
out<<endl;
}
}
//************************************************************************************************
struct Gpspnt{
char name[20];
double x;
double y;
double z;
double x0;
double y0;
double z0;
double mx;
double my;
double mz;
int fixed;//点性标志//
int i;
};
struct xiangliang{
Gpspnt *startp;
Gpspnt *endp;
double deltx;
double delty;
double deltz;
double Dx[3][3];
};
struct Gpsnet{
char netname[40];
int obnum;
int allpnum;
int fixpnum;
double m0;
Gpspnt Pt[MAX];
xiangliang L[MAX];
adj aa;
};
//************************************************************************************************
int finGpsnet(Gpsnet &a,char *fname) // 文件输入Gps网函数
{
ifstream in(fname,ios::nocreate); // 建立文件流,并与输入文件名建立关联
if(!in) {cout<<fname<<" error: file does not exist! "<<endl; return 0;}
// 文件现实性判断
char name[20];
in>>a.netname;
in>>a.obnum;
in>>a.allpnum;
in>>a.fixpnum;
in>>a.m0;
int n(a.fixpnum); // n为已输入名字的点的个数
// 输入控制点信息
for(int i=0;i<n;i++)
{
in>>a.Pt[i].name>>a.Pt[i].x>>a.Pt[i].y>>a.Pt[i].z;
a.Pt[i].fixed=1; // 控制点标记
a.Pt[i].x0=a.Pt[i].x;
a.Pt[i].y0=a.Pt[i].y;
a.Pt[i].z0=a.Pt[i].z;
a.Pt[i].i=i; // 控制点编号，从0到a.fixpnum-1
}
// 输入未知点相关信息（名字在后面输入）
for(i=a.fixpnum;i<a.allpnum;i++)
{
a.Pt[i].fixed=0; // 未知点标记
a.Pt[i].x=0;a.Pt[i].y=0;a.Pt[i].z=0;
a.Pt[i].x0=-PI;a.Pt[i].y0=-PI;a.Pt[i].z0=-PI;
a.Pt[i].mx=0;a.Pt[i].my=0;a.Pt[i].mz=0;
*(a.Pt[i].name)=0;
a.Pt[i].i=i; // 为未知点编号,从a.fixpnum到a.allpnum-1
}
// 输入观测值
for(i=0;i<a.obnum;i++)
{int t=0; // 点名比较标志
in>>name; // 输入起点名
for(int k=0;k<n;k++)
if(strnicmp(name,a.Pt[k].name,20)==0)
{
a.L[i].startp=&(a.Pt[k]); // 找到同名点，起点指针指向该点
t++; // 找到标志
}
if(t==0) {strcpy(a.Pt[n].name,name);
a.L[i].startp=&(a.Pt[n]); // 找不到同名点，该名输给新点
n++;}
in>>name; t=0; // 输入终点名，操作过程同上
for(k=0;k<n;k++)
if(strnicmp(name,a.Pt[k].name,20)==0)
{
a.L[i].endp=&(a.Pt[k]);
t++;}
if(t==0) {strcpy(a.Pt[n].name,name);
a.L[i].endp=&(a.Pt[n]);
n++;}
in>>a.L[i].deltx>>a.L[i].delty>>a.L[i].deltz; // 输入基线向量
for(int b=0;b<3;b++)
for(int c=0;c<3;c++)
in>>a.L[i].Dx[b][c]; // 输入向量协方差阵
}
if(n!=a.allpnum) {cout<<fname<<" error: file provide not correct point number ! "<<endl;
return 0;}
// 文件正确性判断
in.close(); // 关闭输入流及关联文件
// 向屏幕输出原始平差文件
cout<<" 平差文件 "<<fname<<" 数据输入结果："<<endl;
cout<<" "<<a.netname<<" "<<a.obnum<<" "<<a.allpnum<<" "
<<a.fixpnum<<" "<<a.m0<<endl<<endl;
for(i=0;i<a.fixpnum;i++) // 控制点数据
cout<<" "<<a.Pt[i].name<<" "<<a.Pt[i].x<<" "<<a.Pt[i].y<<" "<<a.Pt[i].z<<endl;
cout<<endl;
for(i=0;i<a.obnum;i++) // 基线向量数据
{cout<<" "<<a.L[i].startp->name<<" "<<a.L[i].endp->name<<" "<<a.L[i].deltx<<" "<<a.L[i].delty<<" "<<a.L[i].deltz<<endl;
for(int b=0;b<3;b++)
for(int c=0;c<3;c++)
cout<<a.L[i].Dx[b][c];
cout<<endl;
// 近似坐标计算
do{n=0; // 近似坐标标志，=1表示仍有点近似坐标未知
for(i=0;i<a.obnum; i++)
if( a.L[i].startp->x0==-PI || a.L[i].endp->x0==-PI) { n=1; break;}
for(i=0;i<a.obnum; i++) // 由基线向量计算近似坐标
{
if( a.L[i].startp->x0!=-PI && a.L[i].endp->x0==-PI)
{a.L[i].endp->x0=a.L[i].startp->x0+a.L[i].deltx;
a.L[i].endp->y0=a.L[i].startp->y0+a.L[i].delty;
a.L[i].endp->z0=a.L[i].startp->z0+a.L[i].deltz;
}
if(a.L[i].startp->x0==-PI && a.L[i].endp->x0!=-PI)
{a.L[i].startp->x0=a.L[i].endp->x0-a.L[i].deltx;
a.L[i].startp->y0=a.L[i].endp->y0-a.L[i].delty;
a.L[i].startp->z0=a.L[i].endp->z0-a.L[i].deltz;
}
}
}while(n==1);
//*********************** 平差数据准备 ***************************
a.aa.m=3*(a.obnum); // 观测值个数
a.aa.n=3*(a.allpnum); // 未知点个数（极大权处理，含控制点）
// 误差方程系数阵计算
for(i=0;i<a.aa.m;i++) // 根据基线向量及起、终点号确定
for(int j=0;j<a.aa.n;j++)
a.aa.A[i][j]=0;
for(i=0;i<a.obnum;i++)
{ a.aa.A[3*i][3*a.L[i].endp->i]=1;
a.aa.A[3*i+1][3*a.L[i].endp->i+1]=1;
a.aa.A[3*i+2][3*a.L[i].endp->i+2]=1;
a.aa.A[3*i][3*a.L[i].startp->i]=-1;
a.aa.A[3*i+1][3*a.L[i].startp->i+1]=-1;
a.aa.A[3*i+2][3*a.L[i].startp->i+2]=-1;
}
cout<<" "<<" 误差方程系数阵计算结果: "<<endl;
matdis(a.aa.A,a.aa.m,a.aa.n);
cout<<endl;
//观测值权阵计算
double D[MAX][MAX];int k;
for(i=0;i<a.obnum;i++)
{for(int j=3*i;j<3*i+3;j++)
for( k=3*i;k<3*i+3;k++)
D[j][k]=a.L[i].Dx[j-3*i][k-3*i];
D[j][k]= D[j][k]/a.m0*a.m0;}
inverse(D,a.aa.P,a.aa.m);
cout<<endl<<endl<<" 平差数据准备，请稍等...... "<<endl<<endl;
cout<<" 观测值权阵计算结果 :"<<endl;matdis(a.aa.P,a.aa.m,a.aa.m);
cout<<endl;
// 误差方程常数项计算
for(int i=0;i<a.obnum;i++)
{a.aa.l[3*i][0]=a.L[i].deltx-a.L[i].endp->x0+a.L[i].startp->x0;
a.aa.l[3*i+1][0]=a.L[i].delty-a.L[i].endp->y0+a.L[i].startp->y0;
a.aa.l[3*i+2][0]=a.L[i].deltz-a.L[i].endp->z0+a.L[i].startp->z0;
}
cout<<endl;
cout<<" "<<" 误差方程常数项计算结果 ："<<endl;
matdis(a.aa.l,a.aa.m);
cout<<endl;
// 平差数据保存在date.txt文件中
ofstream out("date.txt");
out<<a.netname<<endl;
out<<a.obnum<<endl;
out<<a.allpnum<<endl;
matout(a.aa.A,a.aa.m,a.aa.n,out);
out<<endl;
matout(a.aa.P,a.aa.m,a.aa.m,out);
out<<endl;
matout(a.aa.l,a.aa.m,out);
out.close();
return 1;
}
//************************************************************************************************
int Gpsadj(Gpsnet &a,char *outfile) // GPS网平差函数
{
fsetadj(a.aa,"date.txt");
cout<<endl<<" 正在平差计算，请稍等...... "<<endl<<endl;
doadj(a.aa,3*a.fixpnum,0); // 极大权法最小二乘平差：
for(int i=0;i<a.allpnum;i++) // 未知点坐标及精度计算
{
a.Pt[i].x=a.Pt[i].x0+a.aa.X[3*i][0];
a.Pt[i].y=a.Pt[i].y0+a.aa.X[3*i+1][0];
a.Pt[i].z=a.Pt[i].z0+a.aa.X[3*i+2][0];
a.Pt[i].mx=a.m0*sqrt(a.aa.QXX[3*i][3*i]);
a.Pt[i].my=a.m0*sqrt(a.aa.QXX[3*i+1][3*i+1]);
a.Pt[i].mz=a.m0*sqrt(a.aa.QXX[3*i+2][3*i+2]);
}
// 平差结果屏幕输出：
cout<<endl<<" "<<" 验后单位权中误差：+-"<<a.aa.m0<<endl<<endl<<" 未知数改正数dX: "<<endl;
matdis(a.aa.X,a.aa.n);
cout<<endl<<" "<<" 观测值改正数V:"<<endl;
matdis(a.aa.V,a.aa.m);
cout<<endl<<" "<<" 未知点坐标及精度 ："<<endl;
for(i=a.fixpnum;i<a.allpnum;i++)
cout<<" "<<a.Pt[i].name<<"坐标="<<a.Pt[i].x<<" +-"<<a.Pt[i].mx<<" "<<a.Pt[i].y<<" +-"<<a.Pt[i].my<<" "<<a.Pt[i].z<<" +-"<<a.Pt[i].mz<<endl;
cout<<endl;
// 平差结果文件保存:
ofstream out(outfile);
if(!out) cout<<"can not open save file!"<<endl;
out<<" GPS网"<<a.netname<<"平差计算 "<<endl<<endl;
out<<"1. 原始数据： "<<endl<<endl;
out<<" "<<a.netname<<" "<<a.obnum<<" "<<a.allpnum<<" "<<a.fixpnum<<" "<<a.m0<<endl;
for(i=0;i<a.fixpnum;i++)
out<<" "<<a.Pt[i].name<<" "<<a.Pt[i].x<<" "<<a.Pt[i].y<<" "<<a.Pt[i].z<<endl;
out<<endl;
for(i=0;i<a.obnum;i++)
out<<" "<<a.L[i].startp->name<<" "<<a.L[i].endp->name<<" "<<a.L[i].deltx
<<" "<<a.L[i].delty<<" "<<a.L[i].deltz<<endl;
for(int b=0;b<3;b++)
for(int c=0;c<3;c++)
out<<a.L[i].Dx[b][c];
out<<endl;
out<<"2. 平差数据："<<endl<<endl;
out<<" 2.1 误差方程系数阵： "<<endl;
matout(a.aa.A,a.aa.m,a.aa.n,out);
out<<endl<<endl;
out<<" 2.2 误差方程权阵： "<<endl;
matout(a.aa.P,a.aa.m,a.aa.m,out);
out<<endl<<endl;
out<<" 2.3 未知点近似坐标： "<<endl;
for(i=a.fixpnum;i<a.allpnum;i++)
{ out<<" ";
out<<a.Pt[i].name<<" "<<a.Pt[i].x0<<" "<<a.Pt[i].y0<<" "<<a.Pt[i].z0<<" ";
out<<endl;
}
out<<endl<<endl;
out<<" 2.4 误差方程常数项： "<<endl;
matout(a.aa.l,a.aa.m,out);
out<<endl;
out<<"3. 平差结果 "<<endl<<endl;
out<<" 3.1 观测值改正数V： "<<endl;
matout(a.aa.V,a.aa.m,out);
out<<endl<<endl;
out<<" 3.2 单位权中误差m0：+-"<<a.aa.m0<<endl;
out<<endl<<endl;
out<<" 3.3 未知数改正数dx："<<endl;
for(i=a.fixpnum;i<a.allpnum;i++)
{ out<<" ";
out<<a.aa.X[i][0]<<" ";
out<<endl;
}
out<<endl<<endl;
out<<" 3.4 未知点坐标及精度： "<<endl;
for(i=a.fixpnum;i<a.allpnum;i++)
{ out<<" ";
out<<" "<<a.Pt[i].name<<"坐标="<<a.Pt[i].x<<" +-"<<a.Pt[i].mx<<" "<<a.Pt[i].y<<" +-"<<a.Pt[i].my<<" "<<a.Pt[i].z<<" +-"<<a.Pt[i].mz<<endl;
out<<endl;
}
out<<endl<<endl;
out<<" 3.5 未知参数协方差阵： "<<endl;
for(i=a.fixpnum;i<a.allpnum;i++)
{ out<<" ";
for(int j=a.fixpnum;j<a.allpnum;j++)
out<<a.aa.m0*a.aa.m0*a.aa.QXX[i][j]<<" ";
out<<endl;
}
out.close();
return 1;
}
int Gdoadj(Gpsnet &a,char *infilename,char* outfilename)
{
if(finGpsnet(a,infilename)){
Gpsadj(a,outfilename);
return 1;
}
else return 0;
}
//****************************** main() ************************************************
void main()
{
cout.precision(16);
cout.width(12);
// XYnet aa;
//finXYnet(aa,"pingmian1.txt");
// XYnetdis(aa);
//setx0y0(aa);
//setXYadj(aa);
//doXYadj(aa);
Gpsnet a;
Gdoadj(a,"sd.txt","levdf.txt");
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -