单向空间后方交会源程序

#include
//#include
#include
using namespace std;

double Average(double const *Data,int num)//求均值
{
double sum=0;
for (int i=0;i{
sum+=*(Data+i);
}
return sum/num;
}

void MatrixMultiply(double const *A,double const *B,double *Result,int a,int b,int c)//矩阵相乘
{
for (int i=0;ifor (int j=0;j{
*(Result+c*i+j)=0;
for (int k=0;k*(Result+c*i+j)+=(*(A+i*b+k))*(*(B+k*c+j));
}
}

int brinv(double a[], int n) //矩阵求逆，来自网络
{
int *is,*js,i,j,k,l,u,v;
double d,p;
is=(int*)malloc(n*sizeof(int));
js=(int*)malloc(n*sizeof(int));
for (k=0; k<=n-1; k++)
{
d=0.0;
for (i=k; i<=n-1; i++)
for (j=k; j<=n-1; j++)
{
l=i*n+j;
p=fabs(a[l]);
if (p>d)
{
d=p; is[k]=i; js[k]=j;}
}
if (d+1.0==1.0)
{
free(is);
free(js);
return(0);
}
if (is[k]!=k)
for (j=0; j<=n-1; j++)
{
u=k*n+j;
v=is[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if (js[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k;
v=i*n+js[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
l=k*n+k;
a[l]=1.0/a[l];
for (j=0; j<=n-1; j++)
if (j!=k)
{
u=k*n+j; a[u]=a[u]*a[l];}
for (i=0; i<=n-1; i++)
if (i!=k)
for (j=0; j<=n-1; j++)
if (j!=k)
{
u=i*n+j;
a[u]=a[u]-a[i*n+k]*a[k*n+j];
}
for (i=0; i<=n-1; i++)
if (i!=k)
{
u=i*n+k;
a[u]=-a[u]*a[l];
}
}
for (k=n-1; k>=0; k--)
{
if (js[k]!=k)
for (j=0; j<=n-1; j++)
{
u=k*n+j;
v=js[k]*n+j;
p=a[u];
a[u]=a[v];
a[v]=p;
}
if (is[k]!=k)
for (i=0; i<=n-1; i++)
{
u=i*n+k;
v=i*n+is[k];
p=a[u];
a[u]=a[v];
a[v]=p;
}
}
free(is);
free(js);
return(1);
}


void main()
{
const double x[4]={-0.08615,-0.0534,-0.01478,0.01046},//已知数据
y[4]={-0.06899,0.08221,-0.07663,0.06443},
X[4]={36589.41,37631.08,39100.97,40426.54},
Y[4]={25273.32,31324.51,24934.98,30319.81},
Z[4]={2195.17,728.69,2386.5,757.31},

f=0.15324;//x0=0,y0=0

double Zs=f*35000,//变量初始值
Xs=Average(X,4),
Ys=Average(Y,4),
Fai=0,
Omg=0,
Kab=0;

double R[3][3],x_tmp[4],y_tmp[4],Z_assistance[4],A[8][6],A_assistance[6][6],A_assistance2[6][8],X_correction[6],L[8];
X_correction[0]=200;
cout.precision(10);

cout<<"\n初始值:\nf ("<cout<<"迭代阈值条件:\nfabs(X_correction[0])<0.000001\nfabs(X_correction[1])<0.000001\nfabs(X_correction[2])<0.000001\nfabs(X_correction[3])<0.001\nfabs(X_correction[4])<0.001\nfabs(X_correction[5])<0.001\n"<cout<<"------------------------------

------------------------------"<
for (int T=1;
fabs(X_correction[0])>0.000001||
fabs(X_correction[1])>0.000001||
fabs(X_correction[2])>0.000001||
fabs(X_correction[3])>0.001||
fabs(X_correction[4])>0.001||
fabs(X_correction[5])>0.001
;T++)
{
R[0][0]=cos(Fai)*cos(Kab)-sin(Fai)*sin(Omg)*sin(Kab);//计算旋转矩阵
R[0][1]=-cos(Fai)*sin(Kab)-sin(Fai)*sin(Omg)*cos(Kab);
R[0][2]=-sin(Fai)*cos(Omg);
R[1][0]=sin(Kab)*cos(Omg);
R[1][1]=cos(Kab)*cos(Omg);
R[1][2]=-sin(Omg);
R[2][0]=sin(Fai)*cos(Kab)+cos(Fai)*sin(Omg)*sin(Kab);
R[2][1]=-sin(Fai)*sin(Kab)+cos(Fai)*sin(Omg)*cos(Kab);
R[2][2]=cos(Fai)*cos(Omg);


for (int i=0;i<4;i++)
{
Z_assistance[i]=(R[0][2]*(X[i]-Xs)+R[1][2]*(Y[i]-Ys)+R[2][2]*(Z[i]-Zs));
x_tmp[i]=-f/Z_assistance[i]*(R[0][0]*(X[i]-Xs)+R[1][0]*(Y[i]-Ys)+R[2][0]*(Z[i]-Zs));//计算像点坐标近似值
y_tmp[i]=-f/Z_assistance[i]*(R[0][1]*(X[i]-Xs)+R[1][1]*(Y[i]-Ys)+R[2][1]*(Z[i]-Zs));
}

for (i=0;i<4;i++)
{
A[i*2][0]=(R[0][0]*f+R[0][2]*(x[i]))/Z_assistance[i];//计算各点相应线性系数
A[i*2][1]=(R[1][0]*f+R[1][2]*(x[i]))/Z_assistance[i];
A[i*2][2]=(R[2][0]*f+R[2][2]*(x[i]))/Z_assistance[i];
A[i*2][3]=y[i]*sin(Omg)-cos(Omg)*(x[i]/f*(x[i]*cos(Kab)-y[i]*sin(Kab))+f*cos(Kab));
A[i*2][4]=-f*sin(Kab)-x[i]/f*(x[i]*sin(Kab)+y[i]*cos(Kab));
A[i*2][5]=y[i];

A[i*2+1][0]=(R[0][1]*f+R[0][2]*(y[i]))/Z_assistance[i];
A[i*2+1][1]=(R[1][1]*f+R[1][2]*(y[i]))/Z_assistance[i];
A[i*2+1][2]=(R[2][1]*f+R[2][2]*(y[i]))/Z_assistance[i];
A[i*2+1][3]=-x[i]*sin(Omg)-cos(Omg)*(y[i]/f*(x[i]*cos(Kab)-y[i]*sin(Kab))-f*sin(Kab));
A[i*2+1][4]=-f*cos(Kab)-y[i]/f*(x[i]*sin(Kab)+y[i]*cos(Kab));
A[i*2+1][5]=-x[i];
}

double At[6][8];
for (i=0;i<8;i++)
for (int j=0;j<6;j++)
At[j][i]=A[i][j];//计算A的转置At

MatrixMultiply(&(At[0][0]),&(A[0][0]),&(A_assistance[0][0]),6,8,6);//计算辅助矩阵(At*A)

brinv((double *)A_assistance,6);//转换(At*A)为自身逆矩阵

MatrixMultiply(&(A_assistance[0][0]),&(At[0][0]),&(A_assistance2[0][0]),6,6,8);//计算第二辅助矩阵{[1/(A*At)]*At}

for (i=0;i<4;i++)
{
L[2*i]=x[i]-x_tmp[i];
L[2*i+1]=y[i]-y_tmp[i];//计算L阵
}

MatrixMultiply(&(A_assistance2[0][0]),&(L[0]),&(X_correction[0]),6,8,1);

Xs+=X_correction[0];
Ys+=X_correction[1];
Zs+=X_correction[2];
Fai+=X_correction[3];
Omg+=X_correction[4];
Kab+=X_correction[5];

cout<<"第"<cout<<"Xs: "<

[5]<<" + "<cout<<"------------------------------------------------------------"<}

double Ax[8],m0=0,m[6];

MatrixMultiply((double *)A,(double *)X_correction,(double *)Ax,8,6,1);
for (int i=0;i<8;i++)
m0+=(Ax[i]-L[i])*(Ax[i]-L[i]);
m0=sqrt(m0/2);//单位权中误差

for (i=0;i<6;i++)
m[i]=m0*sqrt(A_assistance[i][i]);//个未知数中误差

cout<cout<<"Xs ("<cout<<"R: "<cout<<" "<cout<<" "<
}