?? gps網平差程序.cpp
字號:
// 未知數協因數陣："<<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;
in>>a.Pt[i].x;
in>>a.Pt[i].y;
in>>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
}
// 輸入觀測值
//cout<<a.obnum<<endl;
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;
cout<<"1.1 控制點數據"<<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;
cout<<"1.2 基線向量數據"<<endl<<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;
cout<<"基線向量協方差陣"<<endl;
for(int b=0,f=1;b<3;b++)
for(int c=0;c<3;c++)
{
cout<<a.L[i].Dx[b][c]<<" ";
if(f%3==0)
cout<<endl;
f++;
}
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.aa.m;i++)
for(int j=0;j<a.aa.m;j++)
D[i][j]=0;
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;
matdis(a.aa.P,a.aa.m,a.aa.m);
cout<<endl<<endl<<" 平差數據準備，請稍等...... "<<endl<<endl;
cout<<" 觀測值權陣計算結果 :"<<endl;matdis(a.aa.P,a.aa.m,a.aa.m);
cout<<endl;
// 誤差方程常數項計算
for( 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.aa.m<<endl;
out<<a.aa.n<<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.aa.m0*sqrt(a.aa.QXX[3*i][3*i]);
a.Pt[i].my=a.aa.m0*sqrt(a.aa.QXX[3*i+1][3*i+1]);
a.Pt[i].mz=a.aa.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;
out<<"1.1 控制點數據 "<<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;
out<<"1.2 基線向量觀測值 "<<endl<<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;
out<<"基線向量協方差陣"<<endl;
for(int b=0, f=1;b<3;b++)
for(int c=0;c<3;c++)
{out<<a.L[i].Dx[b][c]<<" ";
if(f%3==0)
out<<endl;
f++;
}
out<<endl;
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=0;i<a.aa.n;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.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,"GPS.txt","GPSres.txt");
}
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -