一、三邊定位位置解析
通過對已知的三個基站的坐標和三個基站分別到未知點的距離來確定，未知點的坐標。
注：這個解析算法代碼是通過對Decawave公司的開源代碼剪切更改而來的。
代碼：

// ------------------------------------------------------------------------------------------------------------------- // // File: trilateration.cpp // // Copyright 2016 (c) Decawave Ltd, Dublin, Ireland. // // All rights reserved. // // Author: // // -------------------------------------------------------------------------------------------------------------------#include <stdio.h> #include <string.h> #include <math.h> //#include <stdlib.h> #include <math.h>struct point_t {double x, y; };struct circle_t {struct point_t center;double r; };int double_equals(double a, double b) {static const double ZERO = 1e-9;return fabs(a - b) < ZERO; }double distance_sqr(struct point_t* a, struct point_t* b) {return (a->x - b->x) * (a->x - b->x) + (a->y - b->y) * (a->y - b->y); }double distance(struct point_t* a, struct point_t* b) {return sqrt(distance_sqr(a, b)); }int insect(struct circle_t circles[], struct point_t points[]) {double d, a, b, c, p, q, r;double cos_value[2], sin_value[2];if (double_equals(circles[0].center.x, circles[1].center.x)&& double_equals(circles[0].center.y, circles[1].center.y)&& double_equals(circles[0].r, circles[1].r)){return -1;}d = distance(&circles[0].center, &circles[1].center);if (d > circles[0].r + circles[1].r|| d < fabs(circles[0].r - circles[1].r)){return 0;}a = 2.0 * circles[0].r * (circles[0].center.x - circles[1].center.x);b = 2.0 * circles[0].r * (circles[0].center.y - circles[1].center.y);c = circles[1].r * circles[1].r - circles[0].r * circles[0].r- distance_sqr(&circles[0].center, &circles[1].center);p = a * a + b * b;q = -2.0 * a * c;if (double_equals(d, circles[0].r + circles[1].r)|| double_equals(d, fabs(circles[0].r - circles[1].r))){cos_value[0] = -q / p / 2.0;sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);points[0].x = circles[0].r * cos_value[0] + circles[0].center.x;points[0].y = circles[0].r * sin_value[0] + circles[0].center.y;if (!double_equals(distance_sqr(&points[0], &circles[1].center),circles[1].r * circles[1].r)){points[0].y = circles[0].center.y - circles[0].r * sin_value[0];}return 1;}r = c * c - b * b;cos_value[0] = (sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;cos_value[1] = (-sqrt(q * q - 4.0 * p * r) - q) / p / 2.0;sin_value[0] = sqrt(1 - cos_value[0] * cos_value[0]);sin_value[1] = sqrt(1 - cos_value[1] * cos_value[1]);points[0].x = circles[0].r * cos_value[0] + circles[0].center.x;points[1].x = circles[0].r * cos_value[1] + circles[0].center.x;points[0].y = circles[0].r * sin_value[0] + circles[0].center.y;points[1].y = circles[0].r * sin_value[1] + circles[0].center.y;if (!double_equals(distance_sqr(&points[0], &circles[1].center),circles[1].r * circles[1].r)){points[0].y = circles[0].center.y - circles[0].r * sin_value[0];}if (!double_equals(distance_sqr(&points[1], &circles[1].center),circles[1].r * circles[1].r)){points[1].y = circles[0].center.y - circles[0].r * sin_value[1];}if (double_equals(points[0].y, points[1].y)&& double_equals(points[0].x, points[1].x)){if (points[0].y > 0){points[1].y = -points[1].y;}else{points[0].y = -points[0].y;}}return 2; }void Cross_Point(struct circle_t circles[], struct point_t Location[]) {int cross_num = 5;struct point_t cross_points[2];cross_num = insect(circles, cross_points);// 0 1if (cross_num == 2){double points_AC_0 = distance(&cross_points[0], &circles[2].center);double points_AC_1 = distance(&cross_points[1], &circles[2].center);if (abs((int)(points_AC_0 - circles[2].r) )< abs((int)(points_AC_1 - circles[2].r)))//cross_point[0]{Location[0].x = cross_points[0].x;Location[0].y = cross_points[0].y;}else{Location[0].x = cross_points[1].x;Location[0].y = cross_points[1].y;}}else if (cross_num == 1 || cross_num == 0){Location[0].x = cross_points[0].x;Location[0].y = cross_points[0].y;} }struct point {float x, y; };float norm(struct point p) // get the norm of a vector 求向量的范數(shù) {return pow(pow(p.x, 2) + pow(p.y, 2), .5); }void trilateration_1(struct point point1, struct point point2, struct point point3, double r1, double r2, double r3) {struct point resultPose;//unit vector in a direction from point1 to point 2 從點1到點2方向上的單位向量double p2p1Distance = pow(pow(point2.x - point1.x, 2) + pow(point2.y - point1.y, 2), 0.5);struct point ex = { (point2.x - point1.x) / p2p1Distance, (point2.y - point1.y) / p2p1Distance };struct point aux = { point3.x - point1.x,point3.y - point1.y };//signed magnitude of the x component x分量的符號大小double i = ex.x * aux.x + ex.y * aux.y;//the unit vector in the y direction. y方向的單位向量。Tstruct point aux2 = { point3.x - point1.x - i * ex.x, point3.y - point1.y - i * ex.y };struct point ey = { aux2.x / norm(aux2), aux2.y / norm(aux2) };//the signed magnitude of the y component y分量的符號大小double j = ey.x * aux.x + ey.y * aux.y;//coordinates 協(xié)調(diào)double x = (pow(r1, 2) - pow(r2, 2) + pow(p2p1Distance, 2)) / (2 * p2p1Distance);double y = (pow(r1, 2) - pow(r3, 2) + pow(i, 2) + pow(j, 2)) / (2 * j) - i * x / j;//result coordinates 結(jié)果坐標double finalX = point1.x + x * ex.x + y * ey.x;double finalY = point1.y + x * ex.y + y * ey.y;resultPose.x = finalX;resultPose.y = finalY;printf("TAG LOC My1:x = %3.2f,y = %3.2f\r\n", resultPose.x, resultPose.y);//打印未知點坐標}int main() {struct point points[3];//基站1points[0].x= 0;points[0].y = 0;double r1 = 14.2; //基站距離未知點的距離//基站2points[1].x = 20;points[1].y = 0;double r2=14.5;//基站距離未知點的距離//基站3points[2].x = 0;points[2].y = 10;double r3 = 10;//基站距離未知點的距離trilateration_1(points[0], points[1], points[2], r1, r2, r3);return 0; }

運行結(jié)果：

TAG LOC My1:x = 9.78,y = 10.08

總結(jié)

以上是生活随笔為你收集整理的三边定位_位置解析(C++)的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。