[发明专利]一种复杂环境下的三维定位追踪方法

摘要

本发明提供了一种复杂环境下的三维定位追踪方法，包括：步骤1，读取TDOA数据和TOF数据；步骤2，解算三维结果步骤3，反推主基站的TOF数据和从基站的TDOA数据；步骤4，对MS数组进行二维Taylor迭代，将结果存入数组X；步骤5，通过最小二乘法解算，如果无实数解则将z轴结果置0，记为zz，否则解算出两个z轴实数解z(1)、z(2)；步骤6，将z(1)、z(2)分别通过最小二乘法反推出每个基站的TDOA数据，选出解算出的TDOA与实际TDOA数据误差最小的z，并将这个z记为zz；步骤7，进行三维Taylor解算，将结果放入数组Fn；步骤8，对Fn数组进行平均滤波；步骤9，读取下一条数据。