定位算法-三边测距定位

article/2025/11/10 10:40:31

作者：姜小明 @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;
}