多边定位算法用c语言实现,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_Loc