多点定位法之最小二乘法快速求解——蓝牙定位

前言

三点定位法是蓝牙定位中非常实用的算法，但是实际使用的时候，我们可能会遇到超过三点的情况，那么此时一种可取的方式就是只取最强的三个信号，也可以通过最小二乘法来对多点进行快速求解。

思路

最小二乘法的精髓在于通过矩阵运算来快速求解。那么如何把蓝牙信号转换为矩阵呢？

我们假设有一组蓝牙坐标 $P\{(x_1,y_1),(x_2,y_2),...,(x_n,y_n)\}$和对应的蓝牙距离值（通过蓝牙RSSI计算得到） $D=\{d_1,d_2,...,d_n\}$，可设此时目标位置在 $(x,y)$处，我们可以得到公式：
$\begin{cases} \quad (x_1-x)^2+(y_1-y)^2=d_1^2\\ \quad (x_2-x)^2+(y_2-y)^2=d_2^2\\ \quad\quad\quad\quad\quad ...\\ \quad (x_n-x)^2+(y_n-y)^2=d_n^2 \end{cases}$
化简得：
$\begin{cases} \quad x^2+y^2-2x_1x-2y_1y=d_1^2-x_1^2-y_1^2\\ \quad x^2+y^2-2x_2x-2y_2y=d_2^2-x_2^2-y_2^2\\ \quad\quad\quad\quad\quad\quad\quad\quad ...\\ \quad x^2+y^2-2x_nx-2y_ny=d_n^2-x_n^2-y_n^2 \end{cases}$
对上面的式子同时减去第n行公式，消去 $x^2+y^2$，得：
$\begin{cases} \quad (x_n-x_1)x+(y_n-y_1)y=(d_1^2-d_n^2+x_n^2+y_n^2-x_1^2-y_1^2)/2\\ \quad (x_n-x_2)x+(y_n-y_2)y=(d_2^2-d_n^2+x_n^2+y_n^2-x_2^2-y_2^2)/2\\ \quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad\quad ...\\ \quad(x_n-x_{n-1})x+(y_n-y_{n-1})y=(d_{n-1}^2-d_n^2+x_n^2+y_n^2-x_{n-1}^2-y_{n-1}^2)/2 \end{cases}$
这么一来矩阵就出来了：
$X= \begin{bmatrix} x_n-x_1 & y_n-y_1 \\ x_n-x_2 & y_n-y_2 \\ ...\\ x_n-x_{n-1} & y_n-y_{n-1} \end{bmatrix} \tag{1}$

$\beta= \begin{bmatrix} x \\ y \\ \end{bmatrix} \tag{1}$
$Y= \begin{bmatrix} (d_1^2-d_n^2+x_n^2+y_n^2-x_1^2-y_1^2)/2 \\ (d_2^2-d_n^2+x_n^2+y_n^2-x_2^2-y_2^2)/2 \\ ...\\ (d_{n-1}^2-d_n^2+x_n^2+y_n^2-x_{n-1}^2-y_{n-1}^2)/2 \end{bmatrix} \tag{1}$

可知：
$X\beta=Y$
那么利用最小二乘法就可以快速求解 $\beta$的近似解 $\beta'$了：
$\beta'=(X^TX)^{-1}X^TY$

实现

具体的实现涉及矩阵运算，这里也就不特别说明了，如果是python语言会比较容易（numpy库）。用Java的朋友可以看看这篇文章。