基于测距的UWB刚体定位
设备型号：LinkTrackP-B
文件说明：
src： 代码和msg文件
UWB_config：UWB定位配置文件，包括测距校准，UWB基站和标签位置，坐标系转换参数
UWB_result：UWB结果生成文件，包括测距原始数据和UWB定位结果
UWB.rviz：rviz可视化配置

uwb定位结果msg：
rostopic echo /linktrack_uwb_position

2025.2.6 V1.0版本：
定位算法：纯UWB传感器准三维定位，适用于近似平面载体运动定位。
优化：稳健估计，粗差探测，RTS固定区间平滑
限制：要求anchor数量不少于4个，tag数量不少于4个

2025.9.23 V2.0版本：
定位算法：支持纯UWB传感器 准三维定位/ 真三维定位。
优化：稳健估计，粗差探测，RTS固定区间平滑
限制：要求anchor数量不少于4个，tag数量不少于4个


运行方式：
1.编译程序（vscode或者catkin_make）
2.配置UWB_config(参考自带的文件)
3.根目录下运行程序:
source devel/setup.sh
rosrun RigidAlgo RigidAlgo_node

支持以下方式读入原始数据：
1.bag包 (test.cpp)
2.csv测距文件 (test.cpp)
3.直接运行程序，监听程序