之前写过基于rssi的三点定位算法,但是那是平面的,下面介绍三维空间的四点定位算法:
基于rssi的原理,这里不再重复,请参考以前的博文:
http://blog.csdn.net/u013090676/article/details/73865137
之前三点定位是用的几何的方式求解,但是放在空间就显得极其麻烦了,所以四点定位使用矩阵求解方程的方式:
其中要求的点的坐标是(x,y,z),已知的四个探针的位置为(x1,y1,z1),(x2,y2,z2),(x3,y3,z3),(x4,y4,z4),r1到r4为所求的点到四个探针的距离。
现在我们的任务就是求解上述方程,首先分解:
然后相减:
其中:
这时候就可以转换为矩阵相乘:
假设上面矩阵为A*B=C,那么此时只需要求解A的逆矩阵,然后把逆矩阵同时乘以等式两边,就可以求得x,y,z了。具体求解逆矩阵的方法为高斯-约旦消元法,具体求解方法在这里不重复。
上述的矩阵相乘,求逆矩阵,三点平面定位,四点空间定位的算法,已经上传到码云,地址:
https://gitee.com/yukarijiang/LinaPHPAlgorithm
当然,为啥是用php,因为博主是phper啦……