三边测量定位算法C语言实现
三边定位算法
三边定位算法简介
三个位置已知点(锚节点,圆心)以及其到另外一个未知点(待定位点)的距离即半径(不准确),求位置节点坐标的过程;
实际上(例如用rssi算距离)三个点到未知点的距离半径很可能不交与一点;怎么办呢?
参考文章 https://www.jianshu.com/p/94475aacc95a
这里引用下方法的逻辑:
两圆不相交时,按比例取中点(和)。当两圆相交时,就是拆分成几个三角形,通过一系列三级函数计算出未知的两个交点。最后将三点连成三角形,此三角形的重心(即点M)就是最终定位点,步骤如下:
通过勾股定律用a、b长度计算出线段AB长度(即点A到点B距离),使用 ra + rb 与AB对比即可得知两圆的对应情况,一共有三种情况:两圆相离ra + rb < AB、两圆相切ra + rb == AB、两圆相交ra + rb > AB。
两圆相离:按照两圆半径的比例在线段AZ上求点,即;因为“两圆相切ra + rb == AB”在实际程序中出现的几率太小,所以直接使用“两圆相离”相同的求法。
两圆相交:求出相交点C的坐标 {Cx, Cy},可通过得出Q1,通过得出Q2,最后计算出点C的坐标:
同理可求出点D的坐标。得到C、D两交点后取距离圆心Z点近的交点作为最后三个参考点中的一点。
将最后求得的三个参考点连接成一个三角形,该三角形的重心即为最后的定位点M:
作者:Hank_Zhong
链接:https://www.jianshu.com/p/94475aacc95a
来源:简书
发现的问题
但是这篇文章对应的如下代码好像不太对,,
具体就是Eo=0.5*EF那里(第11行),这应该是一种工程近似,在两圆相交的很少的情况之下可以这样近似,可实际情况复杂多变,两个圆相交很多的情况很常见;
总要判断相对位置正负号很麻烦;
-(CGPoint)sidePointCalculationWith:(double)x1 :(double)y1 :(double)r1 :(double)x2 :(double)y2 :(double)r2 :(double)x3 :(double)y3{//勾股定理 sqrt(X)是X开根号 pow(X,n)是X的n次方//取beacon1圆心A 与 beacon2圆心B的距离double AB = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));double rAB = (r1 + r2);if (rAB > AB && (r1 < AB && r2 < AB)) {//两圆有相交点,两圆相交点为C、D。两圆与AB的相交点为E、F。o是EF的中点。double EF = rAB - AB;double Eo = EF * 0.5;double AE = r1 - EF;double Ao = AE + Eo;double AQ1 = acos((x2 - x1) / AB);double AQ2 = acos(Ao / r1);double BF = r2 - EF;double Bo = BF + Eo;
// double BQ1 = acos(fabs(x1 - x2) / AB);double BQ2 = acos(Bo / r2);//原点{0,0}在左上角的情况下double Cx = x1 + (r1 * cos(AQ1 + AQ2));double Cy = 0.0;double Dx = x2 - (r2 * cos(AQ1 + BQ2));double Dy = 0.0;if (x1 < x2) {Dx = x2 - (r2 * cos(AQ1 + BQ2));if (y1 < y2) {Cy = y1 + (r1 * sin(AQ1 + AQ2));Dy = y2 - (r2 * sin(AQ1 + BQ2));}else{Cy = y1 - (r1 * sin(AQ1 + AQ2));Dy = y2 + (r2 * sin(AQ1 + BQ2));}}else{Cy = y1 + (r1 * sin(AQ1 + AQ2));if (y1 < y2) {Dy = y2 - (r2 * sin(AQ1 + BQ2));}else{Dy = y2 + (r2 * sin(AQ1 + BQ2));}}double Cc = sqrt(pow(Cx - x3, 2) + pow(Cy - y3, 2));double Dc = sqrt(pow(Dx - x3, 2) + pow(Dy - y3, 2));return Cc < Dc ? CGPointMake(Cx, Cy) : CGPointMake(Dx, Dy);}else{//两圆无相交点return [self midpointCalculationWith:x1 :y1 :r1:x2 :y2 :r2];}
}
重新按照这个逻辑写
思路
仍然是从三个点之中取出两个点,求这两个点之间的参考点,两两之间求,求三次得到三个参考点再取重心;原代码中的两圆相离或相切部分没有问题,不再赘述,主要关注两圆相交求参考点的问题;
其中有两个子问题:
1:求两圆的交点
参考这篇:求解两圆相交的交点坐标
利用其中的方法二,可以避免分母为零的情况;由两圆的交点可以得到两圆之间交点所连的线段的公式(两点式);
2:求线段上一点到线段外一点距离最小
这个可以把线段转换为一元参数式求解,得到的二次方程可以通过一元二次公式求解析解的;
代码
这个函数需要被调用三次的,每次求的是两个已知点之间的参考点;
//C语言函数 输入三个已知点的坐标以及两个圆的半径
struct ref_p Find_Position(double x1,double x2,double x3,double y1,double y2,double y3,double r1,double r2)
{//sqrt(X)是X开根号 pow(X,n)是X的n次方struct ref_p tmp;double AB = sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2));//两个圆心之间的距离
// r1=r1>AB?AB:r1;
// r2=r2>AB?AB:r2;double rAB = (r1 + r2);
// if (rAB > AB && (r1 < AB && r2 < AB)) // 如果一个圆半径大于两个圆心的距离则认为相离,这使得更大半径圆的信任程度降低if (rAB > AB && (r1 < (AB+r2) && r2 < (AB+r1))) //如果两圆相交{ double a=(pow(r1,2)-pow(r2,2)+pow(AB,2))/(2*AB); //求两个圆的交点坐标double x0=x1+(x2-x1)*a/AB;double y0=y1+(y2-y1)*a/AB;double hdr=sqrt(pow(r1,2)-pow(a,2))/AB; //表示h和d的比值ratedouble Cx=x0-hdr*(y2-y1);// 两圆交点坐标double Cy=y0+hdr*(x2-x1);double Dx=x0+hdr*(y2-y1);double Dy=y0-hdr*(x2-x1);double t=-((Dx-Cx)*(Cx-x3)+(Dy-Cy)*(Cy-y3))/(pow(Dx-Cx,2)+pow(Dy-Cy,2));//根据一元二次方程解析解的求解公式列出t=t<=0?0:(t>=1?1:t); //为了保证所求点在线段上,t=0或1时为线段的两个端点,t超过了0,1之间的话代表在线段之外,就让其变成端点printf("t=%f\n",t);tmp.ref_x = Cx+t*(Dx-Cx); //通过参数方程的t求参考点tmp.ref_y = Cy+t*(Dy-Cy);return tmp;}else{//两圆相切或相离double a = y1 - y2;//竖边double b = x1 - x2;//横边double rr = r1 + r2;double s = r1 / rr;double x = fabs(x1 - (b * s)) ;double y = fabs(y1 - (a * s)) ;tmp.ref_x = x;tmp.ref_y = y;return tmp;}
}
这样不用担心分母为零的情况,以及判断两圆的相对位置决定正负号什么的,还可以求出比较准确的解析解,代码短一些;
ps:第一次写,写的不好也没啥营养,,,欢迎大家批评指正~
三边测量定位算法C语言实现相关推荐
- java 三点定位_Trilateration三边测量定位算法
Trilateration(三边测量)是一种常用的定位算法 已知三点位置 (x1, y1), (x2, y2), (x3, y3) 已知未知点 (x0, y0) 到三点距离 d1, d2, d3 以 ...
- rssi定位算法 c语言,【论文※】An RSSI Gradient-based AP Localization Algorithm 基于RSSI梯度的AP定位算法...
摘要:Recent rapid rise of indoor location based services for smartphones has further increased the imp ...
- 质心定位算法C语言实现,TOA定位算法.pdf
TOA定位算法 第32卷第12期 仪 器仪 表 学报 VoL32No.12 Chinese ofScientificInstrument 2011年12月 Journal Dee.20ll 测距误差分 ...
- 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 下载次 ...
- 三角定位matlab,基于RSSI校正的三角形质心定位算法
%三边测量的定位算法 %dA,dB,dC为A,B,C到未知节点(假定坐标[x,y]未知)的模拟测量距离 function [P] = Triangle(A,B,C,dA,dB,dC) %A,B,C为三 ...
- 无线传感器网络定位算法
无线传感器网络定位算法 在此,需要强调无线传感器网络节点的自组织性.为避免人为配置节点坐标产生的误差干扰,及考虑到多数网络部署区域是人类不可达区域的情况,通常情况下选择参考节点的随机部署方式.在这种情 ...
- (转载)基于TDOA声源定位算法仿真–MATLAB仿真
(转载)基于TDOA声源定位算法仿真–MATLAB仿真 转载自:https://blog.xxcxw.cn/archives/28 声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUS ...
- matlab 声源定位csdn_[转]基于TDOA声源定位算法仿真--MATLAB仿真
声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUSIC和DOA算法并不适用该场景,本仿真主要用TDOA算法进行定位. 常用的阵列信号定位算法主要有三大类:基于高分辨率谱估计的定位技术 ...
- 基于TDOA声源定位算法仿真–MATLAB仿真
评论或私信 5元获取解压码 声源定位算法是利用麦克风阵列进行声音定位,属于宽带信号,传统的MUSIC和DOA算法并不适用该场景,本仿真主要用TDOA算法进行定位. 常用的阵列信号定位算法主要有三大类: ...
- aoa定位算法matlab仿真,基于信号到达角度(AOA)的定位算法研究
内容摘要:基于信号到达角度(AOA)的定位算法是一种常见的无线传感器网络节点自定位算法,算法通信开销低,定位精度较高.由于各种原因,估测的多个节点位置可能存在不可靠位置,提出了一种改进的基于信号到达角 ...
最新文章
- java培训机构_java编程软件培训机构
- W10中给文件夹添加备注或者注释
- pythonc语言结构_C语言结构体笔记
- python strip()方法 去除字符串头尾指定字符 默认为空格、换行符、制表符、回车符
- 蓝牙耳机测试用例_移动端测试用例设计总结,这些你得懂!
- 信息类产品检测培训讲义-EN55024
- 面向对象中构造函数的小练习
- Ubuntu16.04--code::blocks16.01 的代码拷贝到Windows 8.1--visual studio 2013 注意事项
- 剑指 Offer59-I-滑动窗口的最大值
- 内网穿透工具_utools让你3步搭建一个内网穿透工具
- python sftp连接_python 进行ftp服务器和sftp服务器连接
- 一文了解元宇宙最新发展
- Binary XML file line #6: Error inflating class xxx
- html5中加入视频格式,HTML5音视频格式video和audio
- jedate选中日期后关闭弹层_jeDate 插件修改日期或时间后无法触发change事件。
- 5月书讯 | 哺育小平邦彦、伊藤清等一代数学家的“数学圣经”终于来啦!
- ubuntu 系统 下载GCC FreeType
- 傅里叶变换进行图像变换Matlab实现
- 磁共振成像(MRI)
- python 数组去重的方法