定位算法-三边测距定位

最新推荐文章于 2025-03-21 10:14:43 发布

姜小明同学

最新推荐文章于 2025-03-21 10:14:43 发布

阅读量1.8w

点赞数 11

分类专栏：定位算法

本文链接：https://blog.csdn.net/jjmjim/article/details/108445758

版权

定位算法专栏收录该内容

2 篇文章

订阅专栏

作者：姜小明 @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=b
    gsl_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);
    
    // result
	gsl_vector *sx = gsl_vector_alloc(coefficient_num);

    //Clear data
    gsl_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_num
    gsl_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 number
    printf("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;
}