ibeacon UWB GPS 空间四点定位算法

最近在研究uwb空间四点的精准定位，其实是基于RSSI原理的，蓝牙IBEACON,GPS也差不多基于这个原理

三维空间的四点定位算法：

已知四个基站点的坐标(x1,y1,z1)(x2,y2,z2)(x3,y3,z3)(x4,y4,z4)和到未知点(x,y,z)的距离R1,R2,R3,R4

所以四点定位使用矩阵求解方程的方式：

其中要求的点的坐标是(x,y,z)，已知的四个探针的位置为(x1,y1,z1)，(x2,y2,z2)，(x3,y3,z3)，(x4,y4,z4)，r1到r4为所求的点到四个探针的距离。

现在我们的任务就是求解上述方程，首先分解：

然后相减：

其中：

这时候就可以转换为矩阵相乘:

假设上面矩阵为A*B=C，那么此时只需要求解A的逆矩阵，然后把逆矩阵同时乘以等式两边，就可以求得x，y，z了。

具体求解逆矩阵的方法为高斯-约旦消元法（计算机专业的大一线性代数课程内容）

/**
 * G-J消元法求矩阵逆矩阵
 *
 * @param 
 * @return 一个矩阵对象，为当前的矩阵的逆矩阵
 */
Matrix Matrix::inverseGJ(){
    int i,j,k;
    int dimension = row;
    Matrix output(row,col);
    //将输出矩阵初始化为单位矩阵
    output.eye();
 
    for(i=0;i<dimension;++i)  //依次处理每一列
    {
        for(j=0;i<dimension;++j)  //如果当前行当前列值为0，做行变换
        {
            if(pmm[j*col+i]!=0)
            {
                swap(0,j);
                output.swap(0,j);
                break;
            }
        }
        for(j=0;j<dimension;++j)  //依次处理每一行
        {
            if(j==0)  //如果是第一行，将input[j][i]设置为1，其他元素均除以input[i][i]
            {
                for(k=dimension-1;k>=0;--k)
                    output.setElement(j,k,output.getElement(j,k)/pmm[j*col+i]);
                for(k=dimension-1;k>=i;--k)
                    pmm[j*col+k]/=pmm[j*col+i];
            }
            else  //如果不是第一行，将每一行的input[j][i]设置为0，该行其他元素都要倍数的减去第一行对应位置上的值
            {
                for(k=dimension-1;k>=0;--k)
                    output.setElement(j,k,output.getElement(j,k)-pmm[j*col+i]/pmm[i]*output.getElement(0,k));
                for(k=dimension-1;k>=i;--k)
                    pmm[j*col+k]=pmm[j*col+k]-pmm[j*col+i]/pmm[i]*pmm[k];
            }
        }
        swap(0,(i+1)%dimension);  //每次都将下一次需要处理的行和当前的第一行交换
        output.swap(0,(i+1)%dimension);
    }
    output.reorder(); //因为之前的交换操作，行顺序乱了，需要重新排列一下，即把第一行的数据放到最后一行后面
    
    return output;
}

具体C++代码请加微信：