三点定位法原理及实现——蓝牙定位

前言

最近在做蓝牙室内定位，蓝牙定位用到的一个非常经典的算法就是三点定位法。

原理

三点定位法，顾名思义首先有三个圆点，同时我们也知道这三个圆的半径，最终求得三圆的交点，达到定位效果。如图：

我们的目的是求得O点，利用毕达哥拉斯定理我们可以快速求得O点坐标。不过事与愿违，现实情况并不总是如人意，而是这样：

甚至有的情况其中两个圆或者三个圆完全就不相交，如下图：

由于这些误差原因，实际上想要准确的知道使用者当前位置是相当困难的，但是我们可以通过运算得到一个近似解。为了应付这些情况，我们需从两个圆入手，先找到两两圆之间的中心点，再求三圆的中心点。

除去圆中圆的情况（现实基本上不存在），两圆关系可分为两种，相交，不相交。

两圆相交

可知此时两圆交点A，B，我们的目标点是AB中点C。根据勾股定理我们可知
$$\{\begin{array}{l}PQ=PC+QC\\ Q{A}^2=Q{C}^2+A{C}^2\\ P{A}^2=P{C}^2+A{C}^2\end{array}$$
解得：
P C = P Q 2 + P A 2 − Q A 2 2 P Q PC=\frac{PQ}{2}+\frac{PA^2-QA^2}{2PQ} PC=2PQ​+