UWB 定位_三邊定位位置解析算法-C

一、用途
1、解析未知點的座標。
二、、基本原理

已知三點位置 (x1, y1), (x2, y2), (x3, y3)
已知未知點 (x0, y0) 到三點距離 d1, d2, d3
通過最小二乘法算法來解析出座標。

三、代碼
該代碼是需要輸入四個基站座標和四個基站到未知點的距離的， 如果需要改成三個 直接引用 trilateration(AX, AY, d1, BX, BY, d2, CX, CY, d3);即可。4個基站只是在三個基站的基礎上來解析的，多了一步對4種不同的三個基站的定位位置數據平均化。
.h文件代碼：

#ifndef __MY_LOCATION_H
#define __MY_LOCATION_H
	static int AX,BX,CX,DX,AY,BY,CY,DY;
	void MY_location_Init(int X1, int Y1, int X2, int Y2, int X3, int Y3,int X4,int Y4);
	double* trilateration(double x1, double y1, double d1, double x2, double y2, double d2, double x3, double y3, double d3);
	double* Printf(double d1, double d2, double d3, double d4);
	int P(int x, int n);

#endif

.c文件代碼

#include"MY_Location.h"

/*
??:?x?y??
*/
int  P(int x, int n)
{
	int  val = 1;
	while (n--)
		val *= x;
	return val;

}


void MY_location_Init(int X1, int Y1, int X2, int Y2, int X3, int Y3,int X4,int Y4)
{AX=X1; AY=Y1; BX=X2;BY=Y2; CX=X3; CY=Y3 ;DX=X4,DY=Y4;}
/*
??:??????????
???????
*/
double* trilateration(double x1, double y1, double d1, double x2, double y2, double d2, double x3, double y3, double d3)
{
	
	static double d[4] = { 0.0,0.0 };
	double a11 = 2 * (x1 - x3);
	double a12 = 2 * (y1 - y3);
	double b1 = P(x1, 2) - P(x3, 2) + P(y1, 2) - P(y3, 2) + P(d3, 2) - P(d1, 2);
	double a21 = 2 * (x2 - x3);
	double a22 = 2 * (y2 - y3);
	double b2 = P(x2, 2) - P(x3, 2) + P(y2, 2) - P(y3, 2) + P(d3, 2) - P(d2, 2);
	d[0] = (b1 * a22 - a12 * b2) / (a11 * a22 - a12 * a21);
	d[1] = (a11 * b2 - b1 * a21) / (a11 * a22 - a12 * a21);
	return d;
}


double  *Printf(double d1, double d2, double d3, double d4)
{
	double *Coordinate_A = 0, *Coordinate_B = 0, *Coordinate_C = 0, *Coordinate_D = 0;
	static double XY[2] = { 0,0 };

	Coordinate_A = trilateration(AX, AY, d1, BX, BY, d2, CX, CY, d3);

	Coordinate_B = trilateration(AX, AY, d1, BX, BY, d2, DX, DY, d4);

	Coordinate_C = trilateration(AX, AY, d1, CX, CY, d3, DX, DY, d4);

	Coordinate_D = trilateration(BX, BY, d2, CX, CY, d3,DX, DY, d4);

	XY[0] = (Coordinate_A[0] + Coordinate_B[0] + Coordinate_C[0] + Coordinate_D[0]) / 4;

	XY[1] = (Coordinate_A[1] + Coordinate_B[1] + Coordinate_C[1] + Coordinate_D[1]) / 4;
	return XY;
}

調用的時候 需要先在MY_location_Init(int X1, int Y1, int X2, int Y2, int X3, int Y3,int X4,int Y4);中輸入基站的位置座標

*可以直接複製到Stm32的代碼中使用。

四、調用
代碼：初始化基站座標

MY_location_Init(0, 0,600, 0, 0,600,600,600);//初始化基站座標

coordinate= Printf(D1,D2,D3,D4);//輸入基站分別到未知點的座標	
			printf("X%dY%d",(int)coordinate[0],(int)coordinate[1]);//輸出格式