RSSI 平面 三點定位算法（C語言、JS源碼例程）

目錄

• 前言
  • 安卓app實例
• 圖示
• 公式
  • 公式推導
• 源碼
  • C語言
  • JavaScript
• 測試
  • 測試代碼
  • 不平行xy軸3參考點，隨機生成交點p(x,y)
    • 示例圖
    • 測試數據全為整數
    • 3參考點改為小數
    • 參考點和交點都為小數情況
  • 3參考點構成的三角形其一邊平行x或y軸，隨機生成交點p(x,y)
    • 示例圖
    • 一條邊平行於x軸，交點隨機生成
    • 交點位置位於 2參考點構成的平行於x軸的同一直線上
  • 3參考點構成的三角形其兩邊分別平行xy軸，隨機生成交點p(x,y)
    • 示例圖
• 補充實例
  • 代碼優化
  • 三維測試

前言

本來還想着網上找個現成的直接用。。。沒想到居然轉了轉全是錯的，有的圖畫着三點+三邊定位第四點，代碼實現的三點求三角形重心還是垂直平分線的交點來着，總之都用不了😓。不過算法本身是沒有問題的，那么以下就是我根據公式自己計算出的第四點坐標。
☀️前提條件：

1. 三個參考點坐標已知
2. 三個參考點和第四點的距離已知
3. 三個參考點連成的三角形其兩邊（直角三角形的直角邊）不能平行於xy坐標軸
4. 三個參考點連成的三角形其一邊平行於xy坐標軸時，只有極少情況可以適用（具體有待數學大佬分析）

補充RSSI測距算法（下圖轉自：https://blog.csdn.net/u011958166/article/details/106410888）：

安卓app實例

Cordova 使用 cordova-plugin-ble-central 藍牙插件，實現藍牙設備持續掃描，打印RSSI等信息 RSSI三點定位 demo
apk下載：碼雲倉庫
V2.2版本效果圖

圖示

理想情況，rssi算出的距離剛好三個構成的圓交於一點。

普遍情況1，三個圓交出一個范圍

普遍情況2，三個圓不想交

普遍情況3，腳踏兩條船

情況4，我套你猴子

情況5，孤立

情況6，拉手手

情況7，相切三角陣

情況8，三代人

情況9，箭靶

情況10，蒼ying

差不多了，有空接着更

公式

圖片轉自：https://blog.csdn.net/Luuunatic/article/details/108100569
基於理想情況，三圓交於一點

公式推導

左右平方2

去括號

同理b也可以求出

a式-b式，去除x^2 和 y^2，得到二元一次方程式

移項得

同除以 x或y的乘項，得到 x+某y 或 某x+y


同理推出 a式-c式


再使用 （a式-b式）- （a式-c式），減掉對應的x 或 y，從而進行求解 y 或 x


再提出 x 或 y的乘項除掉

LaTEX公式：\frac{x*(xb-xa)}{yb-ya}-\frac{x*(xc-xa)}{yc-ya}=\frac{da^2-db^2-xa^2-ya^2+xb^2+yb^2}{2*(yb-ya)}-\frac{da^2-dc^2-xa^2-ya^2+xc^2+yc^2}{2*(yc-ya)}

簡化就不做了，因為程序實現已經到此為止了。此時我們就可以求出x和y了，再算出 （a式-b式）- （b式-c式）和 （a式-c式）- （b式-c式）的x和y，一共三組 x 和 y，代碼中為temp_x[i]和temp_y[i]。
❗❗❗需要注意的是除數為0的情況那么公式就不在成立，所以 三個參考點連成的三角形其兩邊（直角三角形的直角邊）不能平行於xy坐標軸！！！

for(i = 0; i < 3; i++)
{
	j = (i + 1) > 2 ? 2 : (i + 1);
	k = k > 1 ? 0 : k;
	if(x_divide_y[k] - x_divide_y[j] != 0)
	{
		temp_x[i] = (dxyy[k] - dxyy[j]) / (x_divide_y[k] - x_divide_y[j]);
		temp_y[i] = (dxyx[k] - dxyx[j]) / (y_divide_x[k] - y_divide_x[j]);
	}
	else
	{
		temp_x[i] = 0;
		temp_y[i] = 0;
	}

	// printf("temp_x[%d]:%lf, temp_y:%lf\n", i, temp_x[i], temp_y[i]); 
}

最后再對三組數據求平均得出最終結果 x 和 y。

x = (temp_x[0] + temp_x[1] + temp_x[2]) / 3;
y = (temp_y[0] + temp_y[1] + temp_y[2]) / 3;

源碼

C語言

#include <stdio.h>
#include <conio.h>
#include <math.h>

int main(void)
{
	/*
		說明：參考的三點坐標及距離位置點的距離。
		不適用情況：三個參考點連成的三角形其兩邊（直角三角形的直角邊）不能平行於xy坐標軸，例如p1(0,0),p2(3,0),p3(0,4),交點(3,4)
		測試數據：p1(0,0),p2(3,4),p3(6,0),交點(6,8)
	*/
	double ref_x[3] = {0, 3, 6};
	double ref_y[3] = {0, 4, 0};
	double ref_d[3] = {4, 3, 4};
	// 計算出的三組 (d[i]方-d[j]方-x[i]方+y[j]方+x[j]方-y[i]方)/(2*(x[j]-x[i]))
	double dxyx[3];
	// 計算出的三組 (d[i]方-d[j]方-x[i]方+y[j]方+x[j]方-y[i]方)/(2*(y[j]-y[i]))
	double dxyy[3];
	// 計算出的三組 (x[i]-x[j])/(y[i]-y[j])
	double x_divide_y[3];
	// 計算出的三組 (y[i]-y[j])/(x[i]-x[j])
	double y_divide_x[3];
	// 計算出的三組x y坐標
	double temp_x[3], temp_y[3];
	// 平均x y坐標
	double x = 0, y = 0;
	int i = 0, j = 0, k = 0;

	for(i = 0; i < 3; i++)
	{
		printf("p[%d](%lf, %lf), dis=%lf\n", i, ref_x[i], ref_y[i], ref_d[i]);

		j = (i + 1) > 2 ? 2 : (i + 1);
		k = k > 1 ? 0 : k;

		// printf("i=%d,j=%d,k=%d\n", i , j , k);

		// printf("numerator:%lf\n", (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]));

		if(ref_x[j] - ref_x[k] != 0) 
			dxyx[i] = (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]) / 2 /(ref_x[j] - ref_x[k]);
		else
			dxyx[i] = 0;

		if(ref_y[j] - ref_y[k] != 0) 
			dxyy[i] = (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]) / 2 /(ref_y[j] - ref_y[k]);
		else
			dxyy[i] = 0;

		if(ref_y[j] - ref_y[k] != 0)
			x_divide_y[i] = (ref_x[j] - ref_x[k]) / (ref_y[j] - ref_y[k]);
		else
			x_divide_y[i] = 0;

		if(ref_x[j] - ref_x[k] != 0)
			y_divide_x[i] = (ref_y[j] - ref_y[k]) / (ref_x[j] - ref_x[k]);
		else
			y_divide_x[i] = 0;

		// printf("dxyx[%d]:%lf, dxyy[%d]:%lf, ", i, dxyx[i], i, dxyy[i]);
		// printf("x_divide_y[%d]:%lf, y_divide_x[%d]:%lf\n", i, x_divide_y[i], i, y_divide_x[i]);

		k++;
	}

	j = 0;
	k = 0;
	for(i = 0; i < 3; i++)
	{
		j = (i + 1) > 2 ? 2 : (i + 1);
		k = k > 1 ? 0 : k;
		if(x_divide_y[k] - x_divide_y[j] != 0)
		{
			temp_x[i] = (dxyy[k] - dxyy[j]) / (x_divide_y[k] - x_divide_y[j]);
			temp_y[i] = (dxyx[k] - dxyx[j]) / (y_divide_x[k] - y_divide_x[j]);
		}
		else
		{
			temp_x[i] = 0;
			temp_y[i] = 0;
		}

		// printf("temp_x[%d]:%lf, temp_y[%d]:%lf\n", i, temp_x[i], i, temp_y[i]); 
	}
	
	x = (temp_x[0] + temp_x[1] + temp_x[2]) / 3;
	y = (temp_y[0] + temp_y[1] + temp_y[2]) / 3;

	printf("\n[ x:%lf, y:%lf ]\n", x, y); 
	
	getchar();

    return 0;
}

JavaScript

// 三點定位函數，分別傳入 參考點a、b、c的x、y坐標、待測點與參考點a的距離
function threePointLocation(ax, ay, ad, bx, by, bd, cx, cy, cd)
{
	/*
		說明：參考的三點坐標及距離位置點的距離。
		不適用情況：三個參考點連成的三角形其兩邊（直角三角形的直角邊）不能平行於xy坐標軸，例如p1(0,0),p2(3,0),p3(0,4),交點(3,4)
		測試數據：p1(0,0),p2(3,4),p3(6,0),交點(6,8)
	*/
	//var ref_x = [0, 3, 6];
	//var ref_y = [0, 4, 0];
	//var ref_d = [4, 3, 4];
	var ref_x = [];
	var ref_y = [];
	var ref_d = [];
	// 計算出的三組 (d[i]方-d[j]方-x[i]方+y[j]方+x[j]方-y[i]方)/(2*(x[j]-x[i]))
	var dxyx = [];
	// 計算出的三組 (d[i]方-d[j]方-x[i]方+y[j]方+x[j]方-y[i]方)/(2*(y[j]-y[i]))
	var dxyy = [];
	// 計算出的三組 (x[i]-x[j])/(y[i]-y[j])
	var x_divide_y = [];
	// 計算出的三組 (y[i]-y[j])/(x[i]-x[j])
	var y_divide_x = [];
	// 計算出的三組x y坐標
	var temp_x = [], temp_y = [];
	// 平均x y坐標
	var x = 0, y = 0;
	var i = 0, j = 0, k = 0;
	// 存儲交點p坐標
	var p = JSON.parse("{\"x\": 0, \"y\": 0}");
	
	// 初始化數據
	ref_x.push(ax, bx, cx);
	ref_y.push(ay, by, cy);
	ref_d.push(ad, bd, cd);

	for(i = 0; i < 3; i++)
	{
		//console.log("p[" + i +"](" + ref_x[i] + ", " + ref_y[i] + "), dis=" + ref_d[i] + "\n");

		j = (i + 1) > 2 ? 2 : (i + 1);
		k = k > 1 ? 0 : k;
		
		//console.log("numerator:" + (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]));


		if(ref_x[j] - ref_x[k] != 0) 
			dxyx[i] = (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]) / 2 /(ref_x[j] - ref_x[k]);
		else
			dxyx[i] = 0;

		if(ref_y[j] - ref_y[k] != 0) 
			dxyy[i] = (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]) / 2 /(ref_y[j] - ref_y[k]);
		else
			dxyy[i] = 0;

		if(ref_y[j] - ref_y[k] != 0)
			x_divide_y[i] = (ref_x[j] - ref_x[k]) / (ref_y[j] - ref_y[k]);
		else
			x_divide_y[i] = 0;

		if(ref_x[j] - ref_x[k] != 0)
			y_divide_x[i] = (ref_y[j] - ref_y[k]) / (ref_x[j] - ref_x[k]);
		else
			y_divide_x[i] = 0;
			
		//console.log("dxyx[" + i + "]:" + dxyx[i] + ", dxyy[" + i + "]:" + dxyy[i]);
		//console.log("x_divide_y[" + i + "]:" + x_divide_y[i] + ", y_divide_x[" + i + "]:" + y_divide_x[i]);

		k++;
	}

	j = 0;
	k = 0;
	for(i = 0; i < 3; i++)
	{
		j = (i + 1) > 2 ? 2 : (i + 1);
		k = k > 1 ? 0 : k;
		if(x_divide_y[k] - x_divide_y[j] != 0)
		{
			temp_x[i] = (dxyy[k] - dxyy[j]) / (x_divide_y[k] - x_divide_y[j]);
			temp_y[i] = (dxyx[k] - dxyx[j]) / (y_divide_x[k] - y_divide_x[j]);
		}
		else
		{
			temp_x[i] = 0;
			temp_y[i] = 0;
		}
	}

	x = (temp_x[0] + temp_x[1] + temp_x[2]) / 3;
	y = (temp_y[0] + temp_y[1] + temp_y[2]) / 3;

	// console.log("\n[ x:" + x + ", y:" + y + " ]\n"); 
	
	p.x = x;
	p.y = y;
	
	return p;
}

測試

測試代碼

#include <stdio.h>
#include <conio.h>
#include <math.h>
#include <stdlib.h>
#include <time.h>

// 三點定位
void three_point_location(double x1, double y1, double d1, double x2, double y2, double d2, double x3, double y3, double d3)
{
	/*
		說明：參考的三點坐標及距離位置點的距離。
		不適用情況：三個參考點連成的三角形其兩邊（直角三角形的直角邊）不能平行於xy坐標軸，例如p1(0,0),p2(3,0),p3(0,4),交點(3,4)
		測試數據：p1(0,0),p2(3,4),p3(6,0),交點(6,8)
	*/
	double ref_x[3] = {x1, x2, x3};
	double ref_y[3] = {y1, y2, y3};
	double ref_d[3] = {d1, d2, d3};
	// 計算出的三組 (d[i]方-d[j]方-x[i]方+y[j]方+x[j]方-y[i]方)/(2*(x[j]-x[i]))
	double dxyx[3];
	// 計算出的三組 (d[i]方-d[j]方-x[i]方+y[j]方+x[j]方-y[i]方)/(2*(y[j]-y[i]))
	double dxyy[3];
	// 計算出的三組 (x[i]-x[j])/(y[i]-y[j])
	double x_divide_y[3];
	// 計算出的三組 (y[i]-y[j])/(x[i]-x[j])
	double y_divide_x[3];
	// 計算出的三組x y坐標
	double temp_x[3], temp_y[3];
	// 平均x y坐標
	double x = 0, y = 0;
	int i = 0, j = 0, k = 0;

	for(i = 0; i < 3; i++)
	{
		// printf("p[%d](%lf, %lf), dis=%lf\n", i, ref_x[i], ref_y[i], ref_d[i]);

		j = (i + 1) > 2 ? 2 : (i + 1);
		k = k > 1 ? 0 : k;

		// printf("i=%d,j=%d,k=%d\n", i , j , k);

		// printf("numerator:%lf\n", (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]));

		if(ref_x[j] - ref_x[k] != 0) 
			dxyx[i] = (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]) / 2 /(ref_x[j] - ref_x[k]);
		else
			dxyx[i] = 0;

		if(ref_y[j] - ref_y[k] != 0) 
			dxyy[i] = (ref_d[k] * ref_d[k] - ref_d[j] * ref_d[j] - ref_x[k] * ref_x[k] + ref_y[j] * ref_y[j] + ref_x[j] * ref_x[j] - ref_y[k] * ref_y[k]) / 2 /(ref_y[j] - ref_y[k]);
		else
			dxyy[i] = 0;

		if(ref_y[j] - ref_y[k] != 0)
			x_divide_y[i] = (ref_x[j] - ref_x[k]) / (ref_y[j] - ref_y[k]);
		else
			x_divide_y[i] = 0;

		if(ref_x[j] - ref_x[k] != 0)
			y_divide_x[i] = (ref_y[j] - ref_y[k]) / (ref_x[j] - ref_x[k]);
		else
			y_divide_x[i] = 0;

		// printf("dxyx[%d]:%lf, dxyy[%d]:%lf, ", i, dxyx[i], i, dxyy[i]);
		// printf("x_divide_y[%d]:%lf, y_divide_x[%d]:%lf\n", i, x_divide_y[i], i, y_divide_x[i]);

		k++;
	}

	j = 0;
	k = 0;
	for(i = 0; i < 3; i++)
	{
		j = (i + 1) > 2 ? 2 : (i + 1);
		k = k > 1 ? 0 : k;
		if(x_divide_y[k] - x_divide_y[j] != 0)
		{
			temp_x[i] = (dxyy[k] - dxyy[j]) / (x_divide_y[k] - x_divide_y[j]);
			temp_y[i] = (dxyx[k] - dxyx[j]) / (y_divide_x[k] - y_divide_x[j]);
		}
		else
		{
			temp_x[i] = 0;
			temp_y[i] = 0;
		}

		// printf("temp_x[%d]:%lf, temp_y[%d]:%lf\n", i, temp_x[i], i, temp_y[i]); 
	}
	
	x = (temp_x[0] + temp_x[1] + temp_x[2]) / 3;
	y = (temp_y[0] + temp_y[1] + temp_y[2]) / 3;

	printf("ret point( x:%lf, y:%lf )\n", x, y);
}

int main(void)
{
	srand((unsigned int)time(NULL));
	int i = 0, j = 0;
	double x = 0, y = 0;
	double ref_x[3] = {0, 30, 60};
	double ref_y[3] = {0, 70, 40};
	double ref_d[3] = {0, 0, 0};
	for(i=0; i<100; i++)
	{
		x = rand() % 100;
		y = rand() % 100;
		printf("real point( %lf , %lf ) ,", x, y);
		for(j=0; j<3; j++)
			ref_d[j] = sqrt( pow((x - ref_x[j]), 2) + pow((y - ref_y[j]), 2) );
		three_point_location(
			ref_x[0], ref_y[0], ref_d[0],
			ref_x[1], ref_y[1], ref_d[1],
			ref_x[2], ref_y[2], ref_d[2]
		);
	}
	
	getchar();

    return 0;
}

不平行xy軸3參考點，隨機生成交點p(x,y)

示例圖

測試數據全為整數

整數進行的測試，3參考點信息如下：

double ref_x[3] = {0, 30, 60};
double ref_y[3] = {0, 70, 40};
double ref_d[3] = {0, 0, 0};

效果如下：

3參考點改為小數

效果如下：

參考點和交點都為小數情況

3參考點構成的三角形其一邊平行x或y軸，隨機生成交點p(x,y)

示例圖

一條邊平行於x軸，交點隨機生成

此時，參考點重合於x軸，可以發現只有y=0時，其結果才能匹配，其他值都是接近，或是有所偏差。

當我們將自設的參考點和交點都進行+1操作時，其結果也有偏差。
❗❗❗這便說明一個問題，即使在2參考點構成直線平行於x或y軸的情況，交點即使在參考點構成的直線上，也不能保證算法正確。重合於xy軸是特殊情況！

微調參考點坐標后，又可正常運算

交點位置位於 2參考點構成的平行於x軸的同一直線上

計算全部正確。

3參考點構成的三角形其兩邊分別平行xy軸，隨機生成交點p(x,y)

示例圖

20組數據，無一幸免

補充實例

代碼優化

通過先篩選不平行於xy軸的三個參考點

// 判斷三個參考點的x或y相減的所有組合是否至少有一組平行於x或y軸，傳入三個下標。是則返回1，否則返回0
function isParallelXorY(a, b, c) {
    if(x[a] - x[b] == 0 || x[a] - x[c] == 0 || x[b] - x[c] == 0 || 
        y[a] - y[b] == 0 || y[a] - y[c] == 0 || y[b] - y[c] == 0)
        return 1;
    else
        return 0;
}

配合組合算法，算出所有信號范圍內的參考點的所有3組合

// 組合算法參考：https://blog.csdn.net/nanfeibuyi/article/details/79561783
var i = 0, j = 0, t = 0;
var m = 3, n = index;

//用來存儲每次算法產生的當前組合
var a = [0, 0, 0];
//第一種組合，a[0]=1,a[1]=2,...a[m-1]=m;
for(i=0; i<m; i++)
{
    a[i] = i+1;
}

//當組合為最后一組時，循環結束；即a[0]=n-m+1,...,a[m-1]=n;j用來判斷進位，以及進位之后的調整
for(j=m; a[0]<=(n-m+1); )
{
    //最后一位不斷遞增，直到達到最大值，產生進位
    for(; a[m-1]<=n; a[m-1]++)
    {
        // 將組合下標傳入判斷函數，返回0則表示此組合的三個參考點可以做為定位算法使用
        if(0 == isParallelXorY((a[0]-1), (a[1]-1), (a[2]-1)))
        {
            var temp_p = threePointLocation(x[(a[0]-1)], y[(a[0]-1)], d[(a[0]-1)], 
                x[(a[1]-1)], y[(a[1]-1)], d[(a[1]-1)], x[(a[2]-1)], y[(a[2]-1)], d[(a[2]-1)]);
            p.push(temp_p);
            // log("temp_p:(" + temp_p.x + "," + temp_p.y + ")", "log");
        }
    }
    
    //判斷a[1]--a[m-2]是否有進位 如果 a[m-1]>n 產生進位
    for(j=m-2; j>=0; j--)
    {
        a[j]++;
        //a[j]不進位,那么a[j-1]也不進位，結束繼續判斷
        if(a[j] <= (j+n-m+1))
        {
            break;
        }
    }
    
    //調整，使得a[index-1],a[index],a[index]順序排列，其中a[index]產生進位
    for(j++; j>0 && j<m; j++)
    {
        a[j] = a[j-1]+1;
    }
}

最后再對所有符合條件后算出的坐標集求平均值，得到坐標點

// 存儲總和、平均x/y
var sum_x = 0, sum_y = 0, avg_x = 0, avg_y = 0;
for(i=0; i<p.length; i++)
{
    sum_x += p[i].x;
    sum_y += p[i].y;
}
// 計算平均值
if(p.length > 0) {
    avg_x = sum_x / p.length;
    avg_y = sum_y / p.length;

    document.getElementById("coordinate").innerHTML = "坐標：（ "+ avg_x.toFixed(4) + "，" + avg_y.toFixed(4) + "）";
}
else {
    document.getElementById("coordinate").innerHTML = "坐標：（?，?）";
}

三維測試

好像三維中定位二維坐標也勉強能用（有待大佬解釋了0.0）

求三維坐標有待研究