采用距离平方和距离平方变化率的WSN/AHRS紧组合方法

摘要

本发明公开了一种采用距离平方和距离平方变化率的WSN/AHRS紧组合方法，包括：选取导航区域内任意一个参考节点的位置作为坐标原点，构建相对坐标系；在所述相对坐标系中将AHRS即姿态和方位参照系统、WSN即无线传感器网络进行集成，通过扩展卡尔曼滤波器对得到的同步导航数据在导航计算机中进行数据融合；构建扩展卡尔曼滤波器的状态方程和观测方程；将AHRS采集到的当前时刻的未知节点的位置和速度与滤波器输出的AHRS误差做差，最终得到当前时刻的未知节点的位置和速度的最优估计。本发明有益效果：由于该紧组合方法中将距离平方变化率作为数据滤波器的观测向量，因此无需新添设备就可以完成对目标节点速度误差的预估，进而完成对目标节点的速度预估。