最近在研究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++代碼請加微信：

????

總結

以上是生活随笔為你收集整理的ibeacon UWB GPS 空间四点定位算法的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。