摄影测量最终程序.txt

通过星历文件计算卫星的坐标

#include<iostream>
#include<iomanip> 
#include<fstream>
#include<cmath>
using namespace std;
int main()
{


	ofstream table("E:\\121011.txt",ios_base::app);

int i,j,r,rr=0;
long double xy[4][2]={{-86.15,-68.99},
                 {-53.40,82.21},
                 {-14.78,-76.63},
                 {10.46,64.43}};
long double XY[4][3]={{36589.41,25273.32,2195.17},
                 {37631.08,31324.51,728.69},
                 {39100.97,24934.98,2386.50},
                 {40426.54,30319.81,757.31}};
const int a=8,b=6;
long double f=153.24,m=10000,a1,b1,c1,a2,b2,c2,a3,b3,c3,
            XAX,YAY,ZAZ,x,y,L[8][1]={0.0};
long double p=0,w=0,k=0;//定义角度元素
long double c[b][b]={0.0},A[a][b]={0.0},AA[b][b]={0.0};//d[b][1]为改正数
long double Xs,Ys,Zs,ip=206265;
       Xs=(XY[0][0]+XY[1][0]+XY[2][0]+XY[3][0])/4.0;
       Ys=(XY[0][1]+XY[1][1]+XY[2][1]+XY[3][1])/4.0;
	   Zs=m*f/1000+(XY[0][2]+XY[1][2]+XY[2][2]+XY[3][2])/4.0;
do
{
    
	long double AA[b][b]={0.0};

    
        table<<"cout six number:"<<endl;
	table<<"Xs  "<<setprecision(8)<<setw(14)<<Xs<<endl;
	table<<"Ys   "<<setprecision(8)<<setw(14)<<Ys<<endl;
	table<<"Zs   "<<setprecision(8)<<setw(14)<<Zs<<endl;
	table<<"p   "<<setprecision(8)<<setw(14)<<p<<endl;
	table<<"w   "<<setprecision(8)<<setw(14)<<w<<endl;
	table<<"k   "<<setprecision(8)<<setw(14)<<k<<endl;
	table<<"\n\n\n\n\n";

    a1=cos(p)*cos(k)-sin(p)*sin(w)*sin(k);
    b1=cos(w)*sin(k);
    c1=sin(p)*cos(k)+cos(p)*sin(w)*sin(k);
    a2=-cos(p)*sin(k)-sin(p)*sin(w)*cos(k);
    b2=cos(w)*cos(k);
    c2=-sin(p)*sin(k)+cos(p)*sin(w)*cos(k);
    a3=-sin(p)*cos(w);
    b3=-sin(w);
    c3=cos(p)*cos(w);

    table<<"输出九个系数:\n";
	table<<"a1   "<<setprecision(8)<<setw(14)<<a1<<endl;
	table<<"a2   "<<setprecision(8)<<setw(14)<<a2<<endl;
	table<<"a3   "<<setprecision(8)<<setw(14)<<a3<<endl;
	table<<"b1   "<<setprecision(8)<<setw(14)<<b1<<endl;
	table<<"b2   "<<setprecision(8)<<setw(14)<<b2<<endl;
	table<<"b3   "<<setprecision(8)<<setw(14)<<b3<<endl;
	table<<"c1   "<<setprecision(8)<<setw(14)<<c1<<endl;
	table<<"c2   "<<setprecision(8)<<setw(14)<<c2<<endl;
	table<<"c3   "<<setprecision(8)<<setw(14)<<c3<<endl;
	table<<"\n\n\n\n\n\n\n";




   for(i=0,j=0;(i<8&&j<4);i+=2,j++)
   {

       XAX=a1*(XY[j][0]-Xs)+b1*(XY[j][1]-Ys)+c1*(XY[j][2]-Zs);
       YAY=a2*(XY[j][0]-Xs)+b2*(XY[j][1]-Ys)+c2*(XY[j][2]-Zs);   
       ZAZ=a3*(XY[j][0]-Xs)+b3*(XY[j][1]-Ys)+c3*(XY[j][2]-Zs);

	   table<<"XAX: "<<setprecision(8)<<setw(14)<<XAX<<endl;
	   table<<"YAY: "<<setprecision(8)<<setw(14)<<YAY<<endl;
	   table<<"ZAZ: "<<setprecision(8)<<setw(14)<<ZAZ<<endl;

       x=-f*XAX/ZAZ;//x的新值
       y=-f*YAY/ZAZ;//y的新值


	  //table<<"x: "<<setprecision(8)<<setw(14)<<x<<endl;
	  //table<<"y: "<<setprecision(8)<<setw(14)<<y<<endl;


       L[i][0]=xy[j][0]-x;
       L[i+1][0]=xy[j][1]-y;
	    table<<"L[i][0]: "<<setprecision(8)<<setw(14)<<L[i][0]<<endl;
	   table<<"L[i+1][0]: "<<setprecision(8)<<setw(14)<<L[i+1][0]<<endl;

       A[i][0]=(a1*f+a3*xy[j][0])/ZAZ;
       A[i][1]=(b1*f+b3*xy[j][0])/ZAZ;
       A[i][2]=(c1*f+c3*xy[j][0])/ZAZ;

       A[i][3]=xy[j][1]*sin(w)-(xy[j][0]*(xy[j][0]*cos(k)-xy[j][1]*sin(k))/f+f*cos(k))*cos(w);
       A[i][4]=-f*sin(k)-xy[j][0]*(xy[j][0]*sin(k)+xy[j][1]*cos(k))/f;
       A[i][5]=xy[j][1];

       A[i+1][0]=(a2*f+a3*xy[j][1])/ZAZ;
       A[i+1][1]=(b2*f+b3*xy[j][1])/ZAZ;
       A[i+1][2]=(c2*f+c3*xy[j][1])/ZAZ;

       A[i+1][3]=-xy[j][0]*sin(w)-(xy[j][1]*(xy[j][0]*cos(k)-xy[j][1]*sin(k))/f-f*sin(k))*cos(w);
       A[i+1][4]=-f*cos(k)-xy[j][1]*(xy[j][0]*sin(k)+xy[j][1]*cos(k))/f;
       A[i+1][5]=-xy[j][0];
     }
table<<"\nA[i][j]:\n";
for(i=0;i<a;i++)
   for(j=0;j<b;j++) 
   {
	    table<<setprecision(8)<<setw(18)<<A[i][j];
		if(j==b-1)  table<<endl;
   }
    table<<"\n\n\n";

for(i=0;i<b;i++)
   for(j=0;j<b;j++)
        for(r=0;r<a;r++)
	    AA[i][j]+=A[r][i]*A[r][j];//AA[i][j]=A^TA
//for(i=0;i<b;i++)
 //  for(j=0;j<b;j++)
//AA[i][j]=xs*AA[i][j];

table<<"\nAA[i][j]:\n";	 
for(i=0;i<b;i++)
 for(j=0;j<b;j++) 
 {
	     table<<setprecision(8)<<setw(18)<<AA[i][j];
		 if(j==b-1)  table<<endl;
  }
  table<<"\n\n\n\n";


//A^TA结束
//第一次矩阵相乘结束,下面对AA求逆	
//矩阵求逆程序片断开始 
    int h,g;
    long double pp; 
    long double q[b][2*b]={0.0};
    for(i=0;i<b;i++)//构造高斯矩阵 
        for(j=0;j<b;j++)
		{
            q[i][j]=AA[i][j];
		}
table<<"\n1:q[i][j]:\n";
for(i=0;i<b;i++)
 for(j=0;j<2*b;j++) 
 {
	   table<<"   "<<setprecision(8)<<setw(18)<<q[i][j];
		if(j==2*b-1)  table<<endl;
 }
 table<<"\n\n\n\n\n";


    for(i=0;i<b;i++) 
        for(j=b;j<2*b;j++) 
		{
		    if(i+b==j)  q[i][j]=1; 
                       else     q[i][j]=0;
	   }
table<<"\n2:q[i][j]:\n";
for(i=0;i<b;i++)
   for(j=0;j<2*b;j++) 
   {
	    table<<"   "<<setprecision(8)<<setw(18)<<q[i][j];
		if(j==2*b-1)  table<<endl;
  }
 cout<<"\n\n\n\n\n\n";

		 	 
    for(h=g=0;h<b-1;h++,g++)//消去对角线以下的数据 
        for(i=h+1;i<b;i++) 
		{
		    for(j=i;j<b;j++)
			{
	        	    if((q[h][h]==0)&&(q[j][h]!=0))
			    {
			         for(r=h;r<2*b;r++)
					 {
				      long double RR;
			              RR=q[h][r];
				      q[h][r]=q[j][r];
				      q[j][r]=RR;
						  					 
					 }				  
			     break;    
			    }
		   }
			 
	    if(q[i][g]==0) 
            continue; 
            pp=q[h][g]/q[i][g]; 
            for(j=g;j<2*b;j++) 
			{
		        q[i][j]*=pp; 
                 q[i][j]-=q[h][j]; 
			} 
		} 

table<<"\n3:q[i][j]:\n";
for(i=0;i<b;i++)
    for(j=0;j<2*b;j++) 
	{
 	   table<<"   "<<setprecision(8)<<setw(18)<<q[i][j];
 		if(j==2*b-1) table<<endl;
    }
   table<<"\n\n\n\n\n\n";



    for(h=g=b-1;g>0;g--,h--) // 消去对角线以上的数据 
        for(i=g-1;i>=0;i--) 
		{
	        if(q[i][h]==0) 
            continue; 
            pp=q[h][g]/q[i][g]; 
            for(j=0;j<2*b;j++) 
			{				  
		        q[i][j]*=pp; 
                q[i][j]-=q[g][j];			 
			}
		} 

table<<"\n3---4:q[i][j]:\n";
for(i=0;i<b;i++)
   for(j=0;j<2*b;j++) 
   {
	    table<<setprecision(8)<<setw(18)<<q[i][j];
		if(j==2*b-1)  table<<endl;
   }
   table<<"\n\n\n\n\n\n";

     for(i=0;i<b;i++)  //将对角线上数据化为1 
	 { 
	     pp=1.0/q[i][i]; 
         for(j=0;j<2*b;j++) 
             q[i][j]*=pp;
	 } 
table<<"\n4:q[i][j]:\n";
for(i=0;i<b;i++)
   for(j=0;j<2*b;j++) 
   {
	    table<<setprecision(8)<<setw(18)<<q[i][j];
		if(j==(2*b-1))  table<<endl;
  }
   table<<"\n\n\n\n\n\n";

	  
     for(i=0;i<b;i++) //提取逆矩阵并除xs 
          for(j=0;j<b;j++) 
              c[i][j]=q[i][j+b]; 
     
//矩阵求逆程序片断结束
//(A^TA)-1结束,此时C=(A^A)-1
table<<"\nc[i][j]:\n\n";
for(i=0;i<b;i++)
   for(j=0;j<b;j++) 
   {
	   table<<setprecision(8)<<setw(18)<<c[i][j];
		if(j==(b-1))  table<<endl;
   }
   table<<"\n\n\n\n\n\n";

//第二次矩阵相乘开始

//CA^T开始
	 long double x[b][a]={0.0};
	 for(i=0;i<b;i++)
	     for(j=0;j<a;j++)
		  for(r=0;r<b;r++)
		       x[i][j]+=c[i][r]*A[j][r];//A^T[i][j]=A[j][i]

table<<"\nx[i][j]:\n\n";
for(i=0;i<b;i++)
   for(j=0;j<a;j++) 
   {
	   table<<setprecision(8)<<setw(18)<<x[i][j];
		if(j==(a-1))  table<<endl;
   }
   table<<"\n\n\n\n\n\n";
			 		 
	  
//CA^T结束

//(CA^T)L开始
   long double d[b][1]={0};
	for(i=0;i<b;i++)
	     for(j=0;j<1;j++)
		  for(r=0;r<a;r++)
			d[i][j]+=x[i][r]*L[r][j];
	 

table<<"\nd[i][j]:\n\n";
for(i=0;i<b;i++)
   for(j=0;j<1;j++) 
   {
	   table<<setprecision(8)<<setw(18)<<d[i][j]<<endl;
	 
   }
   table<<"\n\n\n\n\n\n";
			 	 
//(CA^T)L结束

//第二次矩阵相乘结束

    
    Xs+=d[0][0];
    Ys+=d[1][0];
    Zs+=d[2][0];

    p+=d[3][0];
    w+=d[4][0];
    k+=d[5][0]; 

    rr++;//循环次数增1
	cout<<"rr="<<rr<<endl;
	if((abs(d[3][0]*ip)<6)&&(abs(d[4][0]*ip)<6)&&(abs(d[5][0]*ip)<6))
	{
		table<<"外方位元素的最终结果为：\n"<<endl;
        table<<"Xs  "<<setprecision(8)<<setw(14)<<Xs<<endl;
	    table<<"Ys   "<<setprecision(8)<<setw(14)<<Ys<<endl;
	    table<<"Zs   "<<setprecision(8)<<setw(14)<<Zs<<endl;
	    table<<"p   "<<setprecision(8)<<setw(14)<<p<<endl;
	    table<<"w   "<<setprecision(8)<<setw(14)<<w<<endl;
	    table<<"k   "<<setprecision(8)<<setw(14)<<k<<endl;
	    table<<"\n";
     	break;
	}

}while(rr<20);
}