通过两个距离传感器计算无人机PID的偏航角

我正在使用Parrot AR 2.0无人机构建控制系统，可以在其中使用上/下（z），左/右（y），前进/后退（x），左转和右转（偏航）的推力控制通过计算机上的Ruby库。

该系统的目标是，使无人机在向上/向下和向左/向右移动时，与墙壁保持一定距离并平行于墙壁。我们在左右向前的道具上增加了两个声纳距离传感器。

我遇到的主要问题是弄清楚两个距离传感器如何等于偏航读数（ψ），因此我可以将其输入PID，然后对向左或向右转的推力采取行动更正。

也许从两个距离到偏航角的转换中获得一些帮助将是一个很大的帮助，但是对PID的任何想法也将受到极大的赞赏，因为这是我第一次使用它。