声音定位系统算法实现2012电赛D题

（2012年电子设计大赛 D题）二维平面TDOA算法实现，VC仿真通过精度；0.001mm，移植到 MSP430F149，算法精度：0.01mm。附带说明文档，详细注解， 方便大家继续研习

//******************************************************************
//（2012年电子设计大赛）D题：声音定位系统 （二维平面TDOA定位算法） 算法测试
// 描述: 以题目所标注的零点为坐标原点，四角各坐标则可分别标号并表示为 
//          
//			  2.（0350）---------------------- 3.（500350）
//
//            |                                 |
//            |                                 |
//            |          发生源（xy）          |
//            |                                 |
//            |                                 |
//            |                                 |   
//
//            1.（0  0）------------------------4.（5000）
// 参数：t1 表示声音传播到接受点12之间的时间差  
//       t2 表示声音传播到接受点13之间的时间差
//       c  表示理论中的声速
//       Q  宏定义为 π的值
// 调试验证环境： 运行仿真环境：VC++ 6.0  （精度0.01mm）
//                核心代码移植系统环境： IAR5.4 MSP430F149 （精度：0.1mm）
// 作者：小浣熊  qcxycwf@163.com
// 时间：2012.8.2 版本 V1.0 （验证通过）
// 参考文献：1.《基于到达时间差的平面声无源定位技术研究》
//           2.《基于TDOA定位技术的仿真研究》
//           3.《声源定位实验 —— 一个具有明确应用背景和前沿技术特点的综合性实验》
//*********************************************************************
#include “stdio.h“
#include “math.h“

#define  c  340    //定义声速为340M/S

//测试点1（这里取点当声音传播到1.2点的时间差为0，且声音传播到1.4点的时间差为0）
#define  t1 0//测试假设  这个发声部分在最特殊的中点
#define  t2 0

//测试点 2  坐标点（110110），先算出时间差，然后验证算法
//（这里取点当声音传播到1.2点的时间差为0.000318930，且声音传播到1.4点的时间差为0.000733636）
//#define t1 0.000318930
//#define t2 0.000733636


#define  Q 3.141592654
//坐标点定位系统（注意这里是以 内部基准坐标 而建立）

void main（）
{
	float x1y1x2y2temp;
	//分别表示以1.（00）为原点建立的坐标系下，另外两个定位点基准点的坐标
	float U;		//计算声源 极坐标 角度的中间变量
	float H;		//声源的 极坐标 （角度）
	float r;		//声源距离坐标原点的距离
	float ABD;	//分别用来存放数据 ABD 对应各自的算法公式

	x1 = 0;
	y1 = 0.35; //（X1Y1）即 基准点2
	x2 = 0.5;  //（X2Y2）即 基准点4
	y2 = 0; 

	A = x2*（x1*x1+y1*y1-c*t1*c*t1） - x1*（x2*x2+y2*y2-c*t2*c*t2）;
	B = y2*（x1*x1+y1*y1-c*t1*c*t1） - y1*（x2*x2+y2*y2-c*t2*c*t2）;
	D = c*t1*（x2*x2+y2*y2-c*t2*c*t2） - c*t2*（x1*x1+y1*y1-c*t1*c*t1）;
	U = atan（B/A）;//求解U的角度（换算成度数了）


	printf（“所求的中间变量 A 为:%f\n“A）;
	printf（“所求的中间变量 B 为:%f\n“B）;
	printf（“所求的中间变量 D 为:%f\n“D）;
	printf（“所求的角度 U 为:%f°\n“U*180/Q）;//（换成了度数）
	
	
	temp = D/sqrt（A*A+B*B）;//把这个数据算出来
	printf（“所求的中间变量 temp 为:%f\n “temp）;

	H = acos（D/sqrt（A*A+B*B））+U;//测得角度 H（也就是声源的角度）
	r = fabs（（x1*x1+y1*y1-c*t1*c*t1）/（2*（x1*cos（H）+y1*sin（H）+c*t1）））;//绝对值函数（求解距离）
	printf（“\n\n\n\n声源与坐标原点之间的距离r为:%f 米，与X轴缩成角度角度 H 为:%f°\n\n“rH*180/Q）;
//显示坐标
	printf（“声源的的横坐标X为：%f mm 纵坐标Y为: %f mm\n\n“r*cos（H）*1000r*sin（H）*1000）;
	printf（“声源坐标为（%f%f）\n\n“r*cos（H）*1000r*sin（H）*1000）;
}

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     目录           0  2012-08-09 16:55  2012年电子设计 D题算法\
     目录           0  2012-08-06 09:41  2012年电子设计 D题算法\Debug\
     文件        5787  2012-08-09 16:55  2012年电子设计 D题算法\Debug\TDOA.obj
     文件       33792  2012-08-09 16:55  2012年电子设计 D题算法\Debug\vc60.idb
     文件       45056  2012-08-09 16:55  2012年电子设计 D题算法\Debug\vc60.pdb
     文件      204839  2012-08-09 16:55  2012年电子设计 D题算法\Debug\算法.exe
     文件      247440  2012-08-09 16:55  2012年电子设计 D题算法\Debug\算法.ilk
     文件      175516  2012-08-06 02:00  2012年电子设计 D题算法\Debug\算法.pch
     文件      459776  2012-08-09 16:55  2012年电子设计 D题算法\Debug\算法.pdb
     文件         411  2012-08-06 07:12  2012年电子设计 D题算法\resource.h
     文件       16608  2012-08-06 07:25  2012年电子设计 D题算法\script1.aps
     文件        1447  2012-08-06 07:12  2012年电子设计 D题算法\script1.rc
     文件        3130  2012-08-09 16:55  2012年电子设计 D题算法\TDOA.c
     文件      155131  2012-08-06 00:18  2012年电子设计 D题算法\基于TDOA定位技术的仿真研究.pdf
     文件       80304  2012-08-06 00:06  2012年电子设计 D题算法\基于到达时间差的平面声无源定位技术研究（1）.pdf
     文件      128016  2012-08-06 01:45  2012年电子设计 D题算法\声源定位实验_一个具有明确应用背景和前沿技术特点的综合性实验.pdf
     文件        4258  2012-08-06 07:12  2012年电子设计 D题算法\算法.dsp
     文件         516  2012-08-06 01:58  2012年电子设计 D题算法\算法.dsw
     文件       41984  2012-08-09 16:55  2012年电子设计 D题算法\算法.ncb
     文件       48640  2012-08-09 16:55  2012年电子设计 D题算法\算法.opt
     文件        1733  2012-08-09 16:55  2012年电子设计 D题算法\算法.plg