【UWB】公式推导计算坐标值

假设基站 A0A0A0 的坐标为（x0，y1，z1x_0，y_1，z_1x0，y1，z1），基站 A1A1A1 的坐标为（x2，y2，z2x_2，y_2，z_2x2，y2，z2），基站 A2A2A2 的坐标为（x3，y3，z3x_3，y_3，z_3x3，y3，z3）,基站 A3A3A3 的坐标为（x4，y4，z4x_4，y_4，z_4x4，y4，z4）,需要求解的标签坐标为（x，y，zx，y，zx，y，z），则有：

(x−x0)2+(y−y0)2+(z−z0)2=R02(x−x1)2+(y−y1)2+(z−z1)2=R12(x−x2)2+(y−y2)2+(z−z2)2=R22(x−x3)2+(y−y3)2+(z−z3)2=R32\begin{aligned} (x - x_0)^2 + (y - y_0)^2 + (z - z_0)^2 = R_0^2 \\ (x - x_1)^2 + (y - y_1)^2 + (z - z_1)^2 = R_1^2 \\ (x - x_2)^2 + (y - y_2)^2 + (z - z_2)^2 = R_2^2 \\ (x - x_3)^2 + (y - y_3)^2 + (z - z_3)^2 = R_3^2 \\ \end{aligned}(x−x0)2+(y−y0)2+(z−z0)2=R02(x−x1)2+(y−y1)2+(z−z1)2=R12(x−x2)2+(y−y2)2+(z−z2)2=R22(x−x3)2+(y−y3)2+(z−z3)2=R32

其中，R1,R2,R3,R4R_1, R_2, R_3, R_4R1,R2,R3,R4 分别是基站测得的与标签的距离。

将这些式子展开得到：
x2−x02−2xx0+y2−y02−2yy0+z2−z02−2zz0=R02x2−x12−2xx1+y2−y12−2yy1+z2−z12−2zz1=R12x2−x22−2xx2+y2−y22−2yy2+z2−z22−2zz2=R22x2−x32−2xx3+y2−y32−2yy3+z2−z32−2zz3=R32\begin{aligned} x^2 - x_0^2 - 2 x x_0 + y^2 - y_0^2 - 2 y y_0 + z^2 - z_0^2 - 2 z z_0 = R_0^2 \\ x^2 - x_1^2 - 2 x x_1 + y^2 - y_1^2 - 2 y y_1 + z^2 - z_1^2 - 2 z z_1 = R_1^2 \\ x^2 - x_2^2 - 2 x x_2 + y^2 - y_2^2 - 2 y y_2 + z^2 - z_2^2 - 2 z z_2 = R_2^2 \\ x^2 - x_3^2 - 2 x x_3 + y^2 - y_3^2 - 2 y y_3 + z^2 - z_3^2 - 2 z z_3 = R_3^2 \\ \end{aligned}x2−x02−2xx0+y2−y02−2yy0+z2−z02−2zz0=R02x2−x12−2xx1+y2−y12−2yy1+z2−z12−2zz1=R12x2−x22−2xx2+y2−y22−2yy2+z2−z22−2zz2=R22x2−x32−2xx3+y2−y32−2yy3+z2−z32−2zz3=R32

第 2, 3, 4 行的式子各自减去第 1 行式子，得到：
2(x0−x1)x+2(y0−y1)y+2(z0−z1)z=λ12(x0−x2)x+2(y0−y2)y+2(z0−z2)z=λ22(x0−x3)x+2(y0−y3)y+2(z0−z3)z=λ3\begin{aligned} 2(x_0 - x_1) x + 2(y_0 - y_1) y + 2(z_0 - z_1) z = \lambda_1 \\ 2(x_0 - x_2) x + 2(y_0 - y_2) y + 2(z_0 - z_2) z = \lambda_2 \\ 2(x_0 - x_3) x + 2(y_0 - y_3) y + 2(z_0 - z_3) z = \lambda_3 \\ \end{aligned}2(x0−x1)x+2(y0−y1)y+2(z0−z1)z=λ12(x0−x2)x+2(y0−y2)y+2(z0−z2)z=λ22(x0−x3)x+2(y0−y3)y+2(z0−z3)z=λ3

其中，
λ1=R12−R02−x12+x02−y12+y02−z12+z02λ2=R22−R12−x22+x12−y22+y12−z22+z12λ3=R32−R22−x32+x22−y32+y22−z32+z22\begin{aligned} \lambda_1 = R_1^2 - R_0^2 - x_1^2 + x_0^2 - y_1^2 + y_0^2 - z_1^2 + z_0^2 \\ \lambda_2 = R_2^2 - R_1^2 - x_2^2 + x_1^2 - y_2^2 + y_1^2 - z_2^2 + z_1^2 \\ \lambda_3 = R_3^2 - R_2^2 - x_3^2 + x_2^2 - y_3^2 + y_2^2 - z_3^2 + z_2^2 \\ \end{aligned}λ1=R12−R02−x12+x02−y12+y02−z12+z02λ2=R22−R12−x22+x12−y22+y12−z22+z12λ3=R32−R22−x32+x22−y32+y22−z32+z22

把这些式子转换为矩阵相乘的形式（有关矩阵的知识可以百度一下，知识过多，这里不便详说）：
[2(x0−x1)2(y0−y1)2(z0−z1)2(x0−x2)2(y0−y2)2(z0−z2)2(x0−x3)2(y0−y3)2(z0−z3)][xyz]=[λ1λ2λ3]\begin{aligned} \left[\begin{matrix} 2(x_0 - x_1) & 2(y_0 - y_1) & 2(z_0 - z_1) \\ 2(x_0 - x_2) & 2(y_0 - y_2) & 2(z_0 - z_2) \\ 2(x_0 - x_3) & 2(y_0 - y_3) & 2(z_0 - z_3) \\ \end{matrix}\right] \left[\begin{matrix} x \\ y \\ z \\ \end{matrix}\right] = \left[\begin{matrix} \lambda_1 \\ \lambda_2 \\ \lambda_3 \\ \end{matrix}\right] \end{aligned}⎣⎡2(x0−x1)2(x0−x2)2(x0−x3)2(y0−y1)2(y0−y2)2(y0−y3)2(z0−z1)2(z0−z2)2(z0−z3)⎦⎤⎣⎡xyz⎦⎤=⎣⎡λ1λ2λ3⎦⎤

【UWB】公式推导计算坐标值相关推荐

栅格像元做地理坐标映射后的坐标值是像元中心点还是其他位置？
做了下实验,GDAL做完映射后像元(x,y)--->(lat,lon),得到的坐标值是像元左上角的坐标值. 下面这个看到更清楚:(其实我做了一个替换,实际我求的是左上点和右下点(通过(i+1,y ...
OpenGL shader 中关于顶点坐标值的思考
今天工作中需要做一个事情: 在shader内部做一些空间距离上的计算,而且需要对所有的点进行计算,符合条件的显示,不符合条件的点不显示. 思路很简单,在vertex shader内知道顶点坐标,进行计 ...
圆弧中点坐标值求解（二维平面三维空间）（3.1增加三维部分）-①
圆弧中点坐标值求解二维平面如图:假设O点坐标为(X0,Y0),A 点坐标为(X1,Y1),C点坐标为(X,Y),从A到C的旋转角度为θ, 复数域算法转自知乎:https://www.zhihu. ...
小场景下基于ROS的GPS经纬高度值转换为平面XYZ坐标值，并用RVIZ显示轨迹
一.实现原理在小范围场景下,可以假设GPS经纬度值都在一个平面上,地理正东方向为经度正方向,正北方向为纬度正方向,正上方向为高度正方向,至此经纬高度坐标系已经建立.而我们要做的是将其转换到一个以米为 ...
2017年9月11日梁勇 java教材编程练习题第二章 2.15 键盘读取两个点的坐标值（小数），控制台输出两点间距离。...
package com.swift;import java.util.Scanner;public class PToP {public static void main(String[] args) ...
Winform中设置ZedGraph鼠标悬浮显示距离最近曲线上的点的坐标值和X轴与Y轴的标题
场景 Winform中设置ZedGraph鼠标双击获取距离最近曲线上的点的坐标值: https://blog.csdn.net/BADAO_LIUMANG_QIZHI/article/details/ ...
分享：根据svg节点对象类型和路径值转换坐标值
功能用处: 对svg文件的路径节点填充时会使用(相邻两个坐标区域内的四边形的填充颜色不重复). 需要对svg文件中的Path节点或者 Polyline 节点做颜色填充.并且相邻的两个区域之间的颜色不允 ...
matlab显示像素分布,MATLAb-----7--------如何动态显示鼠标的坐标值和图像像素值
MATLAB如何动态显示鼠标的坐标值和图像像素值 http://apps.hi.baidu.com/share/user/41caedc2bda36d61746c6162330a (1)fix(x) ...
matlab显示曲线图中某个点的坐标值
在画好一幅曲线图后,有时候我们需要找出某个坐标的点,可以通过打开数据游标功能,此时鼠标点击的位置就会显示出坐标值,如下图: 此时保存下来的图也会带有显示的坐标值. 如果需要显示多个坐标的值,按住alt ...

【UWB】公式推导计算坐标值

【UWB】公式推导计算坐标值相关推荐

最新文章

热门文章