Chan氏TDOA

本内容是C语言实现Chan 氏算法的TDOA过程，Chan氏算法适合4个基站以上的情况，与之前上传的3个基站的情况不同

#include
#include
#include
#include“Matrix.h“
typedef struct coords{
	double x;
	double y;
}sta_coords;
sta_coords my_sta_coords;

void test_data（double **aint mint n）{
	int ij;
	for（i=0;i	{
		for（j=0;j			printf（“%6.3f “a[i][j]）;
		printf（“\n“）;
	}
	printf（“--------------------------\n“）;
}
/*********************************************************************
作用： 使用Chan氏算法在 4个基站及以上的情况下的算法实现 
参数： 
	M:       基站数目
	disT[i]：待测标签到第i+1个基站的距离与到第0个基站的距离差，即得到
			dis1_2+disT_1_2的值。作为已知传入 ==K[i+1]
	my_coords[] : 为各个基站的坐标值 
	sta_result  : 作为返回的目标坐标值 
返回值：
	若返回0，则失败（当中求逆失败）；返回1 ，表示成功 
*********************************************************************/

int TDOA（int mdouble disT[]sta_coords my_coords[]sta_coords *sta_result）
{
	int ij;
	double k[m-1]k1=0;
	double **Q**Ga**h;
	double **GT**hT;
	double **A1**A2**A3**A4**Za0Mx0My0; 
	double **B **B0**FI;
	double **B1**B2**B3**B4**Za1;
	double **sB**sFI**sGa**sh**sGT;
	double **C1**CovZa**D1;
	double sGa1[3][2]={100111};
	double **F1**F2**F3**F4**Za2;
	
	Q = eye（m-1）;         //生成的单位矩阵
	for（i=0;i		k[i] = （my_coords[i+1].x-my_coords[0].x）*（my_coords[i+1].x-my_coords[0].x）+\
				（my_coords[i+1].y-my_coords[0].y）*（my_coords[i+1].y-my_coords[0].y）;
		printf（“%f “k[i]）;
	}
	printf（“ \n“）;
	//	Ga	矩阵
	Ga = zeros（m-13）;
	for（i=0;i	{
		Ga[i][0]=-my_coords[i+1].x;
		Ga[i][1]=-my_coords[i+1].y;
		Ga[i][2]=-disT[i];
	}
	GT=MatrixInver（Gam-13）;//Ga的转置矩阵GT
	test_data（GT3m-1）;
	
	//求 h 矩阵  m-1行1列 
	h=zeros（m-11）;
	for（i=0;i		h[i][0]=（disT[i]*disT[i]-k[i]+k1）/2;
		printf（“%6.3f “h[i][0]）;
	}
	printf（“ \n“）;
	
	if（!MatrixOpp（Qm-1））
		return 0;
	test_data（Qm-1m-1）;
	
	//第一次对未知向量Za进行粗略估计（根据残差加权最小二乘求解方程组）
	A1=MatrixMulti（GTQ3m-1m-1）;	
	A2=MatrixMulti（A1Ga3m-13）;	
	if（!MatrixOpp（A23））
		return 0;
	A3=MatrixMulti（A2GT33m-1）;
	A4=MatrixMulti（A3Q3m-1m-1）;
	Za0=MatrixMulti（A4h3m-11）;            //3X1矩阵 
	Mx0=Za0[0][0];	//粗略估计值X0Y0 
	My0=Za0[1][0];
	free_dmatrix（A13）;free_dmatrix（A23）;free_dmatrix（A33）;free_dmatrix（A43）;free_dmatrix（Za03）;
//	printf（“Mx0= %6.3f  My0=%6.3f  Za0[2][0]=%6.3f ;\n“Mx0My0Za0[2][0]）;
	
	//第一次求B ，B用来进行权值的确定（权值为测量值误差方差矩阵的逆矩阵时，估计误差的方差最下）
	B = eye（m-1）;  
	for（i=0;i		B[i][i] = sqrt（（my_coords[i+1].x-Mx0）*（my_coords[i+1].x-Mx0）+（my_coords[i+1].y-My0）*（my_coords[i+1].y-My0））;

	//第一次求FI（下一次最小二乘时权值的确定） FI = B*Q*B; 			 
	B0 = MatrixMulti（BQm-1m-1m-1）;
	FI = MatrixMulti（B0Qm-1m-1m-1）;       
	if（!MatrixOpp（FIm-1））                        //inv（FI）
		return 0;
	//第一次WLS结果 再第一次粗略求解的基础上进一步修正残差权值，提高定位精度 Za1 = inv（Ga‘*inv（FI）*Ga）*Ga‘*inv（FI）*h‘; 
	B1=MatrixMulti（GTFI3m-1m-1）;
	B2=MatrixMulti（B1Ga3m-13）;
	if（!MatrixOpp（B23））
		return 0; 
	B3=MatrixMulti（B2GT33m-1）;
	B4=MatrixMulti（B

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       5862  2015-10-15 16:24  Chan_tdoa.c

     文件       4116  2015-10-16 10:12  Matrix.h

----------- ---------  ---------- -----  ----

                 9978                    2