PSINS相对位置观测的协同导航尝试

PSINS

西工大的PSINS工具箱是一种用于惯性导航系统的开源软件工具包。它提供了一系列算法和功能，可用于处理惯性测量单元（IMU）和全球定位系统（GPS）等传感器数据，以实现精确的导航和定位。
西工大的PSINS工具箱在导航中的作用是实现精确的姿态解算、导航解算和集成导航，并提供参数标定功能，以提高导航系统的精度和稳定性。
在协同导航的领域中，工具箱没有进行相关的研究，这里基于工具箱，以主从式协同导航为例，来尝试设计一个基于相对距离观测的协同导航。
【值得注意的是】：仅距离观测时，必须有4个及以上的主机，并以三点法定位和紧耦合/紧组合的原理才能保证滤波收敛。

距离观测

主机和从机之间搭载UWB、WIFI等，即可测量距离，不同的测量方法有不同的误差模型，不再赘述。
PSINS的位置（状态量第7~9维）是纬经高，因此测量相对位置的时候，最好换算成直角坐标系下的xyz来测量。

仿真结果

设计5个节点的协同导航，其中主机2个、从机3个。主机搭载GNSS和IMU，使用经典的组合导航。从机搭载IMU和用于观察主机位置的传感器（可以是视觉，也可以是无线电，等等，符合能测量位置的要求即可），使用EKF进行数据融合。五个节点的运行轨迹如下：

得到的各节点纬度误差曲线图如下：

经度和高度误差的曲线如下：

仿真代码

原创的代码，给出来部分截图：

完整代码链接：https://download.csdn.net/download/callmeup/89328091