定位算法-三边测距定位

作者：姜小明 @github

日期：2020-09-06

关键字：RANSAC, Multilateral positioning, localization

上一篇文章（《使用RANSAC的鲁棒TDOA Chan定位算法》）中提到，如果我们知道当前位置到各个基站的距离，可以很方便的确定位置。本篇文章我们简短介绍测距定位算法及其实现。

1. 三边测距定位算法简介

利用信号源到各个监测站的距离，最少通过三个监测站，我们就能确定信号的位置，该方法为三边测距定位。如下图,以监测站为中心，距离为半径作圆可以确定信号的位置：

2. 算法

2.1 算法推导

算法推导如下图（参考知乎-三边测量及多边测量）：

2.2 算法实现

算法实现如下：

bool calculate_pos_with_index_(const Position2D landmarks[], const double distances[], const int effective_landmark_indexes[], int effective_landmark_num, double* x_out, double* y_out) {if (is_landmarks_in_one_line(landmarks, effective_landmark_indexes, effective_landmark_num)) {printf("all landmarks in one line, cannot locate the position.\n");return false;}int matrix_row_num = effective_landmark_num - 1;int coefficient_num = 2;// Ax=bgsl_matrix* A = gsl_matrix_alloc(matrix_row_num, coefficient_num);gsl_matrix* a = gsl_matrix_alloc(matrix_row_num, coefficient_num);gsl_vector* b = gsl_vector_alloc(matrix_row_num);// resultgsl_vector *sx = gsl_vector_alloc(coefficient_num);//Clear datagsl_matrix_set_zero(A);gsl_vector_set_zero(b);int index_last = effective_landmark_indexes[effective_landmark_num - 1];for (int i = 0; i < effective_landmark_num - 1; i++) {int index_i = effective_landmark_indexes[i];gsl_matrix_set(A, i, 0, (landmarks[index_i].x - landmarks[index_last].x) * 2);gsl_matrix_set(A, i, 1, (landmarks[index_i].y - landmarks[index_last].y) * 2);double bi = landmarks[index_i].x * landmarks[index_i].x - landmarks[index_last].x * landmarks[index_last].x+ landmarks[index_i].y * landmarks[index_i].y - landmarks[index_last].y * landmarks[index_last].y+ distances[index_last] * distances[index_last] - distances[index_i] * distances[index_i];gsl_vector_set(b, i, bi);}gsl_matrix_memcpy(a, A);gsl_vector* tau = gsl_vector_alloc(coefficient_num); //matrix_row_num, coefficient_numgsl_vector* residuals = gsl_vector_alloc(matrix_row_num);gsl_multifit_linear_workspace *w = gsl_multifit_linear_alloc(matrix_row_num, coefficient_num);gsl_multifit_linear_svd(A, w);double rcond = gsl_multifit_linear_rcond(w); // reciprocal condition numberprintf("conditional number is:%f\n", 1.0 / rcond);if (rcond < 1e-4) {printf("conditional number is too large, indicating that the problem is ill-conditioned.\n");return false;}double lambda_gcv = 0.1;double chisq, rnorm, snorm;gsl_multifit_linear_solve(0.0, A, b, sx, &rnorm, &snorm, w);*x_out = gsl_vector_get(sx, 0);*y_out = gsl_vector_get(sx, 1);gsl_matrix_free(A);gsl_matrix_free(a);gsl_vector_free(b);gsl_vector_free(tau);gsl_vector_free(sx);gsl_vector_free(residuals);return true;
}

3. 项目描述

全部工程代码：github-RANSAC_Locator_using_distances

项目使用C语言实现了三边测距定位算法。和上一篇定位文章类似地，本项目使用了RANSAC思想排除异常点的干扰，获取稳定的解。main.c中包含了一段测试代码。编译方式和gsl库配置方法详见readme.md。

定位算法-三边测距定位相关推荐

室内定位算法_室外定位用卫星，室内定位用什么？
提起定位导航,大家首先想到的是北斗.GPS等定位系统. 但是,室内场景受到建筑物的遮挡,GNSS信号快速衰减,甚至完全拒止,卫星定位就无法使用. 近年来,位置服务的相关技术和产业正从室外向室内发展. ...
机器人学习--定位算法AMCL全局定位方案
ROS中已经集成了AMCL(自适应蒙特卡洛定位)的解决方案,按照要求测试一般都没问题. 源码或者相关细节分析参考:机器人学习--定位算法AMCL详解_GGY1102的博客-CSDN博客_amcl定位 ...
蒙特卡洛粒子滤波定位算法_蒙特卡罗定位算法(基于粒子滤波的定位算法) ——原理、理解与仿真...
1 算法原理 1.1 机器人定位问题关于机器人定位,有三大问题,它们分别是: (1)"全局定位":指初始位置未知,机器人靠自身运动确定自己在地图中的位姿. (2)"位姿 ...
室内定位算法_室内定位导航的精确实现需要解决哪些问题？
关于室内定位的精度,不同应用场景下的不同应用对精度的要求不同. 通常,定位精度越高,需要集成的定位算法越复杂,实现成本也越高. 对于当今市场上的室内定位应用,用于个人用户的路径查找和定位的主动定位不需 ...
基于android的考勤定位算法,基于移动定位服务的考勤管理系统
[导读] 考勤管理是企事业单位经营和管理工作的重要环节和保障,主要包括两种方式,一种是手工方式,该方式工作繁杂,很难保证数据的准确与实时,增加了管理成本;另一种是利用专用考勤设备的电脑记录考勤方式,包 ...
wifi定位算法android,WIFI定位算法
网络定位指通过周边wifi列表.基站列表.IP地址等信息确定用户位置的过程. 我们采集的训练数据是每个GPS坐标位置和对应的Wifi和基站列表. 其中每个Wifi和基站有唯一的Mac地址作为ID和信号 ...
目标定位算法（二）之基于测距的定位算法
文章目录基于测距的定位算法 1.最小二乘原理 2.最小二乘定位算法 3.基于RSSI测距的定位算法 4.基于TOA/TDOA的目标定位算法 1)基于TOA测距 2)基于TDOA测距基于测距的定位算 ...
基于RSSI室内定位算法介绍
前言基于RSSI的室内定位算法有很多,像三角/三边质心算法.加权质心算法.最小二乘法.双曲线法.位置指纹算法.融合算法以及各种改进衍生算法等等.而依据定位策略的不同,BLE信标定位算法可分为基于距离 ...
rssi室内定位算法原理_室内定位方案常用的4种定位算法
目前常见的室内定位技术有超宽带UWB室内定位技术,蓝牙室内定位技术,RFID(无线射频识别)定位,超声波定位,Wi-Fi定位等.室内定位依赖于定位算法,定位算法决定了室内定位的模式.室内定位种类虽然比 ...
基于RSSI定位算法的matlab仿真
up目录一.理论基础二.核心程序三.测试结果一.理论基础 Received Signal Strength Indication接收的信号强度指示,无线发送层的可选部分,用来判定链接质量,以及 ...