一种基于UWB与IMU的多算法协同三维定位方法

本发明公开了一种基于UWB与IMU的多算法协同三维定位方法，该方法包括：基于多个UWB基站构建三维空间直角坐标系，通过双边双向测距测得标签与UWB基站的距离，得到对应的距离变量；通过三边定位算法建立非线性方程组，代入距离变量转化为线性误差方程组，通过牛顿迭代法得到标签的一组三维坐标；通过到达时间差得到非线性方程组，基于泰勒展开近似得到线性误差方程组，通过最小二乘法得到标签的另一组三维坐标；对两组三维坐标加权平均得到初始三维坐标；基于初始三维坐标、速度分量、加速度分量构建状态转移模型，基于卡尔曼滤波预测下一时刻的状态并更新标签的三维坐标，输出最终三维空间定位结果。本发明能实现高精度的三维定位。