Joint Velocity Measurement and Localization Algorithm Based on Doppler Frequency Shift

Precisely determining the instantaneous position and corresponding instantaneous velocity of a moving target at any moment holds significant theoretical and practical value in fields such as target tracking and motion analysis. Traditional localization and velocity estimation techniques typically employ separate algorithms to calculate position and velocity, leading to weakened coupling between measured position and velocity or only obtaining average velocity, which fails to accurately reflect the target's instantaneous velocity state. To address these limitations, this paper proposes a universal joint velocity-position estimation algorithm based on Doppler frequency shift (DFS), which innovatively achieves synchronous and precise estimation of a moving target's instantaneous position and velocity vector at any moment, achieving 100% coupling between velocity and position. The algorithm first constructs the target's position and velocity into a unified high-dimensional vector. Subsequently, a novel cost function is established and iteratively optimized to converge to the optimal position and velocity, enabling high-precision localization and velocity estimation. Building on this universal framework, specific implementations for 2D scenarios using acoustic and electromagnetic waves are presented. Simulation results demonstrate that the proposed algorithm achieves positioning accuracy better than 1 millimeter and velocity estimation errors below 0.1 m/s. Furthermore, the algorithm exhibits broad applicability, extendable to 3D space and wave types with Doppler effects (e.g., gravitational waves, neutrino waves), with promising applications in radar, sonar, underwater navigation, and deep-space exploration.