室内定位算法入门篇

前言

貌似今年做室内定位的人多起来了，总有人问我一些非常基础问题，一方面有的人确实是零基础，另一方面我之前写的一些文章也确实没说的很详细，这里就弄个入门篇。

这里写的算法和代码，我会尽量使用用最最简单的，可能在实际运用中并不是那么实用，但是可以帮助零基础的朋友快速入门。

注意

俗话说的好，师父领进门，修行在个人。如果有朋友是想把代码抄过去直接拿来用，那么你可能会失望的。

你需要懂的基础知识

平面坐标系

坐标系咱们小学数学肯定都学过的，这个相信不用我多说了吧。

我这里就用C语言写个类型Position，分别有两个参数x和y，代表平面坐标系的坐标，表示如下：

struct Position{
	int x;
	int y;
}

概率

这个应该是高中知识，这里给各位回顾一下。

一个袋子里有五个球，三个白球，两个红球。从中取出一个球，取到白球的概率是多少？取到红球的概率是多少？
答案：白球0.6，红球0.4

另外，所有事件的概率和等于1。大概只需要知道这么多就够了。

欧氏距离

听名字挺玄乎的，其实还是咱们小学知识。

欧式距离放在二维、三维坐标系里，就是两点之间的直线距离。举个例子，小明在$(x_1,y_1,z_1)$，小明他爸在$(x_2,y_2,z_2)$，小明和他爸的欧式距离就是：
$$d=\sqrt{(x_1-x_2{)}^2+(y_1-y_2{)}^2+(z_1-z_2{)}^2}$$
所以欧氏距离的本质就是任意维度中两个点之间的距离。

算法

惯性导航

假设小明在一个平面坐标系的原点$(0,0)$上，他向北（$90°$）走了10米，又向东（$0°$）走了10米，咱们都知道他现在应该在$(10,10)$的位置上对吧。那么我们就先用代码来表示小明是怎么移动的：

//oldPosition表示移动前的坐标
//distace表示移动的距离
//angle表示移动的方向
Position getNewPosition(Position oldPosition,float distance,float angle){
	Position newPosition;
	
	newPosition.x = oldPosition.x + distance.x * cos(angle);
	newPosition.y = oldPosition.y + distance.y * sin(angle);
	
	return newPosition;
}

然后把小明的移动过程用代码描述一遍（下面的$PI=\pi$，代码里面打不出来，这里说明下）：

static float PI = 3.141592;

void XiaoMingMove(){
	//初始化坐标起点（0，0）
	Position position;
	position.x=0;
	position.y=0;
	
	//向北走10米，北的方向在坐标系里是90度，转换成弧度也就是PI/2
	position=getNewPosition(position,10,PI/2);
	
	//向东走10米，东的方向在坐标系里是0度，弧度就是0
	position=getNewPosition(position,10,0);
}

对，这玩意就可以称之为惯性导航了。简而言之，知道了小明前进的距离和方向，就能根据移动前的位置计算出小明移动后所在的位置。

蓝牙定位

从理论上来说，蓝牙定位是有唯一解的，但是实际运用中由于信号误差比较大，按照理论方法计算出来的结果可信度往往不高，所有现在一般的做法是将其转换为一个概率问题。

举个例子，小明在一个三角形中，假设三角形的顶点分别是$A(x_A,y_A)$、$B(x_B,y_B)$、$C(x_C,y_C)$。

同时告诉你小明在A点的概率是0.6，在B点的概率是0.3，在C点的概率是0.1，要求计算出小明所在的位置。一般来说有两种解决方案：

1. 在A点的概率最大，就直接选择A点
   $$\{\begin{array}{l}x=x_A\\ y=y_A\end{array}$$
2. 根据概率来计算坐标。这是啥意思呢，本质就是求个均值：
   $$\{\begin{array}{l}x=0.6x_A+0.3x_B+0.1x_C\\ y=0.6y_A+0.3y_B+0.1y_C\end{array}$$

不用多说，肯定是方案二相对更准确些。

蓝牙定位的本质就是这么个概率问题，那么可能有人（谁！？）就要问了，这概率怎么求呢？现在市面上的蓝牙或者也叫ibeancon，在连接端（比如你的手机）是可以计算出蓝牙的信号强度（又叫RSSI）的，我们可以认为蓝牙信号强度越强，当前蓝牙的概率就越大，信号强度越弱，当前蓝牙的概率就越小。

另外，通过RSSI在接收端可以近似计算出当前位置距离蓝牙的距离，这里我并不想提供相应的转换公式，因为这个介绍需要考虑到一些蓝牙实际运用中的参数，介于篇幅有限，各位自行了解吧（可以参考这篇文章）。

我这里重点介绍如何将得到的蓝牙距离如何转换为概率。还是用刚才三角形，已知小明根据蓝牙信号测出自己到三个蓝牙的距离分别为$s_A$，$s_B$，$s_C$。距离越小，信号越强，概率就越大，说明距离和概率是反比关系。然而具体是个怎么样的反比关系，其实我也不清楚，得根据实践来摸索。反正既然是反比关系，这里简单点，就让其为倒数关系：
$$p=\frac{1}{s}$$
（当然也可以是$1/s^2$、$1/\sqrt{s}$等）。另外，假设有
$$sum=\frac{1}{\frac{1}{s_A}+\frac{1}{s_B}+\frac{1}{s_C}}$$

三个点的概率计算式为：
$$\{\begin{array}{l}{p}_A=\frac{1}{s_A}\frac{1}{sum}\\ p_B=\frac{1}{s_B}\frac{1}{sum}\\ p_C=\frac{1}{s_C}\frac{1}{sum}\end{array}$$
为什么要除上$sum$呢？因为概率之和需要等于1，因此这个处理过程又叫做归一化处理。

代码实现一哈。

//poses蓝牙坐标数组
//dises蓝牙距离数组
//bt_num蓝牙数量
Position getBTPosition(Position[] poses,float[] dises,const int bt_num){
	//初始化坐标（0，0）
	Position position;
	position.x=position.y=0;
	
	//计算sum
	float sum=0;
	for(int i=0;i<bt_num;++i){
		sum += 1/dises[i];
	}
	
	//计算每个蓝牙的概率
	float p[bt_num]={0};
	for(int i=0;i<bt_num;++i){
		p[i] = 1 / (dises[i] * sum);
	}
	
	//根据概率求得当前位置
	for(int i=0;i<bt_num;++i){
		position.x += poses[i].x * p[i];
		position.y += poses[i].y * p[i];
	}

	return position;
}

有的朋友跟我说没学过C语言，这里我就尽量使用通用的语法来描述，众口难调，如果还看不懂的话我也是没辙了。另外，蓝牙的位置怎么得到，由于每个蓝牙具有唯一标识号，可以通过唯一标识在数据库里进行查找坐标。这里吐槽一句，居然还有人问我数据库里的坐标哪来的，你问安装蓝牙的师傅啊，蓝牙安在哪就是哪了。

指纹法

指纹法是一类算法的统称，比如人脸识别就是一种指纹法，相机先拍了你的脸，然后把你的脸放到数据库里一一进行对比，最后找到了你，或者找错找到了别人。

这里讲一种最简单的指纹法，就是对比数据之间的欧式距离，找到最短欧氏距离所在的点，就找到了定位位置。

举个例子，还是在三角形内，

作为指纹法，肯定需要在定位之前先采集数据（就跟人脸识别一样，得先把脸记录下来），假设我们的设备在三角形的三个点采集的数据（这里的数据是个抽象值，可以是磁场或者其他数据）分别为：$D_A(a_1,a_2,a_3)$，$D_B(b_1,b_2,b_3)$，$D_C(c_1,c_2,c_3)$，这个时候，小明在三角形的某处采集的信号为$D_{XM}(x_1,x_2,x_3)$。

由于没有具体得数值，我们就假设其中欧式距离最小的点为B，那么就可以认为小明此时在B点处。

代码实现一下

//poses信号采集点的坐标数组
//datas信号采集点的数据集合
//data当前位置采集的信号数据
//bt_num信号点的数量
Position getPosition(Position poses[],float datas[][],float data[],int bt_num){
	//保存最小欧式距离
	float min_d = -1;
	//保存最小欧式距离所在的点的下标
	int index = 0;
	
	//寻找最小欧氏距离
	for(int i=0; i < bt_num; ++i){
		//计算欧氏距离，根据上面得描述，每个点采集得数据维度是3，
		float d=sqrt(
			(datas[i][0]-data[i][0]) * (datas[i][0]-data[i][0]) +
			(datas[i][1]-data[i][1]) * (datas[i][1]-data[i][1]) +
			(datas[i][2]-data[i][2]) * (datas[i][2]-data[i][2])
		);
		
		//判断是否是小于当前记录最小的欧氏距离
		if(min_d < 0 || d < min_d){
			index = i;
			min_d = d;
		}
	}
	
	return poses[index];
}

这个同样是给大家做个示范，实际运用效果如何，就不好说了，一般来说，采集数据的维度越多，指纹法匹配的精度就越高。

定位融合

融合本质上是指将两种定位方式结合在一起，具体怎么结合，方法其实很多种。

举个例子，我现在得到了两种定位方法的坐标$P_1(x_1,y_1)$，$P_2(x_2,y_2)$，我假设定位方法一的可信度为0.6，方法二的可信度为0.4，那么融合的坐标为：
$$\{\begin{array}{l}x=0.6x_1+0.4x_2\\ y=0.6y_1+0.4y_2\end{array}$$
so easy！当然这只是一种最简单的融合策略，根据每种定位方式的特性，我们可以采取不同的融合方式，这就需要各位自行摸索了。

总结

这篇文章基本上囊括了最基础的定位算法，由于室内定位这个行业没有太多的理论体系支撑，大部分的时候往往需要自己来摸索，而且每个人的做法不同，这篇文章就当是抛砖引玉了。写的时候比较匆忙，如果各位有什么疑虑或者文章中有什么不对的地方，欢迎留言。