#include "stdio.h"
#include "math.h"
#include "Matrixmultiply.c"
#include "Matrixtranspose.c"
#include "Matrixinverse.c"
void main()
{
int i,j,k,f=0;
double x0=0.0, y0=0.0,fk=0.15324; //内方位元素
double m=39689; //估算比例尺
double R[3][3],XG[6][1],AT[6][8],ATA[6][6],ATL[6][1];
double AXG[8][1],V[8][1],VT[1][8],VTV[1][1],m0,D[6][6];
double Xs=0.0, Ys=0.0, Zs=0.0,Q=0.0,W=0.0,K=0.0;
double X,Y,Z,L[8][1],A[8][6];
double B[4][5]={-0.08615,-0.06899,36589.41,25273.32,2195.17,-0.05340,0.08221,37631.08,31324.51,728.69,-0.01478,-0.07663,39100.97,24934.98,2386.80,0.01046,0.06443,40426.54,30319.81,757.31};
for(i=0;i<4;i++)
{
Xs=Xs+B[i][2];
Ys=Ys+B[i][3];
Zs=Zs+B[i][4];
}
Xs=Xs/4; Ys=Ys/4; Zs=Zs/4+m*fk;
do//迭代计算
{
f++;
//组成旋转矩阵
R[0][0]=cos(Q)*cos(K)-sin(Q)*sin(W)*sin(K);
R[0][1]=-cos(Q)*sin(K)-sin(Q)*sin(W)*cos(K);
R[0][2]=-sin(Q)*cos(W);
R[1][0]=cos(W)*sin(K);
R[1][1]=cos(W)*cos(K);
R[1][2]=-sin(W);
R[2][0]=sin(Q)*cos(K)+cos(Q)*sin(W)*sin(K);
R[2][1]=-sin(Q)*sin(K)+cos(Q)*sin(W)*cos(K);
R[2][2]=cos(Q)*cos(W);
//计算系数阵和常数项
for( i=0, k=0,j=0;i<=3;i++,k++,j++)
{
X=R[0][0]*(B[i][2]-Xs)+R[1][0]*(B[i][3]-Ys)+R[2][0]*(B[i][4]-Zs);
Y=R[0][1]*(B[i][2]-Xs)+R[1][1]*(B[i][3]-Ys)+R[2][1]*(B[i][4]-Zs);
Z=R[0][2]*(B[i][2]-Xs)+R[1][2]*(B[i][3]-Ys)+R[2][2]*(B[i][4]-Zs);
L[j][0]=B[i][0]-(x0-fk*X/Z);
L[j+1][0]=B[i][1]-(y0-fk*Y/Z);
j++;
A[k][0]=(R[0][0]*fk+R[0][2]*(B[i][0]-x0))/Z;
A[k][1]=(R[1][0]*fk+R[1][2]*(B[i][0]-x0))/Z;
A[k][2]=(R[2][0]*fk+R[2][2]*(B[i][0]-x0))/Z;
A[k][3]=(B[i][1]-y0)*sin(W)-((B[i][0]-x0)*((B[i][0]-x0)*cos(K)-(B[i][1]-y0)*sin(K))/fk+fk*cos(K))*cos(W);
A[k][4]=-fk*sin(K)-(B[i][0]-x0)*((B[i][0]-x0)*sin(K)+(B[i][1]-y0)*cos(K))/fk;
A[k][5]=B[i][1]-y0;
A[k+1][0]=(R[0][1]*fk+R[0][2]*(B[i][1]-y0))/Z;
A[k+1][1]=(R[1][1]*fk+R[1][2]*(B[i][1]-y0))/Z;
A[k+1][2]=(R[2][1]*fk+R[2][2]*(B[i][1]-y0))/Z;
A[k+1][3]=-(B[i][0]-x0)*sin(W)-((B[i][1]-y0)*((B[i][0]-x0)*cos(K)-(B[i][1]-y0)*sin(K))/fk-fk*sin(K))*cos(W);
A[k+1][4]=-fk*cos(K)-(B[i][1]-y0)*((B[i][0]-x0)*sin(K)+(B[i][1]-y0)*cos(K))/fk;
A[k+1][5]=-(B[i][0]-x0);
k++;
}
Matrixtranspose(A,AT,8,6);
Matrixmultiply(AT,A,ATA,6,8,6);
Matrixinverse(ATA,6);
Matrixmultiply(AT,L,ATL,6,8,1);
Matrixmultiply(ATA,ATL,XG,6,6,1);
Xs=Xs+XG[0][0]; Ys=Ys+XG[1][0]; Zs=Zs+XG[2][0];
Q=Q+XG[3][0]; W=W+XG[4][0]; K=K+XG[5][0];
}while(XG[3][0]>=6.0/206265.0||XG[4][0]>=6.0/206265.0||XG[5][0]>=6.0/206265.0);
printf("迭代次数:%d",f);
Matrixmultiply(A,XG,AXG,8,6,1);
for( i=0;i<8;i++) //计算改正数
V[i][0]=AXG[i][0]-L[i][0];
Matrixtranspose(V,VT,8,1);
Matrixmultiply(VT,V,VTV,1,8,1);
m0=VTV[0][0]/2;
for(i=0;i<6;i++)
for(j=0;j<6;j++)
D[i][j]=m0*ATA[i][j];
//屏幕输出误差方程系数阵、常数项、改正数
printf("\n\n误差方程系数矩阵A为:\n\n");
for (i=0;i<6;i++)
{
for(j=0;j<6;j++)
printf("%13.5e ",A[i][j]);
printf("\n");
}
printf("\n常数项L为:\n\n");
for (i=0;i<8;i++)
{
for(j=0;j<1;j++)
printf("%13.5e ",L[i][j]);
printf("\n");
}
printf("\n改正数XG为:\n\n");
for (i=0;i<6;i++)
{
for(j=0;j<1;j++)
printf("%13.5e ",XG[i][j]);
printf("\n");
}
printf("\n相片的外方位元素为:\n\n");
printf(" Xs=%13.7e, Ys=%13.7e, Zs=%13.7e \n\n",Xs,Ys,Zs);
printf(" Q=%13.7e, W=%13.7e, K=%13.7e \n",Q,W,K);
printf("\n旋转矩阵R为:\n\n");
for (i=0;i<3;i++)
{
for(j=0;j<3;j++)
printf("%13.5e ",R[i][j]);
printf("\n");
}
printf("\n精度评定结果D为:\n\n");
for (i=0;i<6;i++)
{
for(j=0;j<6;j++)
printf("%13.5e ",D[i][j]);
printf("\n");
}
}