#include #include using namespace std; #define Max 50 //全局变量 int N=10; //n 个卫星 double xi[10]; //卫星X 坐标/cm double yi[10]; //卫星y 坐标/cm double ti[10][3]; //3 个用户接收时间/us //计算行列式 double det(double a[Max][Max],int n) { int i,j,start; double k,temp,result=1; for(start=0;start<=n-2;start++) { i=1; while(a[start][start]==0&&start+i<=n-1) //首项为零时需要进行行变换 { for(j=start;j<=n-1;j++) { temp=-1*a[start][j]; a[start][j]=a[start+i][j]; a[start+i][j]=temp; } i++; } if(start+i==n) continue; for(i=start+1;i<=n-1;i++) //首项不为零时化为对角形行列式 { k=a[i][start]/a[start][start]; for(j=start;j<=n-1;j++) a[i][j]=a[i][j]-k*a[start][j]; } } for(i=0;i<=n-1;i++) result*=a[i][i]; return result; } //计算线性方程：方程增广矩阵 a[][]，阶数 n，其解向量r[] int equalation(double a[Max][Max+1],double r[Max],int n) { double det(double a[Max][Max],int n); double det0[Max][Max],det1[Max][Max]; int i,j,k; for(i=0;i<=n-1;i++) for(j=0;j<=n-1;j++) det0[i][j]=a[i][j]; if(det(det0,n)==0) return 0; //返回0，该方程无解 for(k=0;k<=n-1;k++) { for(i=0;i<=n-1;i++) for(j=0;j<=n-1;j++) { det0[i][j]=a[i][j]; det1[i][j]=a[i][j]; } for(i=0;i<=n-1;i++) det1[i][k]=a[i][n]; r[k]=det(det1,n)/det(det0,n); } return 1; } //计算平方 double sqr(double x) { return x*x; } //辅助函数g double g(double x,double y,double c,double xi,double yi,double ti) { return sqr(xi-x)+sqr(yi-y)-sqr(c*ti); } //最小二乘法函数f 对X 的偏导fx double fx(double x,double y,double c,int k) //k 为用户编号(1,2,3) { int i; double sum=0; for (i=0;i<=N-1;i++) sum+=g(x,y,c,xi[i],yi[i],ti[i][k-1])*(xi[i]-x); return sum; } //最小二乘法函数f 对Y 的偏导fy double fy(double x,double y,double c,int k) { int i; double sum=0; for (i=0;i<=N-1;i++) sum+=g(x,y,c,xi[i],yi[i],ti[i][k-1])*(yi[i]-y); return sum; } //最小二乘法函数 f 对 c 的偏导 fc...
