蓝牙定位--多点质心定位

废话不多说，直接上干货,后附已验证通过的质心算法

总体逻辑

step1

在理想环境下，已知三个信标的坐标和到坐标的距离，使用三点定位能获得三个圆的交点，从而得到位置。

而现实情况下却情况难料。这时候使用质心法来解决复杂的位置定位

strp2

所谓质心，就是横坐标、纵坐标分别为N个点的横坐标平均值、纵坐标平均值的点。即：假定N个点的坐标分别（x1,y1)，(x2,y2)，……，则质心的坐标为（(x1+x2+…)/N, (y1+y2+…)/N)。
如图，p1,p2,p3交点做平均，得到的就是三个圆中心位置的质心了，每两个圆有两个交点，如何确定那些交点是p1,2,3而不是外面的三个交点呢？

step3

最小二乘法：最小二乘法思想就是求解未知参数，使得理论值与观测值之差的平方和达到最小。

用在这里当然不是为了拟合曲线，我们可以参考最小的思想，圆1与圆2，圆3交点之间距离的平方最小的值的三个点作为p1,p2,p3。

step4

既然p1,p2,p3能求到，那么现在的问题在于如何求圆的交点，对于相交的圆是很好计算的，但相离或者包含的圆如何计算交点呢？如图，当两个圆相离或者包含时候，最佳的定位位置就是红色点的位置，所以，对于相离圆可以同时扩大两圆半径，使其相交。对于相含的圆，大圆减小半径，小圆扩大半径，使其相交。

step5

最后，三个圆两两计算获得1到2个交点，通过多个交点得到p1,p2,p3，通过这三个点计算质心。
可以在多个圆中找三个距离最近的圆计算（rssi越小确信值越高）。也可以N个圆循环选出三个圆进行计算。

将上面步骤逆向，编写代码即可

代码逻辑

step1

在装有所有圆坐标和半径的容器中循环，选出3个圆进行计算，统计所有的质点，均值后作为该点的位置坐标（list没有随机访问，只能通过迭代器，甚是麻烦）

typedef struct bea
{
	string  Bea_ID;
	double  Bea_Dis;
	pair<double, double> X_Y;
}BEA_DIS;
//。。。。
	for (list<BEA_DIS>::iterator it = dev_dis.Dev_Bea_Dis->begin(); it != dev_dis.Dev_Bea_Dis->end();)
	{   //it 每轮自加3  为3的倍数
		list<BEA_DIS>::iterator it_buf = it;//it备份
		if (++it == dev_dis.Dev_Bea_Dis->end())
		{
			buf_data.Dev_X_Y = make_pair(buf_x_y.first / num, buf_x_y.second / num);
			return buf_data;
		}
		auto bt = it;
		if (++it == dev_dis.Dev_Bea_Dis->end())
		{
			buf_data.Dev_X_Y = make_pair(buf_x_y.first / num, buf_x_y.second / num);
			return buf_data;
		}
		for (auto tt = it; tt != dev_dis.Dev_Bea_Dis->end(); ++tt)//第三个数据
		{
			for (; bt != tt; ++bt)//第二个数据
			{
				printf("$$输入三个坐标：%f,%f=%f,%f=%f,%f\n", it_buf->X_Y.first, it_buf->X_Y.second, bt->X_Y.first, bt->X_Y.second, tt->X_Y.first, tt->X_Y.second);
				pair<double, double> buf= threepoint(make_pair(it_buf->Bea_Dis, it_buf->X_Y), make_pair(bt->Bea_Dis, bt->X_Y), make_pair(tt->Bea_Dis, tt->X_Y));
				printf("$$输出坐标：%f::%f\n", buf.first,buf.second);
				if (buf.first != 0 && buf.second != 0)
				{
					buf_x_y += buf;
					num++;
				}
			}
		}
		it = ++it_buf;
	}

step2

threepoint（）函数对传入的三个圆，返回三个圆的质心
Jd（）函数传入两个圆，返回两个圆的交点（1个或者两个）。取得三组交点后，计算p1,p2,p3通过这三点得到质点。参数 pair<double, pair<double, double>>dev1，第一个double为半径，后面两个为x和y。

pair<double, double> threepoint(pair<double, pair<double, double>>dev1, pair<double, pair<double, double>>dev2, pair<double, pair<double, double>>dev3)
{
	pair<double, double> p1 ;// 有效交叉点1
	pair<double, double> p2 ;// 有效交叉点2
	pair<double, double> p3 ;// 有效交叉点3
	pair<double, double> zx ;//计算三点质心
	list<pair<double, double>> jds1 = jd(dev1.second.first,dev1.second.second,dev1.first, dev2.second.first, dev2.second.second, dev2.first);// r1,r2交点
		list<pair<double, double>> jds2 = jd(dev1.second.first, dev1.second.second, dev1.first, dev3.second.first, dev3.second.second, dev3.first);// r1,r3交点
		list<pair<double, double>> jds3 = jd(dev2.second.first, dev2.second.second, dev2.first, dev3.second.first, dev3.second.second, dev3.first);// r2,r3交点
	double t = 1000000;
	for (pair<double, double> jd1 : jds1)
	{
		for (pair<double, double> jd2 : jds2)
		{
			for (pair<double, double> jd3 : jds3)
			{
				double jd1_2_dis = sqrt(pow(jd1.first - jd2.first, 2) + pow(jd1.second - jd2.second, 2));
				double jd1_3_dis= sqrt(pow(jd1.first - jd3.first, 2) + pow(jd1.second - jd3.second, 2));
				double jd2_3_dis= sqrt(pow(jd2.first - jd3.first, 2) + pow(jd2.second - jd3.second, 2));
				if (jd1_2_dis + jd1_3_dis + jd2_3_dis < t)
				{
					p1 = jd1;
					p2 = jd2;
					p3 = jd3;
					t = jd1_2_dis + jd1_3_dis + jd2_3_dis;
				}
			}
		}
	}
	zx.first = (p1.first + p2.first + p3.first) / 3;//质心
	zx.second = (p1.second + p2.second + p3.second) / 3;
	return zx;
}

step3

Jd两圆求交点算法。想离和包含的圆修改半径进行递归。+0.2是为了获得两个交点，更利于找到更精确的位置.。后面有时间会写一篇求两圆交点的推导。

static list<pair<double, double>> jd(double x1, double y1, double r1, double x2, double y2, double r2)
{
	list<pair<double, double>>points;//交点坐标
	pair<double, double> coor;
	double d = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));// 两圆心距离
	if (d > r1 + r2 ) 
	{   //相离
		double buf =( d - r1 - r2 )/ 2.0+0.2;
		r2 += buf;
		r1 += buf;
		return jd(x1,y1,r1,x2,y2,r2);
	}
	else if (d < abs(r1 - r2))
	{
		//两圆相含
		return points;

		double buf = (abs(r1 - r2)-d) / 2.0 + 0.2;
		if (r1 > r2)//r1包含r2
		{
			r1 -= buf;
			r2 += buf;
		}
		else//r2包含r1
		{
			r2 -= buf;
			r1 += buf;
		}
		return jd(x1, y1, r1, x2, y2, r2);
	}
	else if (x1 == x2 && y1 == y2)
	{   
		return points;// 同心圆 
	}

	else if (y1 == y2 && x1 != x2&&d < (r1 + r2))// 两圆向交
	{
		double a = ((r1 * r1 - r2 * r2) - (x1 * x1 - x2 * x2)) / (2 * x2 - 2 * x1);
			if (d == abs(r1 - r2) || d == r1 + r2) //圆心距==半径差或者==半径和
		{
			// 只有一个交点时
			coor.first= a;
			coor.second = y1;
			points.push_back(coor);
		}
		else 
		{
			// 两个交点
			double t = r1 * r1 - (a - x1) * (a - x1);
			coor.first = a;
			coor.second= y1 + sqrt(t);
			points.push_back(coor);
			coor.first = a;
			coor.second = y1 - sqrt(t);
			points.push_back(coor);
		}
	}
	else if (y1 != y2&&d < (r1 + r2))// 两圆向交
	{
		double k, disp;
		k = (2 * x1 - 2 * x2) / (2 * y2 - 2 * y1);
		disp = ((r1 * r1 - r2 * r2) - (x1 * x1 - x2 * x2) - (y1 * y1 - y2 * y2)) / (2 * y2 - 2 * y1);// 直线偏移量
		double a, b, c;
		a = (k * k + 1);
		b = (2 * (disp - y1) * k - 2 * x1);
		c = (disp - y1) * (disp - y1) - r1 * r1 + x1 * x1;
		double disc;
		disc = b * b - 4 * a * c;// 一元二次方程判别式
		if (d == abs(r1 - r2) || d == r1 + r2)//圆心距==半径差或者==半径和
		{
			//一个交点
			coor.first = (-b) / (2 * a);;
			coor.second= k * coor.first + disp;
			points.push_back(coor);
		}
		else
		{
			//2个交点
			coor.first = ((-b) + sqrt(disc)) / (2 * a);
			coor.second= k * coor.first + disp;
			points.push_back(coor);
			coor.first = ((-b) - sqrt(disc)) / (2 * a);
			coor.second = k * coor.first + disp;
			points.push_back(coor);
		}
	}
	return points;
}