zhj.txt

雷达定位算法

#include <stdio.h>
#include <math.h>
void main()
{
	double a[30][4],b[3][4],s,s1,s2,s3,g[30],x,y,z,t,q1,q2,q3;
    int i,j,k,n,m=0;

	printf("please input data:\n");
	for(i=0;i<30;i++){
		for(j=0;j<4;j++)
			scanf("%lf",&a[i][j]);
		g[i]=a[i][3]*a[i][3]-(a[i][0]*a[i][0]+a[i][1]*a[i][1]+a[i][2]*a[i][2]);
		t=a[i][3];
		a[i][3]=g[i];
		g[i]=t;
	}
	q1=q2=q3=0;
	for(i=0;i<28;i++)
		for(j=i+1;j<29;j++)
			for(k=j+1;k<30;k++){
				for(n=0;n<2;n++){
					b[0][n]=2*a[j][n]-2*a[i][n];
					b[1][n]=2*a[k][n]-2*a[i][n];
				}
				b[0][3]=a[i][3]-a[j][3];
				b[1][3]=a[i][3]-a[k][3];
			    s=b[0][0]*b[1][1]-b[0][1]*b[1][0];
			    if(s==0)
				   break;
			    s1=b[0][3]*b[1][1]-b[1][3]*b[0][1];
				s2=b[0][0]*b[1][3]-b[0][3]*b[1][0];
                x=s1/s;
				y=s2/s;
		        z=sqrt(g[i]*g[i]-(a[i][0]-x)*(a[i][0]-x)-(a[i][1]-y)*(a[i][1]-y));
				if(z>=0)
	                m++;
				else break;
				if(m==1)
					m++;
                q1=(q1*(m-1)+x)/m;
				q2=(q2*(m-1)+y)/m;
				q3=(q3*(m-1)+z)/m;
			    printf("i=%d,j=%d,k=%d,x=%.3f,y=%.f,z=%.3f\n",i,j,k,x,y,z);
				
			}
			printf("q1=%.3f,q2=%.3f,q3=%.3f\n",q1,q2,q3);
}