室内定位算法入门篇

前言

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

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

注意

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

你需要懂的基础知识

平面坐标系

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

我这里就用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