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++代码请加微信：

ibeacon UWB GPS 空间四点定位算法相关推荐

基于RSSI三维空间四点定位算法
之前写过基于rssi的三点定位算法,但是那是平面的,下面介绍三维空间的四点定位算法: 基于rssi的原理,这里不再重复,请参考以前的博文: http://blog.csdn.net/u01309067 ...
已知空间四点，求四点拟合的球圆心和半径
参考汇总: (55条消息) 球面拟合算法_liyuanbhu的博客-CSDN博客_球面拟合 (54条消息) 球面数据拟合算法简介__IRONMAN_的博客-CSDN博客_最佳拟合球面空间四点确定球心 ...
【欧式距离：二维和三维定位算法】
[关于开发UWB模块所使用的二维和三维定位算法] 本文将介绍开发UWB模块中所使用到的二维定位算法(三点定位算法) 链接:三维空间的构建(四点定位算法) 内容概览概念介绍需要的材料算法欧几里得 ...
简述isodata算法的原理_基于UWB技术的室内定位方法简述
1. UWB室内定位概述: UWB室内定位技术与传统通信技术有极大的差异,它不需要使用传统通信体制中的载波,而是通过发送和接收具有纳秒或纳秒级以下的极窄脉冲来传输数据,从而具有GHz量级的带宽.超宽带 ...
UWB定位算法比较TDOA和TWR究竟哪个好
使用UWB技术进行定位时,有两种基本定位算法:基于差分飞行时间(TDOA)和双向测距(TWR)的UWB定位算法,这两种算法各有优劣,下面将进行比较. 超宽带是一种可用于室内定位的短距离无线电通信.与蓝 ...
GPS、WiFi、基站、蓝牙(iBeacon)、AGPS几种定位方式总结
1.GPS GPS(Global Positioning System)即全球定位系统,它是由美国研究的一种定位方式,特点是:不需要SIM卡,不需要连接网络,只要在户外,基本上就能随时随地的准确定位. ...
基于UWB的室内SDS_TWR测距算法优化和定位算法融合的研究
1.内容简介略257 2.内容说明 1.RSSI定位方法基于接收信号强度RSS(Receive Signal Strength)[57]方法通过三个及以上己知位置的锚节点来测量移动节点发射的信号场 ...
matlab水声定位,基于水平投影的ROV水下GPS精确定位算法
基于水平投影的ROV水下GPS精确定位算法基于水平投影的ROV水下GPS精确定位算法张颖军付本国刘光 (海军潜艇学院,山东青岛,266046) 摘要精确的水下定位是提高水下机器人(ROV) ...
matlab rdk,rtklib_2.4.2_p3 gps导航定位算法，有最先进的rtk matlab 238万源代码下载- www.pudn.com...
文件名称: rtklib_2.4.2_p3下载收藏√ [ 5 4 3 2 1 ] 开发工具: C++ Builder 文件大小: 14970 KB 上传时间: 2014-07-20 下载次 ...
rssi室内定位算法原理_室内定位方案常用的4种定位算法
目前常见的室内定位技术有超宽带UWB室内定位技术,蓝牙室内定位技术,RFID(无线射频识别)定位,超声波定位,Wi-Fi定位等.室内定位依赖于定位算法,定位算法决定了室内定位的模式.室内定位种类虽然比 ...

ibeacon UWB GPS 空间四点定位算法

ibeacon UWB GPS 空间四点定位算法相关推荐

最新文章

热门文章