RSSI 平面三点定位算法（C语言JS源码例程）

Posted 2021-10-17 Love丶伊卡洛斯

tags:

篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了RSSI 平面三点定位算法（C语言JS源码例程）相关的知识，希望对你有一定的参考价值。

前言

本来还想着网上找个现成的直接用。。。没想到居然转了转全是错的，有的图画着三点+三边定位第四点，代码实现的三点求三角形重心还是垂直平分线的交点来着，总之都用不了😓。不过算法本身是没有问题的，那么以下就是我根据公式自己计算出的第四点坐标。
☀️前提条件：

三个参考点坐标已知
三个参考点和第四点的距离已知
三个参考点连成的三角形其两边（直角三角形的直角边）不能平行于xy坐标轴
三个参考点连成的三角形其一边平行于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];
	// 计算以上是关于RSSI 平面 三点定位算法（C语言JS源码例程）的主要内容，如果未能解决你的问题，请参考以下文章 
 Cordova 使用 cordova-plugin-ble-central 蓝牙插件，实现蓝牙设备持续扫描，打印RSSI等信息 RSSI三点定位 demo
 定位问题基于matlab RSSI和模拟退火优化粒子群算法求解无线传感器网络定位问题含Matlab源码 1766期
 JavaScript三点定圆
 定位仿真基于matlab RSSI三边定位仿真含Matlab源码 1690期
 基于MATLAB的RSSI定位算法仿真
 基于RSSI定位算法的matlab仿真

RSSI 平面 三点定位算法（C语言JS源码例程）

目录

前言

安卓app实例

图示

公式

公式推导

源码

C语言

javascript

测试

测试代码

RSSI 平面三点定位算法（C语言JS源码例程）