c++ - DWM1000 获取 X 位置

Question

我目前正在编写用于 DWM1000 模块的代码 我正在使用两个锚点和一个标签 我收到两个锚点到标签的距离并试图获取标签的 x,y 位置。但是我的标签的 x 坐标不正确，并且看起来就像 y 坐标一样，因为当我将标签移近锚点时它会发生变化，我希望有人可以查看我的代码并查看我做错了什么。

float a_r = (pow(-dist_right,2) + pow(dist_left,2) - pow(dist_l_r,2)) / (-2*dist_l_r);
      x = dist_l_r/2 - a_r;
      float t = pow(dist_right,2) - pow(a_r,2);
      if(dist_left < dist_right){
        Serial.println("Left");;
      }else{
        Serial.println("Right");
      }

      Serial.print("Distance Right: ");Serial.println(dist_right);
      Serial.print("Distance Left: ");Serial.println(dist_left);
      diff = abs(dist_left - dist_right);
      Serial.print("Difference: ");Serial.println(diff);
      Serial.print("A_R: ");Serial.println(a_r);
      Serial.print("T: ");Serial.println(t);
      Serial.print("X: ");Serial.println(x);

Answer 1

两个参考点三边测量产生两种解决方案 - 实际位置和错误位置。在参考点沿某些不可逾越的障碍（例如墙壁）形成基线的情况下，则可以通过此附加信息解决歧义。

鉴于：

署名：由 NavigationGuy -自己的作品，CC BY-SA 4.0，https://commons.wikimedia.org/w/index.php?curid=75898777

然后

因此代码：

x = ((dist_left * dist_left) - (dist_right * dist-right) + (dist_l_r * dist_l_r)) /
    (2 * dist_l_r) ;

y = sqrt((dist_left * dist_left) - (x * x)) ;

在您的代码中，您的a_r变量对应于x上面（我的参照系相对于您的参照系旋转了 90 度，但在任何情况下都是任意的），但您的x变量与上面没有相似之处y。您的x变量与不正交成正比a_r，因此它的变化取决于a_r.

为了维护您的参考框架和变量命名，然后：

x = sqrt((dist_left * dist_left) - (a_r * a_r)) ;

如果您愿意：

x = sqrt(pow(dist_left,2) - pow(a_r, 2)) ;