本代码实现了一个基于 到达角（AOA） 和 到达时间（TOA） 的混合定位算法，结合 扩展卡尔曼滤波（EKF） 对三维运动目标的轨迹进行滤波优化。代码通过模拟动态目标与基站网络，展示了从信号测量、定位解算到轨迹滤波的全流程，适用于城市峡谷、室内等复杂环境下的定位研究。

运行结果

定位导航示意图：

误差曲线（滤波前后对比）：