Extended Kalman Filter Simulation for 3D TDOA/AOA Localization - Main Program

3D TDOA (Time Difference of Arrival) and AOA (Angle of Arrival) localization is a positioning method that combines time difference information and angle of arrival measurements, commonly used in wireless sensor networks, UAV positioning, and indoor navigation scenarios. The Extended Kalman Filter (EKF) is a filtering algorithm suitable for nonlinear systems, effectively improving positioning accuracy.

This main simulation program focuses on achieving high-precision position estimation through the EKF algorithm using TDOA and AOA measurement data in three-dimensional space. The program implementation begins by establishing a sensor network geometric model and simulating target motion trajectories. Subsequently, it utilizes TDOA measurement data to calculate relative distance differences between the target and multiple sensors, while incorporating AOA measurement data to provide angular constraints, further enhancing positioning reliability.

In the EKF implementation, the state vector typically includes target position and velocity information, while the observation equation is linearized based on TDOA and AOA measurement models. Through iterative prediction and update steps, the EKF progressively optimizes positioning results, reducing the impact of measurement noise and system errors. The simulation output can intuitively display the target's true trajectory, measured trajectory, and filtered estimated trajectory, facilitating analysis of algorithm convergence and stability.

This simulation program is not only suitable for academic research but can also be used for preliminary performance verification of practical systems, such as optimizing sensor placement configurations and tuning filter parameters. The code implementation includes key functions for sensor data fusion, Jacobian matrix calculations for nonlinear system linearization, and covariance matrix updates for error estimation.