#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