单位四元数空间后方交会 - 新人交流区

zhchao531

等　级：新手上路
帖　子：4
专家分：0
注　册：2007-11-3

楼主

问题点数：0 回复次数：3

单位四元数空间后方交会

*/ --------------------------------------------------------------------------------------
*/ 出自：编程中国 http://www.bc-cn.net
*/ 作者： zhchao531
*/ 时间： 2007-11-3 编程论坛首发
*/ 声明：尊重作者劳动，转载请保留本段文字
*/ --------------------------------------------------------------------------------------
#include <iostream.h>
#include <math.h>
#include <iomanip.h>
# define N 4
#define FF 10
double js(double s[8][8],int n)//求代数余子式
{
int z,j,k;
double b[8][8],r,total=0; //b[M][M]用于存放，在矩阵s[N][N]中元素s[0]的余子式
if(n>2)
{
for(z=0;z<n;z++)
{
for(j=0;j<n-1;j++)
for(k=0;k<n-1;k++)
if(k>=z) b[j][k]=s[j+1][k+1];
else b[j][k]=s[j+1][k];
if(z%2==0) r=s[0][z]*js(b,n-1); //递归调用
else r=(-1)*s[0][z]*js(b,n-1);
total=total+r;
}
}
else if(n==2) total=s[0][0]*s[1][1]-s[0][1]*s[1][0];
return total;
}
void juzhen_ni(double s[8][8],double b[8][8],int n)//矩阵求逆
{
int z,j,k,l,m,g;
double a[8][8],r,temp;
for(z=0;z<n;z++)
{
l=z;
for(j=0;j<n;j++)
{
m=j;
for (k=0;k<n-1;k++)
for(g=0;g<n-1;g++)
{
if(g>=m&&k<l) a[k][g]=s[k][g+1];
else if(k>=l&&g<m) a[k][g]=s[k+1][g];
else if(k>=l&&g>=m) a[k][g]=s[k+1][g+1];
else a[k][g]=s[k][g];
}
b[z][j]=js(a,n-1);
}
}
r=js(s,8);
for(z=0;z<8;z++) //求代数余子式，此时b[M][M]中存放的为原矩阵各元素对应的"代数余子式"
for(j=0;j<8;j++)
if((z+j)%2!=0 && b[z][j]!=0) b[z][j]=-b[z][j];
for(z=0;z<8;z++) //对b[M][M]转置,此时b[M][M]中存放的为原矩阵的伴随矩阵
for(j=z+1;j<8;j++)
{
temp=b[z][j];
b[z][j]=b[j][z];
b[j][z]=temp;
}

for(z=0;z<8;z++) //*求逆矩阵，此时b[M][M]中存放的是原矩阵的逆矩阵
for(j=0;j<8;j++)
b[z][j]=b[z][j]/r;
}

void main()
{
int i,j,k;//循环控制
double f,x0,y0,t,w0,k0,m0,M;//摄影比例尺，内方位元素，三旋转角
double Xs,Ys,Zs,qx,qy,qz,q,w;//线方位元素，四元素
int flag=0;//控制循环次数，防止死循环
x0=M=0.0;
y0=0.0;
f=153.84;
x0=x0/1000.0;
y0=y0/1000.0;
double pho_dot_x[N],pho_dot_y[N],Ear_dot_X[N],N_ni[8][8],Ear_dot_Y[N],X0[N],v0[8],Y0[N],Ear_dot_Z[N],Z0[N],l[8][1],C[8][7],a[3],b[3],c[3],fx0[N],fy0[N],m[7],v[8];
//此部分可以改为输入形式，为调试方便，就将值代入
pho_dot_x[0]=-90.7232;pho_dot_x[1]=-97.0095;pho_dot_x[2]=79.4501 ;pho_dot_x[3]=89.6083;
pho_dot_y[0]=-34.6413;pho_dot_y[1]=86.5755;pho_dot_y[2]=65.0315;pho_dot_y[3]=-49.0792;
Ear_dot_X[0]=5820.4176;Ear_dot_X[1]=5789.4975;Ear_dot_X[2]=6245.8016;Ear_dot_X[3]=6284.3955;
Ear_dot_Y[0]=5930.9250;Ear_dot_Y[1]=6239.7266 ;Ear_dot_Y[2]=6208.6790 ;Ear_dot_Y[3]=5915.0346;
Ear_dot_Z[0]=3.1145;Ear_dot_Z[1]=6.0341;Ear_dot_Z[2]=3.0491;Ear_dot_Z[3]=2.9837;
Xs=Ys=Zs=qx=qy=qz=0.0,q=1.0;
double x_0[7]={1.0,0.0,0.0,0.0,0.0,0.0,0.0};//改正数
double B[1][7]={0.0,0.0,0.0,1.0,0.0,0.0,0.0};//限制条件方程系数
while (fabs(x_0[0])>=0.000001||fabs(x_0[1])>=0.000001||fabs(x_0[2])>=0.000001
||fabs(x_0[3])>=0.000001||fabs(x_0[4])>=0.000001||
fabs(x_0[5])>=0.000001||fabs(x_0[6])>=0.000001)//控制条件，开始跌代
{
flag++;
B[0][3]=q,B[0][4]=qx,B[0][5]=qy,B[0][6]=qz;
a[0]=q*q+qx*qx-qy*qy-qz*qz;b[0]=2*(qx*qy-q*qz);c[0]=2*(qx*qz+q*qy);
a[1]=2*(qy*qx+q*qz);b[1]=q*q-qx*qx+qy*qy-qz*qz;c[1]=2*(qy*qz-q*qx);
a[2]=2*(qz*qx-q*qy);b[2]=2*(qz*qy+q*qx);c[2]=q*q-qx*qx-qy*qy+qz*qz;
for (i=0;i<N;i++)
{
X0[i]=a[0]*(Ear_dot_X[i]-Xs)+b[0]*(Ear_dot_Y[i]-Ys)+c[0]*(Ear_dot_Z[i]-Zs);
Y0[i]=a[1]*(Ear_dot_X[i]-Xs)+b[1]*(Ear_dot_Y[i]-Ys)+c[1]*(Ear_dot_Z[i]-Zs);
Z0[i]=a[2]*(Ear_dot_X[i]-Xs)+b[2]*(Ear_dot_Y[i]-Ys)+c[2]*(Ear_dot_Z[i]-Zs);
fx0[i]=-f*(X0[i]/Z0[i]);
fy0[i]=-f*(Y0[i]/Z0[i]);
}
for (i=0;i<N;i++)
{
//求间接平差系数
C[2*i][0]=(a[0]*f+a[2]*(pho_dot_x[i]-x0))/Z0[i];
C[2*i][1]=(b[0]*f+b[2]*(pho_dot_x[i]-x0))/Z0[i];
C[2*i][2]=(c[0]*f+c[2]*(pho_dot_x[i]-x0))/Z0[i];
C[2*i][3]=-f*((2*q*(Ear_dot_X[i]-Xs)+-2*qz*(Ear_dot_Y[i]-Ys)+2*qy*(Ear_dot_Z[i]-Zs))+(pho_dot_x[i]-x0)*(-2*qy*(Ear_dot_X[i]-Xs)+2*qx*(Ear_dot_Y[i]-Ys)+2*q*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i][4]=-f*((2*qx*(Ear_dot_X[i]-Xs)+2*qy*(Ear_dot_Y[i]-Ys)+2*qz*(Ear_dot_Z[i]-Zs))+(pho_dot_x[i]-x0)*(2*qz*(Ear_dot_X[i]-Xs)+2*q*(Ear_dot_Y[i]-Ys)+-2*qx*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i][5]=-f*((-2*qy*(Ear_dot_X[i]-Xs)+2*qx*(Ear_dot_Y[i]-Ys)+2*q*(Ear_dot_Z[i]-Zs))+(pho_dot_x[i]-x0)*(-2*q*(Ear_dot_X[i]-Xs)+2*qz*(Ear_dot_Y[i]-Ys)+-2*qy*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i][6]=-f*((-2*qz*(Ear_dot_X[i]-Xs)+-2*q*(Ear_dot_Y[i]-Ys)+2*qx*(Ear_dot_Z[i]-Zs))+(pho_dot_x[i]-x0)*(2*qx*(Ear_dot_X[i]-Xs)+2*qy*(Ear_dot_Y[i]-Ys)+2*qz*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i+1][0]=(a[1]*f+a[2]*(pho_dot_y[i]-y0))/Z0[i];
C[2*i+1][1]=(b[1]*f+b[2]*(pho_dot_y[i]-y0))/Z0[i];
C[2*i+1][2]=(c[1]*f+c[2]*(pho_dot_y[i]-y0))/Z0[i];
C[2*i+1][3]=-f*((2*qz*(Ear_dot_X[i]-Xs)+2*q*(Ear_dot_Y[i]-Ys)+-2*qx*(Ear_dot_Z[i]-Zs))+(pho_dot_y[i]-y0)*(-2*qy*(Ear_dot_X[i]-Xs)+2*qx*(Ear_dot_Y[i]-Ys)+2*q*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i+1][4]=-f*((2*qy*(Ear_dot_X[i]-Xs)+-2*qx*(Ear_dot_Y[i]-Ys)+-2*q*(Ear_dot_Z[i]-Zs))+(pho_dot_y[i]-y0)*(2*qz*(Ear_dot_X[i]-Xs)+2*q*(Ear_dot_Y[i]-Ys)+-2*qx*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i+1][5]=-f*((2*qx*(Ear_dot_X[i]-Xs)+2*qy*(Ear_dot_Y[i]-Ys)+2*qz*(Ear_dot_Z[i]-Zs))+(pho_dot_y[i]-y0)*(-2*q*(Ear_dot_X[i]-Xs)+2*qz*(Ear_dot_Y[i]-Ys)+-2*qy*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
C[2*i+1][6]=-f*((2*q*(Ear_dot_X[i]-Xs)+-2*qz*(Ear_dot_Y[i]-Ys)+2*qy*(Ear_dot_Z[i]-Zs))+(pho_dot_y[i]-y0)*(2*qx*(Ear_dot_X[i]-Xs)+2*qy*(Ear_dot_Y[i]-Ys)+2*qz*(Ear_dot_Z[i]-Zs))/f)/Z0[i];
l[2*i][0]=-pho_dot_x[i]+fx0[i];
l[2*i+1][0]=-pho_dot_y[i]+fy0[i];
}
w=(q*q+qx*qx+qy*qy+qz*qz-1)/2;
double CT[7][8],Nbb[7][7],W[7][1];
//C矩阵转置
for(i=0;i<7;i++)
for(j=0;j<8;j++)
{
CT[i][j]=C[j][i];
}
//B矩阵转置
double BT[7][1];
for(i=0;i<7;i++)
for(j=0;j<1;j++)
{
BT[i][j]=B[j][i];
}
//CT与C相乘
for(i=0;i<7;i++)
for(j=0;j<7;j++)
{ Nbb[i][j]=0;
for(k=0;k<8;k++)
Nbb[i][j]+=CT[i][k]*C[k][j];
}
//CT与l相乘
for(i=0;i<7;i++)
for(j=0;j<1;j++)
{ W[i][j]=0;
for(k=0;k<8;k++)
W[i][j]+=CT[i][k]*l[k][j];
}
//求矩阵N
double NN[8][8];
for (i=0;i<7;i++)
for (j=0;j<7;j++)
NN[i][j]=Nbb[i][j];
for (i=0;i<7;i++)
{
NN[i][7]=BT[i][0];
NN[7][i]=B[0][i];
}
NN[7][7]=0.0;
//N求逆
juzhen_ni(NN,N_ni,8);
double WY[8][1];
WY[7][0]=w;
for (i=0;i<7;i++)
WY[i][0]=W[i][0];
double Y[8][1];
for(i=0;i<8;i++)
for(j=0;j<1;j++)
{ Y[i][j]=0.0;
for(k=0;k<8;k++)
Y[i][j]+=N_ni[i][k]*WY[k][j];
}
//求改正数
for (i=0;i<7;i++)
x_0[i]=-Y[i][0];
Xs+=x_0[0];
Ys+=x_0[1];
Zs+=x_0[2];
q+=x_0[3];
qx+=x_0[4];
qy+=x_0[5];
qz+=x_0[6];
if(flag>FF)
break;
}
k0=atan(-(a[2]/c[2]));
w0=asin(-b[2]);
t=atan(b[0]/b[1]);
for(i=0;i<8;i++)
{ v0[i]=0;
for(k=0;k<7;k++)
v0[i]+=C[i][k]*x_0[k];
}
for (i=0;i<8;i++)
v[i]=v0[i]+l[i][0];
for (i=0;i<8;i++)
M+=v[i]*v[i];
cout<<"实验数据为:\n";
cout<<"10600983 -90.7232 -34.6413 5820.4176 5930.9250 3.1145\n";
cout<<"10600011 -97.0095 86.5755 5789.4975 6239.7266 6.0341\n";
cout<<"10800004 79.4501 65.0315 6245.8016 6208.6790 3.0491\n";
cout<<"10800993 89.6083 -49.0792 6284.3955 5915.0346 2.9837\n";
m0=sqrt(M/(8-6));//单位权中误差
for(i=0;i<7;i++)
m[i]=sqrt(N_ni[i][i])*m0;
a[0]=q*q+qx*qx-qy*qy-qz*qz;b[0]=2*(qx*qy-q*qz);c[0]=2*(qx*qz+q*qy);
a[1]=2*(qy*qx+q*qz);b[1]=q*q-qx*qx+qy*qy-qz*qz;c[1]=2*(qy*qz-q*qx);
a[2]=2*(qz*qx-q*qy);b[2]=2*(qz*qy+q*qx);c[2]=q*q-qx*qx-qy*qy+qz*qz;
cout<<"旋转矩阵为:\n";
cout<<a[0]<<"\t"<<a[1]<<"\t"<<a[2]<<endl;
cout<<b[0]<<"\t"<<b[1]<<"\t"<<b[2]<<endl;
cout<<c[0]<<"\t"<<c[1]<<"\t"<<c[2]<<endl;
cout<<"旋转角为:\n";
cout<<setprecision(12);
cout<<"k0="<<k0<<endl;
cout<<"w0="<<w0<<endl;
cout<<"t="<<t<<endl;
cout<<"Xs="<<Xs<<"\t 精度为"<<m[0]<<endl;
cout<<"Ys="<<Ys<<"\t 精度为"<<m[1]<<endl;
cout<<"Zs="<<Zs<<"\t 精度为"<<m[2]<<endl;
cout<<"qo="<<q<<"\t 精度为"<<m[3]<<endl;
cout<<"qx="<<qx<<"\t 精度为"<<m[4]<<endl;
cout<<"qy"<<qy<<"\t 精度为"<<m[5]<<endl;
cout<<"qz="<<qz<<"\t 精度为"<<m[6]<<endl;
cout<<"单位权中误差:\t"<<m0<<endl;
cout<<"q0*q0+qx*qx+qy*qy+qz*qz="<<(q*q+qx*qx+qy*qy+qz*qz)<<endl;
cout<<"迭代次数:"<<flag<<endl;
}