基于Lighthouse的室内定位，计算位置与姿态变化
以下是我在测试时使用的相关环境
- win10 操作系统
- visual studio 2019
如图所示
图片中左边为基站位置，基站不停向外界发送横扫光与竖扫光，而接收器可以接收到基站发出的光信号，通过横扫光与竖扫光可以得到基站与传感器之间的方向向量。具体硬件原理可参考https://github.com/ashtuchkin/vive-diy-position-sensor
那么，如图所示，AB,BC,CD,AD,AC,BD的长度已知，OA,OB,OC,OD的方向已知，可通过余弦定理或者向量的方式计算出OA,OB,OC,OD的长度，从而得出其位置的变化。再将初始位置作为参考位置计算每个传感器的相对位移的变化可计算出旋转矩阵，可将其转换为姿态角（roll,pitch,yaw），得到最终的位姿的变化。
本实验使用LM算法用于计算方程的解，具体算法可参考代码
传感器距离基站越近，其精度越高，因为当距离越远时，计算距离的方程会呈现出一种病态，导致误差较大。在实验中，我们设置的距离为1.6m左右。定点误差大约在5mm左右，可用于室内的超精准位姿估计。
我已将函数封装好，直接运行主函数即可，本版本准备部署到stm32板子中，直接在板子上运算，可提高整体的便携性。另外，我还写了一个python版本的定位算法，如果后续还有需要再上传。