C语言实现的空间后方交会算法代码分享

#include <stdio.h>
#include <math.h>
const n=6;
void inverse (double c[n][n]);
template<typename T1,typename T2>void transpose (T1*mat1,T2*mat2,int a,int b);
template<typename T1,typename T2>void multi(T1*mat1,T2 * mat2,T2 * result,int a,int b,int c);
template<typename T>void input (T*mat,int a,int b);
template<typename T>void output(T*mat,char*s,int a,int b);

void main()
{ 
double R[3][3],XG[6][1],AT[6][8],ATA[6][6],ATL[6][1];

     double x0=0.0, y0=0.0; double fk=0.15324;   //内方位元素
double m=1000;  //估算比例尺
    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]={-86.15,-68.99,36589.41,25273.32,2195.17,-53.40,82.21,37631.08,31324.51,728.69,-14.78,-76.63,39100.97,24934.98,2386.50,10.46,64.43,40426.54,30319.81,757.31};

  for(int 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;
  int f=0 ;
do//迭代计算
{f++;

     R[0][0]=cos(Q)*cos(K)-sin(Q)*sin(W)*sin(K);