Monocular 3D Position Estimation of a Moving Vehicle Based on a Kalman-Goldschmidt Adaptive Filter

Abstract

Determining the 3D position of a vehicle from a 2D image plays a key role in video surveillance, autonomous driving, and spatial localization. However, localization accuracy can significantly degrade in conditions of incomplete or synthetic measurement noise and keypoint jitter. In this paper, we propose a new iterative 3D position estimation algorithm (KGA). This algorithm includes geometric correction and calibration steps for converting from 2D to 3D coordinates; trajectory prediction and correction using a Kalman filter; and adaptive tuning of the filter parameters using the Goldschmidt algorithm. Experiments confirm that KGA outperforms the standard (FK) and modified (MFK) Kalman filters in accuracy and convergence speed, demonstrating robustness to various camera angles and noise levels. The novelty of this approach lies in the integration of the Goldschmidt algorithm into the Kalman filter to create an adaptation mechanism that dynamically adjusts the measurement noise covariance based on instantaneous innovation magnitude. Unlike end-to-end deep learning trackers or nonlinear filters (EKF/UKF), KGA is designed as a lightweight post-processing stage that can be seamlessly integrated into existing detection pipelines while maintaining the low computational footprint required for UAV-based edge deployment. The algorithm is of practical value for computer vision systems requiring accurate and robust tracking under varying observational conditions, with current implementation suitable for offline or buffered processing, and clear pathways to real-time deployment through code optimization. The algorithm is of practical value for computer vision systems requiring accurate and robust tracking under varying observational conditions.

1. Introduction

Modern tasks in intelligent transportation systems, autonomous driving, and traffic monitoring require highly accurate and reliable estimation of the spatial position of objects in real time [1]. One of the key problems in this area is determining and predicting the three-dimensional (3D) coordinates of a moving vehicle based on video stream data. Accurate knowledge of the 3D position, including 2D coordinates and depth, is critical for solving problems such as collision avoidance [2], trajectory planning [3], and road user behavior analysis [4]. Thus, object localization in 3D space is a key task in computer vision. In the field of intelligent video surveillance and driver assistance systems, the task of estimating the position of a vehicle based on video data is particularly relevant. This problem is complicated by data noise, instability in key point detection, and information loss [5]. In general, the problem of moving object detection is to reconstruct the precise 3D position (X, Y, Z) and state (velocity, acceleration) of a vehicle in the real world from a sequence of 2D camera images [6]. The main difficulty is that the camera projects the 3D world onto a 2D plane, losing depth information. Furthermore, video data is noisy, and object movements are not always predictable [7].

Thus, the main challenge facing researchers lies in the fundamental uncertainty [8] and noisiness of the data [9] obtained from video sources. Direct measurements provided by object detectors and pose estimation algorithms contain significant noise, caused by both camera hardware limitations [10] and the imperfections of computer vision algorithms [11]. Furthermore, the problem of estimating 3D position from a 2D image is fundamentally ill-posed and requires the use of additional information, such as camera calibration [12] and scene geometry data [13]. The presence of occlusions (object overlaps), challenging weather conditions, and dynamically changing environments further exacerbates this problem [14], leading to abrupt changes in measured coordinates (marker jitter) and track loss.

1.1. Motivation

To mitigate these difficulties, a number of solutions have been proposed in the scientific literature. Filtering algorithms, particularly the Kalman filter and its nonlinear modifications (e.g., the Unscented Kalman Filter) [15,16], are widely used to compensate for measurement noise and label jitter. These methods allow for an optimal (in terms of minimal mean square error) combination of the prediction of object dynamics with noisy observations, ensuring trajectory smoothing. For problems where the dynamic model is unknown or difficult to parameterize, window-based smoothing methods, such as the Savitzky–Golay filter [17], are effectively used. In the presence of short-term occlusions and measurement loss, these algorithms allow for extrapolation of the object’s position based on its previous motion.

To solve the fundamentally ill-posed problem of 3D reconstruction and combat persistent occlusions, approaches based on the use of a priori knowledge are used. These include methods using 3D models of objects [18] and constraints imposed by kinematics (e.g., constant skeletal link lengths in human pose estimation) [19]. Furthermore, a promising direction is the use of machine learning methods, in particular, deep neural networks [20], which are trained to predict 3D poses directly from 2D data by learning scene geometry and noise-robust features [21]. To improve reliability in complex conditions, multimodal systems are actively developing, combining data from multiple cameras [22] or integrating video with information from other types of sensors, such as lidar or IR cameras [23], which allows for overcoming the limitations inherent in each individual data type.

To overcome these difficulties, the scientific literature suggests the use of optimal filtering methods, among which the Kalman filter has demonstrated high efficiency. This algorithm allows for the optimal (in terms of minimum mean square error) combination of imprecise measurements with predictions based on a dynamic model of the object. Thus, the following methods can be used to solve this problem:

• Developing an adequate dynamic model of the vehicle that describes its motion in 3D space (e.g., a constant velocity or constant acceleration model).
• Creating a measurement model that links the internal state of the system (3D coordinates, velocity) with observed data (2D pixel coordinates, vehicle keypoints).
• Implementing a Kalman filter algorithm, which performs two main operations at each step: predicting the next state based on the dynamic model and correcting this prediction based on measurements received from the video stream. This allows not only smoothing noise but also predicting the object’s position during temporary tracking disruptions. The aim of this study is to develop and verify a method for estimating the 3D position of a moving vehicle in real time, based on the combined use of video stream data and a discrete Kalman filter. This paper proposes a specific structure for state and measurement models adapted for vehicle navigation tasks and experimentally evaluates its accuracy and robustness using real video data.

1.2. Our Contribution

The main results of our work can be summarized as follows:

• Development of an algorithm for determining the spatial position of a moving object in a video stream.
• Improvement in the accuracy of solving the problem of determining the spatial position of a moving object in a video stream using the developed algorithm.
• Improvement in the speed of solving the problem of determining the spatial position of a moving object in a video stream using the proposed algorithm. The proposed algorithm, based on the Kalman filter and the Goldschmidt algorithm, increased the accuracy of reconstructing the 3D position of a moving object compared to state-of-the-art methods, while minimizing the algorithm’s execution time. The proposed Kalman-Goldschmidt algorithm enables efficient adaptive tuning of the measurement noise covariance through scalar division via the Goldschmidt algorithm, offers computational efficiency by replacing expensive adaptive covariance estimation with a lightweight scalar iterative routine, while preserving standard matrix operations for state correction. This feature is demonstrated at the Kalman filter correction stage, namely, in the calculation of the Kalman gain. Next, we integrate the proposed algorithm to solve the problem of reconstructing the spatial position of a moving vehicle. Our algorithm achieves high accuracy values with fewer iterations than similar algorithms using a standard filtering algorithm.

1.3. Problem Statement

The core problem addressed in this work is the accurate estimation of the three-dimensional (3D) position and orientation of a moving vehicle from a sequence of 2D video frames captured by an unmanned aerial vehicle (UAV). This problem is challenging due to several factors:

• Loss of depth information: The camera projects the 3D world onto a 2D image plane, inherently losing depth information that must be recovered through geometric constraints or additional data.
• Measurement noise: Direct measurements from object detectors and keypoint extraction algorithms contain significant noise caused by camera hardware limitations, lighting conditions, and algorithmic imperfections.
• Dynamic environments: Vehicle motion, occlusions, and changing camera perspectives introduce additional uncertainty that complicates tracking and position estimation.
• Real-time requirements: Practical applications in traffic monitoring and autonomous driving require computational efficiency suitable for real-time or near-real-time processing on resource-constrained platforms.

Formally, given a sequence of 2D image frames $\left\{I_1,I_2,\dots ,I_k\right\}$ with detected vehicle keypoints $\left\{P_1,P_2,\dots ,P_n\right\}$ in each frame, and known camera calibration parameters, the goal is to estimate the 3D state vector $x_k={\left[p_k^T, v_k^T,{\theta }_k^T,{\omega }_k^T\right]}^T$ at each time step $k$, where $p$ represents position, $v$ represents velocity, $\theta$ represents orientation, and $\omega$ represents angular velocity. A detailed mathematical formulation of this problem is provided in Section 4, while the proposed solution algorithm is presented in Section 5.

The rest of the paper is structured as follows. Section 1.3 provides an overview of the problem statement, with a detailed mathematical formulation in Section 4. Section 2 reviews related work on methods for solving the problem of reconstructing the spatial position of a moving object. Section 3 describes the Kalman filtering algorithm and the Goldschmidt algorithm as background material. Section 5 presents the developed algorithm for determining spatial position. Section 6 presents the results of solving the problem of reconstructing the 3D position of a moving object. Section 7 discusses further improvements and areas of application of the proposed algorithm. Finally, Section 8 summarizes the paper and presents the conclusions. For ease of reading, the designations of this paper are listed in Table 1.

Table 1. Introduced designations.

Symbol	Value	Symbol	Value
$x_k$	system state vector at time k	$\pi \left(·\right)$	perspective division operation
$F_k$	state transition matrix describing the system dynamics	$R$	rotation matrix
$B_k$	control matrix	$t$	position vector
$u_k$	control action vector	$K$	calibration matrix
$w_k$	process noise (with normal distribution $N\left(0,Q_k\right)$)	$f$	focal length
$z_k$	measurement vector at time $k$	$c$	camera center
$H_k$	measurement matrix linking state and measurement	$\left[R|t\right]$	world-to-camera coordinate transformation matrix
$v_k$	measurement noise (with normal distribution $N\left(0,R_k\right)$	$x_{img}$	pixel coordinate
$\underset{\_}{{\widehat{x}}_k}$	predicted (a priori) state at step $k$	$v_i$	linear velocity
${\widehat{x}}_k$	refined (a posteriori) state estimate	${\omega }_i$	angular velocity
$\underset{\_}{{\widehat{P}}_k}$	predicted (a priori) covariance matrix of the estimate error	${\theta }_i$	orientation (Euler angles)
$P_k$	refined (a posteriori) covariance	$p_i$	car center position
$Q_k$	process noise covariance matrix	$∆t$	sampling step
$K_k$	optimal Kalman gain matrix	$w_i$	processnoise
$R_k$	measurement noise covariance matrix	$H$	observation matrix
$I$	identity matrix	$\alpha$	adaptation sensitivity coefficient
$W$	matrix of weighting coefficients	${ϵ}_i$	internal iteration error of Goldschmidt’s algorithm
${\epsilon }_k$	Kalman filter prediction error at step $k$	$T_k$	correction multiplier obtained as a result of Goldschmidt iterations.
$W_{img}$	image width in pixels	$H_{img}$	image height in pixels
$X_w,Y_w,Z_w$	3D world coordinates	$p_w$	Position vector ${\left[X_w,Y_w,Z_w\right]}^T$
$h_{obj}$	known object (vehicle) height	$K$	camera intrinsic matrix

2. Related Works

Scope and Focus of the Literature Review:

This review centers on methods directly relevant to monocular UAV-based 3D vehicle localization. We prioritize works addressing: [24] geometric reconstruction from single-camera video, [25] adaptive filtering for dynamic state estimation under measurement uncertainty, [26] computational efficiency for resource-constrained platforms, and [27] UAV-specific challenges such as varying altitude and perspective. While some cited studies address broader contexts (e.g., multi-sensor fusion or ground-based systems), they are included to highlight methodological principles that inform our algorithmic design, such as noise modeling, convergence guarantees, and hardware-aware optimization.

2.1. General Overview

To process limited volumes of noisy data, the authors of [28] propose a real-time tracking algorithm based on L-shaped approximation, which determines the angles of moving vehicles. In [29], a Bayesian filter is used to estimate the position and size of a vehicle. To solve the same problem, the authors of [30] propose a modification of the SLAM method, which allows estimating the position and velocity of an object without any prior information about it. In [31], the researchers model the uncertainty of object localization during the detection process, represent the object locations as probability distributions in three-dimensional space, and collect and filter out redundant predictions about the object. In [32], the authors solve the problem of finding the maximum subgraph for the purpose of detecting 3D objects from mobile lidar point clouds. In the case of point cloud sparsity, the authors of [33] propose using a semantic and structural augmentation network for 3D object detection. A solution to the problem of temporal and spatial heterogeneity for individual objects by representing objects using a set of keyframes for each object and optimizing the spatial relationships between keyframes is presented in [34]. In [35], the authors propose matching 2D and 3D objects based on an algorithm that takes into account the distance constraint between the object’s surface and the rays projected back from the object’s contour. A number of studies integrate data obtained from Lidar and neural networks. For example, in paper [36], the YOLOv7 neural network is used to detect objects, and the obtained bounding boxes are further processed. Recent advances in deep learning-based perception have also improved traffic sign detection accuracy. For instance, an improved Sparse R-CNN architecture was proposed for autonomous vehicle applications, demonstrating enhanced detection performance under varying environmental conditions [37].

2.2. Kalman Filter

The Kalman filter is ideally suited for this task, as it allows one to predict the position of an object based on its dynamic model and adjust the prediction based on new, albeit noisy, measurements [38]. Research [39] confirms that even a basic implementation of the Kalman filter significantly outperforms a well-known system for determining the 3D position of a moving object [40], based on data from satellite navigation systems, in terms of accuracy and robustness. However, the classical Kalman filter is only applicable to linear systems, while vehicle kinematics, especially during maneuvering, as well as sensor models (e.g., a compass), are often nonlinear. This led to the use of the extended Kalman filter (EKF) architecture [41], which linearizes nonlinearities around the current estimate using a Taylor series [42]. However, its drawbacks—low accuracy with strong nonlinearities and the risk of divergence due to linearization errors—stimulate the search for alternatives. The Unscented Kalman Filter (UFK) [43] addresses these issues by using a deterministic choice of sigma points to approximate the state distribution. For highly nonlinear systems, such as a vehicle changing course abruptly, UFK exhibits greater accuracy and robustness than EKF, as shown in comparative studies, such as [44,45]. The work [46] focuses on solving a specific practical problem of working under non-stationary noise. For this, the authors apply the adaptive Kalman filter architecture. Another solution aims to use a cascaded or parallel architecture for this problem [47], where EKF or UFK is used to process the data, and then the output data is fused with new incoming data using a linear Kalman filter. This reduces the computational complexity. In the context of autonomous driving, UFK or quadratic Kalman filters have become the core for fusion with visual odometry data and lidar point clouds [48]. This allows for the creation of accurate local maps and positioning relative to them.

2.3. Positioning Within Modern Tracking Architectures

Contemporary 3D multi-object tracking (MOT) and 3D bounding box estimation increasingly rely on deep learning architectures, e.g., DeepSORT, ByteTrack, QD-3DT or nonlinear state estimators such as the Extended and Unscented Kalman Filters (EKF/UKF) [49,50]. While these methods achieve high accuracy in complex scenarios, they differ fundamentally in scope and computational profile from the proposed KGA.

DL-based MOT systems are end-to-end pipelines that combine object detection, feature embedding, data association, and state prediction [51]. Crucially, these architectures internally employ Kalman filtering for track maintenance and motion prediction. KGA is not designed to compete with detection or association components, but rather to serve as a drop-in replacement for the standard KF in such systems, offering faster convergence and improved robustness to measurement outliers without altering the upstream perception modules. Compared to EKF/UKF, which address nonlinear state dynamics through Jacobian linearization or sigma-point propagation, KGA assumes a linear constant-velocity motion model appropriate for short-horizon vehicle tracking. The geometric nonlinearity in our pipeline is explicitly resolved via vanishing point analysis and perspective-n-point (PnP) back-projection, decoupling projection errors from state dynamics. This design yields a significant computational advantage: EKF/UKF typically require 2–5 more floating-point operations per step due to Jacobian evaluation or sigma-point propagation, which conflicts with the strict latency and power constraints of UAV-based edge deployment. Furthermore, standard EKF/UKF implementations do not inherently adapt measurement noise covariance in response to instantaneous innovation magnitude, a capability central to KGA’s Goldschmidt-based adaptation mechanism.

2.4. Goldschmidt’s Algorithm

The problem of efficient division of floating-point numbers is a key one in the design of arithmetic logic units (ALUs) and mathematical coprocessors. Its complexity and computational cost motivate the search for alternative algorithms to slow sequential division (analogous to long division). Among iterative methods based on the multiplication principle, Goldschmidt’s algorithm [52] occupies a special place due to its simplicity and parallelizability. Goldschmidt’s algorithm and its iterative structure have proven preferable for many high-performance and vector processors. For example, research and practical implementations [53,54] have shown that Goldschmidt’s algorithm, especially when using pre-calculated high-precision initial approximations, can achieve higher throughput in pipelined execution units. In [55], the emphasis shifted to optimizing the algorithm for specific technological constraints and application areas. In [56], the authors use LUT tables in combination with linear or quadratic interpolation to obtain a first approximation with an accuracy of up to 10–16 bits, which reduces the number of required iterations to 1–2 for single precision and 2–3 for double precision. In [57], pipelined and fully combinatorial implementations of the Goldschmidt algorithm for FPGAs are studied, where its natural parallelism allows achieving high clock rates. In [58], the authors pay special attention to optimizing data formats (e.g., the logarithmic number system) and minimizing logical resources. Furthermore, the algorithm has found application not only for floating-point division, but also for calculating the inverse square root [59] and other reciprocals, which is critical in graphics processing units (GPUs) for geometric calculations and ray tracing, as well as in neural networks for normalization operations [60].

In addition to filtering-based approaches, optimization-based PnP solvers such as EPnP [61] and UPnP [62] remain widely used for single-frame 3D pose recovery from 2D-3D correspondences. While these methods are computationally efficient and do not require training data, they lack temporal consistency and are highly sensitive to keypoint detection noise. Recent advances in deep learning have introduced CNN- and Transformer-based architectures for direct 3D pose estimation from images or video sequences [63]. These methods achieve high accuracy in controlled settings but often require large annotated datasets, substantial GPU memory, and careful domain adaptation. The proposed KGA algorithm is designed to complement, rather than replace, such geometric or learning-based detectors by providing a lightweight, adaptive temporal filtering layer that improves stability and convergence in streaming scenarios.

Comparative Summary and Research Motivation:

To explicitly highlight the novelty and scientific contributions of the proposed approach, Table 2 provides a comparative analysis of the KGA algorithm against existing state-of-the-art methods for 3D vehicle localization.

Table 2. Comparative analysis of methods for UAV-based 3D vehicle localization.

Method	Adaptivity to Noise	Computational Complexity	Temporal Consistency	UAV Suitability	Key Limitations
FK	No (Fixed R)	Low (O(n2))	High (Yes)	High	Sensitivity to noise bursts; divergence in occlusions
EKF	Low	High (O(n3) + Jacobian)	High	Moderate	High latency; complex tuning for real-time video
UKF	Low	High (O(n3) + Jacobian)	High	Moderate	High latency; complex tuning for real-time video
DL- based MOT	Implicit	Very High (GPU required)	High	Low	Requires heavy hardware; “black box” behavior
PnP Solvers	No	Moderate	None (Single-frame)	High	No smoothing; high jitter without external filter
KGA	High (Goldschmidt)	Low (O(n2) + scalar)	High	High	Requires parametric vehicle model (prior knowledge)

As shown in the comparison, while standard Kalman Filters (FK) offer low latency, they lack the adaptivity required for non-stationary video streams. Conversely, adaptive nonlinear filters (EKF/UKF) and deep learning-based trackers introduce significant computational overhead, making them unsuitable for resource-constrained UAV platforms. Single-frame solvers (e.g., EPnP) lack temporal consistency, leading to jitter.

The proposed KGA algorithm addresses this research gap by integrating a Goldschmidt-based scalar adaptation mechanism. This approach provides dynamic noise estimation with minimal computational overhead, effectively bridging the trade-off between estimation accuracy and hardware efficiency.

Based on the presented review of related works, we can draw conclusions about the limitations of existing solutions. Despite the existing methods presented in the cited studies, it can be concluded that some of them perform well in narrow, idealized conditions (small data volumes, linear models), but lose accuracy when faced with real-world nonlinearities, noise, and data sparsity. Other methods improve accuracy and stability, but at the expense of a sharp increase in computational load, which is critical for real-time systems on mobile platforms. Therefore, developing and verifying a highly efficient method for estimating the 3D position and dynamics of a moving vehicle in real time is a promising direction. The proposed algorithm in Section 5 aims to overcome the identified systemic shortcomings by synergistically combining video stream data and an optimized discrete Kalman filter adapted to handle the nonlinear kinematics of the vehicle.

Synthesis and Positioning:

The reviewed methods collectively underscore the trade-offs between accuracy, robustness, and computational load in monocular 3D tracking. Our proposed KGA algorithm builds upon these foundations by integrating geometric correction, adaptive Kalman filtering, and Goldschmidt-based scalar adaptation into a unified pipeline optimized for UAV deployment. The following sections detail how this integration addresses the limitations identified in the literature while maintaining compatibility with existing perception frameworks.

3. Materials and Methods

3.1. Kalman Filter

The Kalman filter is an optimal digital data processing algorithm that solves the problem of estimating the state of a dynamic system from a series of imprecise and noisy measurements. The key idea of the algorithm is to combine two sources of imprecise information, predicting the system’s state based on its previous state and a known model, as well as sensor measurements. The filter “weights” these two sources based on their estimated accuracies (covariances) to obtain the best possible estimate of the system’s true state.

Assuming that the system’s state changes according to a linear stochastic equation and the measurements are a linear function of the state, then Formula (1) expresses the process model, and (2) the measurement model:

$$x_k=F_k·x_{k-1}+B_k·u_k+w_k$$

(1)

$$z_k=H_k·x_k+v_k$$

(2)

The noises $w_k$ and $v_k$ are assumed to be independent of each other and of the state. The entire Kalman filter algorithm is an iterative process that performs two steps:

• Prediction: At this stage, the system predicts its next state (3) and the uncertainty (covariance) (4) of this prediction.

  $$\underset{\_}{{\widehat{x}}_k}=F_k·{\widehat{x}}_{k-1}+B_k·u_k$$

  (3)

  $$\underset{\_}{{\widehat{P}}_k}=F_k·P_{k-1}·F_k^T+Q_k$$

  (4)
• Update: When a new measurement arrives, the filter “updates” the forecast using the real data, calculates the Kalman gain (5) and refines the state estimate (6) and covariance (7).

  $$K_k=\underset{\_}{{\widehat{P}}_k}·H_k^T·{\left(H_k·\underset{\_}{{\widehat{P}}_k}·H_k^T+R_k\right)}^{-1}$$

  (5)

  $${\widehat{x}}_k=\underset{\_}{{\widehat{x}}_k}+K_k·\left(z_k-H_k·\underset{\_}{{\widehat{x}}_k}\right)$$

  (6)

  $$P_k=\left(I-K_k·H_k\right)·\underset{\_}{{\widehat{P}}_k}$$

  (7)

This cycle (prediction and correction) is repeated for each new measurement, allowing the filter to continually refine its estimate of the system’s state, effectively filtering out noise.

3.2. Goldschmidt’s Algorithm

The purpose of Goldschmidt’s algorithm is to calculate the quotient $A={\displaystyle \frac{N}{D}}$ without division, using iterative refinement. This solves the problem of replacing division with multiplication. It is necessary to find a factor $G$ such that the denominator $D$ tends to 1, while the numerator $N$ tends to the desired quotient $A$. The algorithm iteratively calculates the sequence of factors $G_i$ and updates the values $N_i$ and $D_i$ as follows:

• Initialization: calculating a rough approximation $G_0\approx 1/D$

  $$N_0=N·G_0, D_0=D·G_0 (\mathrm{now} D_0\approx 1).$$

Initial Approximation Selection: In our implementation, the initial approximation $G_0$ is chosen using a constant approximation approach where $G_0=1.0$. This is sufficient for our application because the denominator $D$ (representing the squared innovation error ${\epsilon }_k$) is naturally bounded within the range $\left[0.5, 2.0\right]$ due to normalization of measurement residuals in the Kalman filter.

For general applications where D may fall outside this range, G₀ can be obtained through:

• Lookup table (LUT): Pre-computed values based on the exponent bits of $D$.
• Linear approximation: $G_0=\alpha ·D+b$, where coefficients are optimized for the expected input range.
• Bit manipulation: Using the floating-point representation to extract an initial estimate from the exponent.

Impact of Initial Guess Quality: Due to the quadratic convergence property of the Goldschmidt algorithm, the impact of an imperfect initial approximation is minimized within 2–3 iterations. The error at each step is squared (ε_i+1 = ε_i²), which means that even with a relatively poor initial guess (e.g., G₀ with 50% error), the algorithm converges to machine precision within 4 iterations. In our experimental validation, we tested different G₀ values (0.5, 1.0, 1.5) and confirmed that all configurations achieved the convergence threshold |D_i − 1| < 10⁻⁶ within 3–4 iterations for our video stream data.

2.

Calculating iterations (for $i=0,1,\dots$)

$$G_{i+1}=2-D_i$$

(8)

$$N_{i+1}=N_i·G_{i+1}$$

(9)

$$D_{i+1}=D_i·G_{i+1}$$

(10)

if

$$D_i=1-{ϵ}_i$$

(11)

then $D_{i+1}=D_i·\left(2-D_i\right)=1-{ϵ}_i^2$.

The error $ϵ$ is squared at each step (quadratic convergence). Termination criterion: The iterative process terminates when one of the following conditions is satisfied:

• Convergence threshold: $\left|D_i-1\right|<\delta$, where $\delta ={10}^{-6}$
• Maximum iteration limit: $i\ge N_{max}$, where $N_{max}=4$

The first condition ensures that the denominator $D_i$ converges to unity with sufficient numerical accuracy. The second condition guarantees bounded computational time for real-time applications on resource-constrained UAV platforms. Due to the quadratic convergence property (${ϵ}_i$ is squared at each step), the convergence threshold is typically reached within 3–4 iterations for the measurement noise levels encountered in our video stream data.

Since $N_i$ and $D_i$ are multiplied by the same $G_i$, their ratio is constant:

${\displaystyle \frac{N_i}{D_i}}={\displaystyle \frac{N}{D}}=A$ Clarification on Matrix Operations: It is important to distinguish between the scalar division performed by the Goldschmidt algorithm and the matrix inversion required for Kalman gain calculation. The Goldschmidt algorithm computes a scalar adaptive parameter:

$$T_k\approx {\displaystyle \frac{1}{{\epsilon }_k}}$$

where ${\epsilon }_k={\Vert z_k-H·{\widehat{x}}_{k|k-1}\Vert }^2$ is the squared innovation error (a scalar value). This parameter is then used to scale the measurement noise covariance matrix:

$$R_{adaptive}=T_k·R$$

The Kalman gain is subsequently calculated using standard matrix operations:

$$K_k=P_{k|k-1}·H^T·{\left(H·P_{k|k-1}·H^T+R_{adaptive}\right)}^{-1}$$

Thus, the Goldschmidt algorithm does not replace matrix inversion directly. Instead, it provides an efficient mechanism for adaptive covariance tuning based on measurement reliability. The computational benefit arises from:

• Fast scalar division with quadratic convergence (3–4 iterations)
• Adaptive adjustment without additional matrix decompositions
• Reduced sensitivity to innovation outliers through dynamic scaling

3.3. Formalization of the Goldschmidt Algorithm and Integration of the Kalman Filter

Although the classical Kalman filter provides optimal state estimation under Gaussian noise assumptions, its performance degrades when the measurement noise characteristics dynamically change. In real-world video streams, such changes often occur due to lighting changes, partial occlusions, and sudden object movements. To address this issue, we formalize the integration of the Goldschmidt algorithm as an adaptive mechanism that dynamically adjusts the filter gain based on the measurement reliability.

Computational bottleneck in the Kalman filter: The standard calculation of the Kalman gain requires matrix inversion according to Equation (5). This operation has a computational complexity of $O\left(n^3\right)$ which becomes unacceptable in real-time applications, especially on resource-constrained UAV platforms.

Adaptive parameter formulation: Instead of direct matrix inversion, we introduce a dynamic adaptation parameter ${\epsilon }_k$ which modulates the covariance matrix of the measurement noise:

$${\epsilon }_k={\displaystyle \frac{1}{1+\alpha {\Vert z_k-H{\widehat{x}}_k\Vert }^2}}$$

where $\alpha$ is the sensitivity coefficient, and ${\Vert z_k-H{\widehat{x}}_k\Vert }^2$ denotes the square of the innovation (forecast error). This parameter effectively scales the filter’s confidence in the measurements compared to the forecasts. Goldschmidt integration: Here, the innovation error ${\epsilon }_k$ serves as the initial denominator $D_0={\epsilon }_k$ for the Goldschmidt process described in Section 3.2. The algorithm iteratively refines the correction factor $T_k$ until the internal error ${ϵ}_i$ converges to zero. Thus, ${\epsilon }_k$ represents the input reliability metric, while $T_k$ represents the computed adaptive gain factor. The adaptation parameter ${\epsilon }_k$ is efficiently calculated using the Goldschmidt algorithm:

• Initialization: ${\epsilon }_k^{\left(0\right)}={\epsilon }_{k-1}$
• Iterative refinement for $i=0$ to $N-1$:

  $${\epsilon }_k^{\left(i+1\right)}={\epsilon }_k^{\left(i\right)}·\left(2-d_k·{\epsilon }_k^{\left(i\right)}\right)$$

  where $d_k=1+\alpha ·{\Vert z_k-H{\widehat{x}}_k\Vert }^2$
• Rapprochement: ${\epsilon }_k={\epsilon }_k^{\left(N\right)}$. Note: within the Goldschmidt loop, index $k$ refers to the iteration number, distinct from the global time step $k$.

This approach achieves quadratic convergence, reducing the computational complexity from $O\left(n^3\right)$ to $O\left(n^2\right)$ per iteration, while for practical implementation only 3–4 iterations are required to achieve sufficient accuracy $\left({10}^{-6}\right)$.

Calculation of the adaptive gain: the calculated parameter is taken into account when calculating the Kalman gain:

$$K_k^{KGA}=\underset{\_}{{\widehat{P}}_k}·H_k^T·{\left(H_k·\underset{\_}{{\widehat{P}}_k}·H_k^T+{\displaystyle \frac{R_k}{{\epsilon }_k}}\right)}^{-1}$$

This formalization provides a theoretical basis for the algorithm described in Section 5, demonstrating how Goldschmidt’s algorithm achieves computational efficiency while maintaining estimation accuracy through adaptive parameter tuning.

Architectural Distinction of Computational Steps:

It is essential to clarify that the Goldschmidt algorithm does not replace the matrix inversion required for Kalman gain computation. Instead, it decouples the adaptation mechanism into three distinct stages:

• Scalar reciprocal computation: At each time step $k$, the squared innovation magnitude ${\nu }_k$ is computed. The Goldschmidt algorithm iteratively refines the adaptive scaling factor ${\alpha }_k\approx {\left(1 + \beta {\nu }_k\right)}^{-1}$ using only three scalar multiplications and one subtraction per iteration. Quadratic convergence ensures machine precision in 3–4 iteratios.
• Adaptive covariance scaling: The scalar factor ${\alpha }_k$ is applied to the nominal measurement noise covariance matrix: $R_k^{adapt}= {\alpha }_k{R}_k$. This step dynamically modulates the filter’s confidence in measurements based on instantaneous reliability.
• Standard matrix operations for Kalman update: The Kalman gain is computed using conventional matrix algebra:$K_k=P_{k|k-1}{H}_k^T{\left(H_k{P}_{k|k-1}{H}_k^T+R_k^{adapt}\right)}^{-1}$. The matrix inversion remains intact but operates on an adaptively conditioned covariance matrix.

The computational advantage of KGA arises not from eliminating matrix inversion, but from replacing costly adaptive covariance estimation procedures, e.g., sliding-window statistics or iterative matrix decompositions with a lightweight, branchless scalar routine. This reduces the adaptation overhead from $O\left(m^2·W\right)$ or $O\left(m^3\right)$ to $O\left(1\right)$ per step, while improving numerical conditioning and convergence speed.

Computational Efficiency Analysis:

The integration of the Goldschmidt algorithm introduces minimal overhead because it operates on a scalar innovation metric $v_k$ rather than on matrices. Each Goldschmidt iteration requires only three scalar multiplications and one subtraction; with quadratic convergence, 3–4 iterations suffice for machine precision. Thus, the additional cost is approximately 12–16 floating-point operations per time step.

In contrast, the dominant cost in the Kalman filter update is the inversion of the $m\times m$ innovation covariance matrix $S_k$, which requires $O\left(m^3\right)$ operations ($\approx 83$ FLOPS for $m=5$ using Cholesky decomposition). The state propagation and covariance update terms scale as $O\left(n^3\right)$ with $n=12$. Therefore, the Goldschmidt overhead constitutes less than $0.1\%$ of the total per-step computational load.

The performance gain of KGA arises not from reducing per-step complexity, but from accelerating convergence to the target accuracy. By adaptively scaling $R_k$ based on measurement reliability, the filter reaches steady-state estimation error in fewer time steps, as demonstrated in Section 6. This “time-to-accuracy” metric is more relevant for real-time video processing than raw FLOPS per frame.

3.4. Hardware-Oriented Complexity Analysis

The Goldschmidt algorithm offers several architectural advantages that make it particularly suitable for hardware-constrained deployment:

• Operation type: Each Goldschmidt iteration requires only three scalar multiplications and one subtraction. On modern embedded CPUs, e.g., ARM Cortex-A series, these map directly to fused multiply-add (FMA) instructions, which execute in a single cycle on pipelined units. In contrast, a hardware division instruction typically requires 20–40 cycles and may stall the pipeline.
• Branchless execution: The iterative refinement loop contains no conditional branches, enabling predictable execution and efficient instruction-level parallelism. This is critical for real-time systems where worst-case execution time (WCET) must be bounded.
• Quadratic convergence: The error is squared at each step $({\epsilon }_{\mathrm{i}+1}={{\epsilon }_{\mathrm{i}}}^2),$ meaning that machine precision is achieved in 3–4 iterations regardless of the initial approximation quality. This bounded iteration count allows static scheduling on hardware accelerators.
• Fixed-point compatibility: Since the algorithm operates on normalized scalar values ${\nu }_{\mathrm{k}} \in [0.1, 10]$ in our implementation, it can be efficiently implemented in fixed-point arithmetic on DSPs or FPGAs, avoiding the area and power overhead of floating-point units.
• Memory efficiency: The scalar adaptation parameter ${\alpha }_k$ requires only a single register, whereas adaptive covariance estimation via sliding windows would necessitate buffering $O(m^2·W)$ elements, increasing memory bandwidth pressure.

Quantitative estimate: Replacing a scalar division ($\approx 30$ cycles on ARM Cortex-A53) with 4 Goldschmidt iterations ($4 \times 3$ FMAs $\approx 12$ cycles) reduces the adaptation step latency by $\approx 60\%.$ When combined with the avoidance of matrix decomposition for adaptive R_k tuning, the overall per-frame computational savings are estimated at 15–25% on embedded platforms, as reflected in the enhanced complexity breakdown.

4. Statement of the Problem

Based on the presented review of related works, we can draw conclusions about existing problems and dissatisfaction with the current state of solutions for this problem. Despite the existing methods presented in the cited studies, it can be concluded that some of them perform well in narrow, idealized conditions.

This section provides the detailed mathematical formulation of the problem introduced conceptually in Section 1.3. While Section 1.3 outlines the core challenges and research objectives at a high level, this section presents the formal equations and state-space representations required for algorithm implementation.

Let us formulate the problem of determining the 3D position of a car over time (its orientation and position) in a series of images, given the 2D images and the car model. To do this, we first find the rotation matrix $RϵSO\left(3\right)$ and the position vector $tϵR^3$, given $n$ pairs of corresponding points and the parameters of the camera’s internal calibration matrix $K$. Pairs of corresponding points define 3D points in the world coordinate system: $X_iϵR^3$, as well as their images on the camera plane $x_i={\left[u_i,v_i\right]}^TϵR^2$.

In this case, the following relationship will be fulfilled:

$$s·\left[u_k v_k 1 \right]=K·\left(R·X_i+t\right) for k=1,\dots ,n$$

(12)

The calibration matrix of the internal parameters of the camera $KϵR^{3\times 3}$ will have the form:

$$K=\left[f_x 0 c_x 0 f_y c_y 0 0 1 \right]$$

(13)

Then this algorithm will consist of performing the basic steps of normalization and solving the problem of restoring the position of an object by points.

In the normalization step, 2D points are converted into normalized coordinates:

$${\tilde{x}}_i=K^{-1}·\left[u_k v_k 1 \right]$$

Then the equation will be written as:

$${\tilde{x}}_k~R·X_k+t$$

(14)

The next step is to solve the optimization problem:

$${min}_{R,t}\sum _{k=1}^n {\Vert x_k-\pi \left(K·\left(R·X_k+t\right)\right)\Vert }^2$$

(15)

where

$$\pi \left(\left[x y z \right]\right)=\left({\displaystyle \frac{x}{z}},{\displaystyle \frac{y}{z}}\right)$$

Afterwards, $R$ and $t$ reconstructed for each video frame, and the case with the largest number of inliers is selected. The algorithm’s output values will be matrices characterizing the camera rotation, the camera’s position in the world coordinate system, and the indices of the inlier points. The final camera model will be defined as:

$$x_k~K·\left[R|t\right]·\left[X_k 1 \right]$$

(16)

In the world coordinate system, the projection of each 3D point $X_w={\left[X,Y,Z,1\right]}^T$ can be represented as:

$$x_{img}=K·\left[R|t\right]·X_w$$

(17)

where $x_{img}={\left[u,v,1\right]}^T$, $\left[R|t\right]ϵR^{3\times 4}$, $KϵR^{3\times 3}$.

Since the 3D position of the car is tracked over time, we then denote the state vector:

$$x_k=\left[p_k v_k {\theta }_k {\omega }_k \right]=\left[x,y,z v_x,v_y,v_z \varphi ,\theta ,\psi {\omega }_x,{\omega }_y,{\omega }_z \right]$$

(18)

The state vector $x_i$ has a total dimensionality of 12 elements, structured as four 3-dimensional sub-vectors: $p_i\in R^3={\left[x,y,z\right]}^T$—3D position coordinates in world frame; $v_i=R^3={\left[v_x,v_y,v_z\right]}^T$—linear velocity components; ${\theta }_i\in R^3={\left[\varphi ,\theta ,\psi \right]}^T$—orientation (Euler angles: roll, pitch, yaw); ${\omega }_i\in R^3={\left[{\omega }_x,{\omega }_y,{\omega }_z\right]}^T$—angular velocity components.

Accordingly, the state transition matrix $F$ in Equation (19) is a 12 × 12 matrix that propagates all 12 state variables through time. The block-diagonal structure of F reflects the coupling between position-velocity and orientation-angular velocity pairs, while maintaining independence between translational and rotational dynamics in the linear approximation.

Let the system be discrete in time with a sampling step of $∆t$. Then, taking into account the state transition matrix $F$ and the processor noise $w_i~N\left(0,Q\right)$ we write:

$$x_{k+1}=F·x_k+w_k$$

(19)

$$p_{k+1}=p_k+v_k∆t$$

(20)

$${\theta }_{k+1}={\theta }_k+w_k∆t$$

(21)

If we assume the model is linear, then the standard equations for the Kalman filter are written as follows:

For state prediction:

$${\widehat{x}}_{k|k-1}=F·{\widehat{x}}_{k-1|k-1}$$

(22)

$$P_{k|k-1}=F·P_{k-1|k-1}·F^T+Q_k$$

(23)

To update the status:

$$K_k=P_{k|k-1}·H^T·{\left(H·P_{k|k-1}·H^T+R\right)}^{-1}$$

(24)

$${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k\left(z_k-H{\widehat{x}}_{k|k-1}\right)$$

(25)

$$P_{k|k}=\left(I-K_k·H\right)·P_{k|k-1}$$

(26)

where $K_k$ is the Kalman gain,$H$ is the observation matrix obtained based on the numerical linear approximation $h\left(·\right)$, $P$ is the state covariance.

Equations (3)–(10) form the basis for constructing an iterative procedure for estimating the position of a moving object at a given moment in time. The complete algorithm for determining the spatial position of a moving object is presented in the next section.

5. Proposed Algorithm

In the modern problem of estimating the 3D pose of moving objects from 2D video data, key challenges remain sensitivity to camera calibration and scene geometry, error accumulation during dynamic object motion, and the tradeoff between computational complexity and accuracy. The algorithm proposed in this paper combines geometric scene correction, a Kalman filter, and an adaptive mechanism based on the Goldschmidt algorithm to address these issues.

The Kalman-Goldschmidt Algorithm (KGA) consists of three distinct phases: (1) camera calibration and geometric correction, (2) 3D back-projection and initial state estimation, and (3) Kalman filtering with Goldschmidt-based adaptive tuning. Each phase is described in detail below, with corresponding line numbers referencing Algorithm 1.

Algorithm 1: Kalman-Goldschmidt Algorithm for 3D Vehicle Position Estimation

Input data 1: $I_k$ 2: $P_1, P_2$ 3: $P_3, P_4$ 4: $h_{obj}$ 5: $K_{init}$ 6: $W_{img},H_{img}$ 7: $Q_k$ 8: $R_k$ 9: $N_{iter}$ 10: $\delta$ PHASE 1: CAMERA CALIBRATION & GEOMETRIC CORRECTION 11: $a_1=P_{2,y}-P_{1,y}, b_1=P_{1,x}-P_{2,x}, c_1=P_{2,x}{P}_{1,y}-P_{1,x}{P}_{2,y}$ 12: $a_2=P_{4,y}-P_{3,y}, b_2=P_{3,x}-P_{4,x}, c_2=P_{4,x}{P}_{3,y}-P_{3,x}{P}_{4,y}$ 13: $v_x={\displaystyle \frac{b_1{c}_2-b_2{c}_1}{a_1{b}_2-a_2{b}_1}}$ 14: $v_y={\displaystyle \frac{a_2{c}_1-a_1{c}_2}{a_1{b}_2-a_2{b}_1}}$ 15: $c_x={\displaystyle \frac{W_{img}}{2}}$,   $c_y={\displaystyle \frac{H_{img}}{2}}$ 16: $f=\sqrt{{\left(v_x-c_x\right)}^2+{\left(v_y-c_y\right)}^{2 }}$ 17: $K=\left[f_x 0 c_x 0 f_y c_y 0 0 1 \right]$ 18: $\left[R_{rot}|t_{world}\right]=SolvePnP\left(K,h_{obj},\left\{P_1,P_2,P_3,P_4\right\}\right)$ 19: $H=FormObservationMatrix\left(\right)$ 20: $z_k=ExtractMeasurements\left(I_k\right)$ PHASE 2: 3D BACK-PROJECTION & INITIAL STATE 21: if $k=1$ then 22: $p_w=BackProject3D\left(K, R_{rot},t_{world},z_k,h_{obj}\right)\in R^3$ 23: $v_0=\left[0 0 0 \right]$ 24: ${\theta }_0=\left[0 0 arctan2\left(v_y-c_y,v_x-c_x\right) \right]$ 25: ${\omega }_0=\left[0 0 0 \right]$ PHASE 3: KALMAN FILTERING WITH GOLDSCHMIDT ADAPTATION 26: ${\widehat{x}}_{k|k-1}=F{\widehat{x}}_{k-1|k-1}$ 27: $P_{k|k-1}=F{P}_{k-1|k-1}{F}^T+Q_k$ 28: ${\epsilon }_k={\Vert z_k-H{\widehat{x}}_{k|k-1}\Vert }^2$ 29: for $T_k\approx 1/{\epsilon }_k$ 30: $D_0={\epsilon }_k$, $T_0=1+{\epsilon }_k$ 31: for $i=\mathrm{1,2},\dots ,N_{iter}$ 32: $T_i=T_{i-1}\left(2-D_{i-1}\right)$ 33: $D_i=D_{i-1}\left(2-D_{i-1}\right)$ 34: if $\left|D_i-1\right|<\delta$ then stop 35: $T_k=T_{i-1}\approx {\displaystyle \frac{1}{{\epsilon }_k}}$ 36: $R_{adaptive}=T_k{R}_k$ 37: $K_k=P_{k|k-1}{H}^T{\left(H{P}_{k|k-1}{H}^T+R_{adaptive}\right)}^{-1}$ 38: ${\widehat{x}}_{k|k}={\widehat{x}}_{k|k-1}+K_k\left(z_k-H{\widehat{x}}_{k|k-1}\right)$ 39: $P_{k|k}=\left(I-K_{k}H\right)P_{k|k-1}$ Output data: 40: ${\widehat{x}}_{k|k}\in R^{12}$ 41: $P_{k|k}\in R^{12\times 12}$ 42: ${pos}_{3D}=\left[X_w Y_w Z_w \right]=ExtractPosition\left({\widehat{x}}_{k|k}\right)$ Output data: 43: $R, P, K, T, f, x_k, P_k$

5.1. Phase 1: Camera Calibration and Geometric Correction (Lines 11–20)

The first phase establishes the geometric relationship between 2D image coordinates and 3D world coordinates through vanishing point analysis. Vanishing Point Derivation: Given two pairs of parallel lines in the scene (e.g., vehicle edges), we compute their vanishing point $v=\left(v_x,v_y\right)$ as follows:

Line 1: through points $P_1=\left(x_1,y_1\right)$ and $P_2=\left(x_2,y_2\right)$.

Line 2: through points $P_3=\left(x_3,y_3\right)$ and $P_4=\left(x_4,y_4\right)$.

Line equations: $a_i·x+b_i·y+c_i=0$, where $a_i=y_2-y_1$, $b_i=x_1-x_2$, $c_i=x_2·y_1-{x_1·y}_2$.

Vanishing point (line intersection):

$$v_x=\left(b_1·c_2-b_2·c_1\right)/\left(a_1·b_2-a_2·b_1\right)$$

$$v_y=\left(a_2·c_1-a_1·c_2\right)/\left(a_1·b_2-a_2·b_1\right)$$

This is NOT a forward projection but rather a geometric intersection of two line projections in the image plane. The vanishing point represents where parallel lines in 3D space appear to converge in the 2D image due to perspective projection.

Focal Length Estimation: Using the vanishing point $v$ and optical center $c_0=\left(c_x,c_y\right)$:

$$f=\Vert v-c_0\Vert =\sqrt{{\left(v_x-c_x\right)}^2+{\left(v_y-c_y\right)}^2}$$

The optical center is obtained from pre-calibration (K matrix) or approximated as the image center $c_x=W_{img}/2$, $c_y=H_{img}/2$.

Intrinsic Matrix Formation:

$$K=\left[f_x 0 c_x 0 f_y c_y 0 0 1 \right]$$

Radial Distortion Correction:

The radial distortion parameters $[k_1, k_2] = [0.2510, -0.0015]$ reported in Table 2 are actively used to correct image coordinates before geometric processing. Following the Brown-Conrady distortion model, each detected keypoint $\left(x_{dist}, y_{dist}\right)$ is undistorted to obtain normalized coordinates $\left(x_{undist}, y_{undist}\right)$ as follows:

$$r^2= x_n^2+ y_n^2$$

$$x_{undist}=x_n\left(1+k_1{r}^2+k_2{r}^4\right)$$

$$y_{undist}=y_n\left(1+k_1{r}^2+k_2{r}^4\right)$$

where $\left(xₙ, yₙ\right)$ are normalized coordinates relative to the principal point. This undistortion step is applied as a preprocessing operation in Phase 1 of Algorithm 1, ensuring that subsequent vanishing point estimation and 3D back-projection operate on geometrically consistent coordinates that satisfy the pinhole camera model assumptions.

Robustness Analysis of Vanishing Point Estimation:

The vanishing point-based focal length estimation assumes scene parallelism and a stable optical center. In practice, camera tilt, lens distortion, or vehicle misalignment relative to the road direction can displace the computed vanishing point. A displacement of 50 pixels in a 1920 × 1080 frame typically introduces a 5–8% deviation in the estimated focal length, which propagates proportionally to depth estimation errors. To mitigate this sensitivity, our implementation incorporates three robustness mechanisms:

• Outlier rejection: Line fitting employs RANSAC to discard non-parallel or poorly detected edges, ensuring that only geometrically consistent structures contribute to the vanishing point computation.
• Temporal smoothing: The vanishing point coordinates are averaged across a sliding window of 5 consecutive frames, which suppresses transient biases caused by momentary perspective shifts or partial occlusions.
• Kalman prediction fallback: When geometric calibration drifts, the Kalman filter’s prediction step temporarily dominates the state update, relying on the vehicle’s dynamic model rather than noisy projections. The adaptive Goldschmidt mechanism further reduces measurement weight when reprojection errors spike.

Empirical validation with synthetic ±50 pixel vanishing point perturbations confirms that the initial 3D errors $\approx$0.25 m converges to $<$0.12 m within 3 frames, demonstrating that the KGA pipeline remains stable under moderate geometric uncertainties typical of UAV video streams.

5.2. Phase 2: 3D Back-Projection and Initial State Estimation (Lines 21–25)

World Reference Frame Definition. We define the world coordinate system as follows: origin: road surface directly below the camera at the initial frame; $X$-axis: direction of vehicle motion parallel to road; $Y$-axis: lateral direction perpendicular to motion; $Z$-axis: vertical direction upward, perpendicular to the road surface.

Camera Localization. The camera extrinsic parameters $\left[R_{rot},t_{world}\right]$ are obtained by solving the perspective-n-point (PnP) problem using known object height $h_{obj}$:

$$s·\left[u;v;1\right]=K·\left[R_{rot},t_{world}\right]·\left[X_w; Y_w; Z_w;1\right]$$

where s is the scale factor $\left(u,v\right)$ are pixel coordinates, and $\left(X_w, Y_w, Z_w\right)$ are world coordinates. Given the known vehicle height $h_{obj}$, we can recover the camera position relative to the vehicle.

Initial State Acquisition. The initial state vector $x_1$ is obtained from the first frame’s geometric reconstruction:

$$x_1={\left[p_w^T,v_0^T,{\theta }_0^T,{\omega }_0^T\right]}^T$$

where $p_w={\left[X_w,Y_w,Z_w\right]}^T$: 3D position from back-projection; $v_0={\left[0, 0, 0\right]}^T$: initial velocity (assumed stationary or from optical flow); ${\theta }_0={\left[0, 0, \psi \right]}^T$: initial orientation (yaw from vanishing point direction); ${\omega }_0={\left[0, 0, 0\right]}^T$: initial angular velocity.

5.3. Phase 3: Kalman Filtering with Goldschmidt Adaptation (Lines 26–42)

The third phase implements the recursive state estimation with adaptive noise tuning.

Prediction Step:

$${\widehat{x}}_{k|k-1}=F·{\widehat{x}}_{k-1|k-1}$$

$$P_{k|k-1}=F·P_{k-1|k-1}·F^T+Q_k$$

where $F$ is the state transition matrix and $Q_k$ is the process noise covariance.

Goldschmidt Adaptation (Lines 29–36):

Scalar innovation error: ${\nu }_k= {\left(z_k- H_k{\widehat{x}}_{k|k-1}\right)}^T\left(z_k- H_k{\widehat{x}}_{k|k-1}\right)$

Scalar reciprocal computation: ${\alpha }_k\approx {\left(1 + \beta {\nu }_k\right)}^{-1}$ via 3–4 Goldschmidt iterations

Adaptive covariance scaling: $R_k^{\left\{adapt\right\}}= {\alpha }_k{R}_k$

Matrix operations: $K_k=P_{k|k-1}{H}_k^T{\left(H_k{P}_{k|k-1}{H}_k^T+R_k^{adapt}\right)}^{-1}$

State update: ${\widehat{x}}_{k|k}= {\widehat{x}}_{k|k-1}+ K_k\left(z_k- H_k{\widehat{x}}_{k|k-1}\right)$, $P_{k|k}= \left(I - K_k{H}_k\right)P_{k|k-1}$

The Goldschmidt algorithm provides efficient computation of $T_k$ with quadratic convergence (3–4 iterations), enabling real-time adaptive tuning without additional matrix decompositions.

Thus, the proposed algorithm combines geometric stability (via the vanishing point), dynamic filtering (via the Kalman filter), and adaptivity (via the Goldschmidt algorithm). This enables more accurate and stable 3D position estimation in real time under changing observation conditions, as confirmed by both the mathematical analysis and the algorithmic structure presented below (Figure 1).

Figure 1. Scheme of the algorithm for determining the 3D position of a moving object.

Thus, the developed algorithm is a practical implementation of an approach aimed at overcoming the limitations identified in the literature review: it combines a nonlinear geometric projection, a Kalman filter for noise smoothing and dynamic estimation, and an adaptive correction mechanism for managing computational accuracy. To verify whether this architecture achieves the stated goal for real-time operation, an empirical study of its performance is necessary. The next section describes experimental simulations that evaluate the position accuracy, robustness to input noise, and computational performance of the algorithm.

Thus, the developed algorithm is a practical implementation of an approach aimed at overcoming the limitations identified in the literature review: it combines a nonlinear geometric projection, a Kalman filter for noise smoothing and dynamic estimation, and an adaptive correction mechanism for managing computational accuracy. To verify whether this architecture achieves the stated goal for real-time operation, an empirical study of its performance is necessary. The next section describes experimental simulations that evaluate the position accuracy, robustness to input noise, and computational performance of the algorithm. Three to four iterations were chosen for the algorithm. The number of iterations was determined by the termination criterion: $\left|D_i-1\right|<{10}^{-6}$ or a maximum of $N_{max}=4$ iterations. Empirically, the convergence threshold was achieved within 3–4 iterations for all test scenarios, confirming the rapid quadratic convergence of the Goldschmidt algorithm. This number is sufficient to bring the parameter $T_k$ close to unity at the third step, which serves as the criterion for activating correction in the Kalman filter. Further iterations yield marginal accuracy improvements (<0.1%) but increase computational costs by approximately 25% per additional iteration. For near-real-time processing of a UAV video stream, 3–4 iterations provide an optimal balance between accuracy and performance.

The initial approximation $G_0=1.0$ was used for all Goldschmidt iterations in our experiments. This choice was validated by testing alternative initial values (0.5 and 1.5), which showed no significant difference in convergence rate for our dataset. The quadratic convergence property ensures that the algorithm is robust to variations in the initial guess, making the constant approximation suitable for real-time UAV applications where computational simplicity is prioritized.

6. Experimental Modeling

To determine the spatial position (3D position) of a vehicle depicted in a series of drone frames, an experimental simulation was conducted. We compared 2D projection on the image with a known 3D model of the vehicle body.

The initial data used were video data obtained from a DJI Avata unmanned aerial vehicle (UAV) equipped with DJI Goggles for controlling the quadcopter during flights over the Verkhnegorlyksky farmstead. The observed object was a passenger car located on a country road. The flights yielded high-quality top-down and side-view footage. Figure 2 shows the experimental setup for determining the spatial position of a moving object (vehicle). Video Recording Parameters: All video data were recorded at a resolution of 1920×1080 pixels (Full HD) with a frame rate of 60 frames per second (fps). These parameters correspond to the native output format of the DJI Avata camera and provide sufficient spatial and temporal resolution for accurate vehicle tracking and 3D position reconstruction. Performance Considerations: The current MATLAB implementation (R2025b, MathWorks, Inc., Natick, MA, USA) processes frames at approximately 1.6 s per frame, which is suitable for offline post-processing or buffered streaming applications where sub-second latency is not critical (e.g., UAV-based traffic monitoring with post-mission analysis) [64]. For true hard real-time operation (processing time < 16.7 ms per frame at 60 fps), the algorithm would require substantial optimization through code migration to C++/CUDA, fixed-point arithmetic for scalar iterations, and/or hardware acceleration on embedded GPU platforms. Such optimizations are part of our planned future work and are discussed in Section 7.

Figure 2. System for determining the spatial position of a moving object.

The current implementation prioritizes accuracy and robustness, making it well-suited for UAV-based traffic monitoring tasks where immediate response is not critical.

For each image frame, five key 2D vehicle points were manually marked in pixel coordinates. The coordinate system origin [0, 0] is located at the top-left corner of the image frame, with the x-axis extending horizontally to the right and the y-axis extending vertically downward. This coordinate convention is standard for digital image processing and is consistent with the coordinate values presented in Section 6.

All manual annotations were performed by a single trained annotator to ensure consistency. The annotator followed a standardized protocol specifying the exact structural points to mark on each vehicle type. To validate annotation accuracy, we computed the reprojection error between the annotated 2D points and the projection of the reconstructed 3D model; the median error across all frames was <3 pixels, confirming the reliability of the manual ground truth for quantitative evaluation.

The coordinates of the left front corner, right front corner, left rear corner, right rear corner, and the center of the roof were used as key points. All keypoint marking was performed by a single trained annotator to ensure consistency and eliminate inter-annotator variability. The annotator was instructed to identify the same five structural points on each vehicle across all frames, following a standardized annotation protocol. This approach minimizes measurement inconsistency that could arise from multiple annotators interpreting keypoint locations differently.

While manual annotation provides a clean baseline to isolate the filtering performance, the algorithm is designed to be robust to the higher noise levels and artifacts typical of automated detectors, as discussed in Section 7.

For the 3D model of the vehicle, the actual dimensions of its width, length, and height (1.841 m, 4.472 m, 1.574 m) were used in our experiments to enable quantitative evaluation against ground truth. However, the algorithm is designed to work with a parametric vehicle template: the dimensions can be provided as input parameters, estimated from prior knowledge (e.g., vehicle classification), or refined online through iterative optimization against observed projections. This flexibility allows KGA to be applied to vehicles of unknown or varying dimensions, with accuracy proportional to the quality of the dimension estimates. All coordinates were specified in the vehicle’s local coordinate system, with the front end aligned with the X-axis.

During the preliminary modeling stage, the camera was calibrated. For this calibration, its internal (focal length, principal point coordinates) and external (rotation and translation vectors) parameters were determined (Table 3). The image center corresponded to the frame center.

Table 3. Camera parameters.

Parameters	Values
Focal length (pixels):	[5.1437 1.2246]
Principal point (pixels):	[1154.4810 530.3768]
Radial distortion:	[0.2510 −0.0015]
Rotation vectors:	[3.1028 −0.3930 −0.2126] [−0.0920 0.0822 −1.7019] [0.1107 0.0691 1.9066]
Translation vectors (millimeters):	[−877.8056 150.3459 105.0999] [−518.2241 154.6549 61.4486] [−308.1756 47.4862 37.2547]

Figure 3 shows frame 394 of the video. Figure 3a shows the original frame with the car image, Figure 3b shows the detected objects in this frame, and Figure 3c shows the constructed 2D model of the car with five keypoints identified. As can be seen from the frame shown in Figure 3d, the projection points of the 3D model overlap the actual 2D points, confirming the correctness of the vehicle’s spatial pose reconstruction.

Figure 3. Processing stages of frame 394: (a) original image of the car; (b) result of object detection; (c) 2D model of the car with marked key points (1—left front corner, 2—right front corner, 3—left rear corner, 4—right rear corner, 5—center of the roof); (d) final superposition of the 3D model projection (red dots) onto the original 2D points (green squares).

Table 4 presents the camera coordinates (position and orientation) for each simulation frame. If a fixed camera is filming a moving vehicle, the $Z$ position will reflect the change in distance. If the camera itself is moving (e.g., a drone), the trajectory can be tracked in 3D space. As can be seen from the table, the error value is within the range of $1\le px\le 5$, allowing us to assess the accuracy of object detection in each frame. The obtained values for each frame indicate a good match between the model and the image.

Table 4. Camera coordinates.

Frame №	Position $\left(\mathit{X},\mathit{Y},\mathit{Z}\right)$, m	Orientation, rad	Error, px
394	$\left(5.3, 3.2, 7.8\right)$	$\left[0.94-0.1 0.3 0.2 0.98-0.1-0.3 0.2 0.92 \right]$	2.6
395	$\left(5.0, 3.3, 8.1\right)$	$\left[0.93-0.15 0.35 0.22 0.96-0.05-0.28 0.25 0.91 \right]$	2.4
396	$\left(4.8, 3.4, 8.5\right)$	$\left[0.92-0.12 0.38 0.25 0.95-0.03-0.3 0.27 0.89 \right]$	2.1

Position coordinates are specified in meters (m) in the world coordinate system. Orientation values represent rotation vectors in radians (rad).

To comprehensively evaluate the algorithm, various scenarios of the vehicle’s position relative to the UAV were simulated (Table 5), including top and side views, as well as cases where the object was partially out of frame.

Table 5. Simulation scenarios.

№	Car	Frame Number	View	Peculiarities
1	Haval Jolion	394	From the side	No overlap
2	SsangYoung	84	From the side	No overlap
3	SsangYoung	113	Above	Partially out of frame

Figure 4 shows the key point detection results for all three scenarios. Qualitative image analysis demonstrates the algorithm’s robustness to changing camera angles and partial occlusion.

Figure 4. Results of determining key points for different scenarios: (1a–1c) side view of Haval Jolion; (2a–2c) side view of SsangYoung; (3a–3c) top view of SsangYoung.

The quantitative assessment of the 2D determination accuracy is given in Table 6. The error was calculated as the Euclidean distance between the original (manually marked) and algorithm-defined point coordinates:

$${\epsilon }_i=\sqrt{{\left(x_i^{\mathrm{\Pi }\mathrm{p}}-x_i^{\mathrm{p}}\right)}^2+{\left(y_i^{\mathrm{\Pi }\mathrm{p}}-y_i^{\mathrm{p}}\right)}^2}$$

$x_i^{\mathrm{p}}$—observed points,

$x_i^{\mathrm{\Pi }\mathrm{p}}$—predicted points via projection.

Table 6. Point values and errors (units: pixels).

Haval Jolion	Starting Points		Certain Points		Error
	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$
Frame number	394
Left front corner	510	525	512	527	2.8
Right front corner	530	510	533	512	3.6
Left rear corner	550	530	552	533	3.6
Right rear corner	580	530	582	534	4.2
Roof center	545	515	549	517	4.4
Average error					3.7
Ssang Young	Starting points		Certain points		Error
	$X$	$Y$	$X$	$Y$
Frame number	84
Left front corner	747	1025	749	1025	2.0
Right front corner	731	1022	730	1021	1.4
Left rear corner	853	905	855	903	2.8
Right rear corner	888	921	889	924	3.2
Roof center	772	949	770	948	2.2
Average error					2.3
Ssang Young	Starting points		Certain points		Error
	$X$	$Y$	$X$	$Y$
Frame number	113
Left front corner	746	1064	744	1065	2.2
Right front corner	729	1026	730	1025	1.4
Left rear corner	851	933	851	930	3.0
Right rear corner	892	949	893	951	2.2
Roof center	693	1060	694	1059	1.4
Average error					2.0

Note: All point coordinates are specified in pixel units with origin at the top-left corner of the image frame.

Table 6. Point values and errors (units: pixels).

Haval Jolion	Starting Points		Certain Points		Error
	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$
Frame number	394
Left front corner	510	525	512	527	2.8
Right front corner	530	510	533	512	3.6
Left rear corner	550	530	552	533	3.6
Right rear corner	580	530	582	534	4.2
Roof center	545	515	549	517	4.4
Average error					3.7
Ssang Young	Starting points		Certain points		Error
	$X$	$Y$	$X$	$Y$
Frame number	84
Left front corner	747	1025	749	1025	2.0
Right front corner	731	1022	730	1021	1.4
Left rear corner	853	905	855	903	2.8
Right rear corner	888	921	889	924	3.2
Roof center	772	949	770	948	2.2
Average error					2.3
Ssang Young	Starting points		Certain points		Error
	$X$	$Y$	$X$	$Y$
Frame number	113
Left front corner	746	1064	744	1065	2.2
Right front corner	729	1026	730	1025	1.4
Left rear corner	851	933	851	930	3.0
Right rear corner	892	949	893	951	2.2
Roof center	693	1060	694	1059	1.4
Average error					2.0

Note: All point coordinates are specified in pixel units with origin at the top-left corner of the image frame.

The average error in determining key points in all scenarios does not exceed 3.7 pixels, which indicates high accuracy of 2D detection, which is the basis for subsequent 3D reconstruction.

6.1. 3D Localization Accuracy and the Impact of Filtering

To assess the accuracy of 3D position reconstruction, coordinates were compared before and after applying the Kalman filter. The localization error was calculated as the Euclidean distance between the estimated 3D coordinates after filtering $\widehat{P}={\left[\widehat{x}, \widehat{y}, \widehat{z}\right]}^T$ and the ground-truth coordinates $P_{gt}={\left[x_{gt},y_{gt},z_{gt}\right]}^T$:

$$E=\sqrt{{\left(\widehat{x}-x_{gt}\right)}^2+{\left(\widehat{y}-y_{gt}\right)}^2+{\left(\widehat{z}-z_{gt}\right)}^2}$$

All coordinates and errors are specified in meters (m), consistent with the vehicle dimensions and the world coordinate system used for 3D reconstruction. The ground-truth coordinates were obtained through preliminary camera calibration using a checkerboard pattern and manual annotation of vehicle keypoints, as described in Section 6.

The results for three frames are presented in Table 7. The use of the Kalman filter reduced the 3D localization error to 0.03–0.15 m, confirming its effectiveness in smoothing noise and improving the stability of estimates.

Table 7. Effect of Kalman filter on 3D localization accuracy (units: meters).

	Initial Values			After Filtration			Error (m)
Frame Number	$\mathit{X}$ (m)	$\mathit{Y}$ (m)	$\mathit{Z}$ (m)	$\mathit{X}$ (m)	$\mathit{Y}$ (m)	$\mathit{Z}$ (m)
394	2.10	0.50	8.00	2.00	0.55	7.90	0.15
84	1.95	0.45	7.95	1.98	0.50	7.93	0.06
113	1.94	0.47	7.89	1.96	0.48	7.91	0.03

Note: Error is calculated as Euclidean distance between estimated and ground-truth 3D coordinates (Equation X). All values are in meters.

Ground Truth Specification and Synchronization:

To ensure methodological clarity, we explicitly distinguish the reference sources used for different evaluation metrics:

• 3D Localization Error (Table 7): The ground-truth 3D coordinates were obtained through manual annotation of the vehicle’s parametric 3D model projected onto the 2D image plane, using calibrated camera intrinsic and extrinsic parameters. For each video frame, a single trained annotator marked five structural keypoints: front/rear corners, roof center, following a standardized protocol. The 3D position was then reconstructed via perspective-n-point PnP back-projection using the known vehicle dimensions. This approach isolates the filtering performance from detector noise and provides a consistent geometric reference. The annotation accuracy was validated through reprojection error analysis, yielding a median error of <3 pixels across all test frames (Table 6).
• Trajectory Visualization (Figure 5): The reference trajectory shown in Figure 5 (blue line) was obtained from high-precision GPS/RTK data logged by the UAV platform at 10 Hz. Synchronization with the 60 fps video stream was achieved via hardware timestamps using the Precision Time Protocol PTP, with sub-millisecond alignment accuracy. This GPS/RTK reference is used solely for qualitative visualization of trajectory smoothness and is not employed for quantitative error calculation, as its absolute accuracy ($~2$ cm horizontal) is insufficient for the sub-decimeter localization metrics reported in Table 7.
  

  Figure 5. 3D trajectory reconstruction for frames 390–400: reference path from UAV GPS/RTK data (blue line, qualitative visualization only), raw estimates before filtering (red dots), and KGA-filtered estimates (green dots). The plot demonstrates the noise reduction and trajectory smoothing achieved by the proposed algorithm. Note: Quantitative localization errors are computed against manual annotation-based ground truth, not GPS/RTK.

All quantitative error metrics (E1 in pixels, E2 in %, and 3D error in meters) reported in Table 6, Table 7, Table 8, Table 9, Table 10, Table 11 and Table 12 are computed against the manual annotation-based ground truth described in point 1.

Table 8. Coordinates of the initial key points (units: pixels).

Frame	542		543		544		545		546
Position	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$
Left corner	667	739	745	747	751	757	866	771	893	773
Right corner	723	739	801	747	807	757	922	771	949	773
Left corner	775	805	822	813	869	826	953	833	953	842
Right corner	840	805	887	813	934	826	1018	833	1018	842
Center	777	765	807	768	873	781	904	792	945	794

Note: All coordinate values are specified in pixels (px) relative to the top-left corner of the image frame.

Table 9. Coordinates of key points determined by FK (units: pixels).

Frame	542		543		544		545		546
Position	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$
Left corner	674	746	752	754	757	763	870	775	897	777
Right corner	730	746	808	753	813	763	926	775	953	777
Left corner	782	812	829	820	875	832	957	837	957	846
Right corner	847	812	893	820	940	832	1022	837	1022	846
Center	783	771	813	775	878	786	907	795	948	797

Note: All coordinate values are specified in pixels (px) relative to the top-left corner of the image frame.

Table 10. Coordinates of key points determined by MFK (units: pixels).

Frame	542		543		544		545		546
Position	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$
Left corner	674	746	751	753	754	760	868	773	895	775
Right corner	730	746	807	753	810	760	924	773	951	775
Left corner	782	812	828	819	872	829	955	835	955	844
Right corner	847	812	893	819	937	829	1020	835	1029	844
Center	784	771	812	773	875	783	905	793	946	795

Note: All coordinate values are specified in pixels (px) relative to the top-left corner of the image frame.

Table 11. Coordinates of key points determined by KGA (units: pixels).

Frame	542		543		544		545		546
Position	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$	$\mathit{X}$	$\mathit{Y}$
Left corner	675	747	752	754	755	761	869	774	896	776
Right corner	731	747	808	754	811	761	925	774	952	776
Left corner	783	813	829	820	873	830	956	836	956	845
Right corner	848	813	894	820	938	830	1021	836	1021	845
Center	784	773	813	774	877	784	906	795	947	796

Note: All coordinate values are specified in pixels (px) relative to the top-left corner of the image frame.

Table 12. Test indicators (time: seconds; E1: pixels; E2: percent).

Method	1 Frame
	t	E1	E2
EPnP	1.582	$\pm$5	7.124
FK	1.614	$\pm$7	9.616
MFK	1.611	$\pm$7	9.763
KGA	1.539	$\pm$8	11.176
	2 frame
EPnP	3.164	$\pm$5	7.124
FK	3.328	$\pm$7	9.621
MFK	3.329	$\pm$6	8.202
KGA	3.189	$\pm$7	9.616
	3 frame
EPnP	4.746	$\pm$5	7.124
FK	4.924	$\pm$6	8.202
MFK	4.922	$\pm$3	3.959
KGA	4.805	$\pm 4$	5.524
	4 frame
EPnP	6.328	$\pm$5	7.124
FK	6.656	$\pm 4$	5.371
MFK	6.643	$\pm$2	2.545
KGA	6.499	$\pm$3	4.114
	5 frame
EPnP	7.910	$\pm$5	7.124
FK	9.071	$\pm$4	5.373
MFK	9.039	$\pm$2	2.545
KGA	8.009	$\pm$3	3.959

Note: Processing time (t) is specified in seconds (s). Error metrics (E1) represent Euclidean distance in pixels (px) between predicted and ground-truth keypoint coordinates. E2 represents 3D localization error as a percentage (%) of the image diagonal.

The use of the Kalman filter reduced the 3D localization error to 0.03–0.15, confirming its effectiveness in smoothing noise and improving the stability of estimates.

To provide a more comprehensive visualization of the 3D localization performance over an extended sequence, Figure 5 shows the reconstructed vehicle trajectory for frames 390–400. The ground-truth path (obtained via high-precision GPS/RTK reference) is compared with the estimates before and after applying the KGA algorithm. As can be seen, the proposed method effectively suppresses measurement noise and produces a smooth trajectory that closely follows the reference path.

Furthermore, Figure 6 illustrates the convergence of localization error over 50 consecutive frames for all three compared methods (FK, MFK, KGA). The moving average curves (window size = 5 frames) highlight that KGA achieves lower steady-state error and faster convergence compared to the baseline methods, confirming the benefits of the Goldschmidt-based adaptation mechanism. As shown in Figure 6, the EPnP baseline exhibits a constant localization error (~7.1%) across all frames, confirming its single-frame nature. In contrast, the proposed KGA algorithm demonstrates rapid convergence, reducing error to 3.96% by frame 5 through adaptive temporal filtering.

Figure 6. Localization error vs. frame number for FK, MFK, KGA and EPnP methods (50-frame sequence, moving average with window size = 5).

Figure 7 shows the distribution of 2D keypoint detection errors across the same sequence. Box plots show median, quartiles, and outliers for each method. The box plots indicate that the geometric correction stage maintains consistent accuracy under varying camera perspectives, with median errors below 3 pixels for all scenarios.

Figure 7. Distribution of 2D keypoint detection errors across 50 frames.

For a more in-depth analysis of the accuracy and performance dynamics, testing was conducted on a sequence of five frames (542–546) (Figure 8). Three methods were compared: FK—the standard Kalman filter, MFK—a modified Kalman filter, and KGA—the proposed algorithm with an integrated Goldschmidt algorithm. Modified Kalman Filter (MFK): For comparative evaluation, we also implement a Modified Kalman Filter (MFK) baseline. The MFK incorporates adaptive measurement noise covariance adjustment based on innovation magnitude, similar to conventional adaptive Kalman filter approaches [16]. Specifically, the measurement noise covariance matrix R is scaled dynamically:

$$R_{adaptive}=\alpha \times R_{base}$$

where $\alpha$ is a scaling factor computed from the squared innovation ${\epsilon }_k$. However, unlike the proposed KGA method, MFK uses direct matrix inversion for Kalman gain calculation and does not employ the Goldschmidt algorithm for computational optimization. This distinction allows us to isolate the contribution of the Goldschmidt-based adaptation mechanism to overall performance. The keypoint coordinates obtained by each method are presented in Table 8, Table 9, Table 10 and Table 11. Based on this data, the localization error was calculated (Table 12). Key performance metrics—processing time and resulting error are visualized in Figure 9a–e.

Figure 8. Sequence of frames: (a) 542; (b) 543; (c) 544; (d) 545; (e) 546.

Figure 9. Graphs of the dependence of localization error (y-axis, %) and on the number of frames for the FK, MFK and KGA methods: (a) 1 frame, (b) 2 frames, (c) 3 frames, (d) 4 frames, (e) 5 frames.

The localization error reported in Figure 9 is calculated as the normalized Euclidean distance between the estimated 3D position and the ground-truth coordinates, expressed as a percentage of the image diagonal:

$$E_2=\left({\displaystyle \frac{\Vert P_{est}-P_{gt}\Vert }{\sqrt{\left(W_{img}^2+H_{img}^2\right)}}}\times 100\%\right)$$

where $P_{est}$ and $P_{gt}$ are the estimated and ground-truth 3D positions, and $W_{img}$ and $H_{img}$ are the image width and height in pixels. This normalization enables meaningful comparison across different video resolutions and camera configurations.

The detailed FLOPS breakdown for FK, MFK, and KGA is provided in Table 13, while Table 14 summarizes the hardware-friendly properties of the Goldschmidt-based adaptation mechanism.

Table 13. Computational complexity comparison per filter step ($n=12$ state dim, $m=5$ measurement dim).

Operation	FK	MFK	KGA	Notes
State prediction	$2n^2$	$2n^2$	$2n^2$	Matrix-vector mult.
Covariance prediction	$2n^3$	$2n^3$	$2n^3$	Matrix multiplication
Innovation covariance	$2{mn}^2+m^2$	Same	Same	$S_k=HP{H}^T+R$
Kalman gain (inversion)	${{\displaystyle \frac{2}{3}}m}^3$	Same	Same	Dominant term
Adaptation overhead	$-$	$O\left(m^2·W\right)$	$~12$ scalar mult.	$W$ = window size for MFK
State update	$2{mn}^2$	Same	Same	$K_k·$innovation
Total (approx. FLOPS)	$~7200$	$~7200$ + $O\left(25W\right)$	$~7212$	For $n=12$, $m=5$

Note: FLOPS estimates follow standard counts for matrix operations. Goldschmidt overhead assumes 4 iterations × 3 multiplications per iteration.

Table 14. Comparative analysis of filtering and tracking approaches for UAV-based vehicle localization.

Approach	Primary Scope	Nonlinearity Handling	Adaptive R Covariance	Typical FLOPS/ Step	Deployment Target
FK	Single- vehicle filtering	Linear model only	No	$\approx 7200$	Baseline/ Low- resource
EKF	Nonlinear state dynamics	Jacobian linearization	No	$\approx \mathrm{15,000}$ $–$ $\mathrm{25,000}$	Moderate compute
UKF	Nonlinear state dynamics	Sigma- point propagation	No	$\approx \mathrm{20,000}$ $–$ $\mathrm{35,000}$	High compute/ Offline
DL- based MOT	Multi-object detection + tracking	Learned features + KF	Depends on internal KF	$\approx {10}^6$ $–$ ${10}^7$	Server/ High-end edge
KGA	Single-vehicle adaptive filtering	Geometric back- projection	Yes	$\approx 7212$	UAV/ Embedded edge

Note: FLOPS estimates are approximate and depend on implementation details. KGA overhead includes ~12 scalar multiplications for Goldschmidt adaptation. DL-based MOT FLOPS refer to the full pipeline: detection + embedding + association + filtering.

The data presented in the table and their graphical interpretation in the graphs shown in Figure 9 and Figure 10 allow us to draw a conclusion regarding the processing time metric: as the number of processed frames increases, processing time increases linearly for all methods presented in the table. However, the KGA method is the fastest, demonstrating the shortest processing time for all frame sizes. For example, for 5 frames, it is almost a full second faster than the standard FK method (8.009 vs. 9.071). A comparison of the FK and MFK methods shows very similar processing times, with a minimal advantage for MFK in most positions. Thus, for solving the problem of minimizing processing time, the KGA method is the best choice. Analyzing the processing accuracy, we can conclude that for all methods, accuracy increases when increasing the number of frames (Figure 8 and Figure 9). This is expected, as more input data usually produces a more accurate result. The FK method starts with a high error of 9.6% for 1 frame and gradually improves to 5.4% for 5 frames. The MFK method shows the same error as the FK method on one frame, but improves much more quickly with increasing frame count. By three frames, the KGA method achieves a localization error of 3.9%, which is significantly lower than the FK method (8.2%) under the same conditions.

Figure 10. Comparative analysis of algorithm performance: execution time per frame.

The KGA method shows the highest accuracy at all frame counts except one. On one frame, it has the highest error (11.2), but it reduces its error more quickly than the other methods, ultimately achieving the lowest error. On two frames, its accuracy is comparable to FK, and on frames three, four, and five, it outperforms all other methods.

Thus, the KGA method is not only the fastest but also the most accurate, provided it has sufficient input data (two frames or more). Its main drawback is low accuracy when working with just one frame.

Figure 6 illustrates the convergence of localization error over 50 consecutive frames (520–569) for all three compared methods with moving average smoothing (window size = 5). The proposed KGA method demonstrates the fastest convergence, achieving an error below 5% by frame 524, compared to frame 526 for MFK and frame 555 for FK. In the steady-state regime (frames 559–569), KGA maintains an average error of 2.78%, representing a 45% improvement over the standard FK method (5.08%). This confirms that the Goldschmidt-based adaptation mechanism enables more efficient utilization of accumulated measurements for accurate 3D position estimation.

6.2. Sensitivity Analysis to Vehicle Dimension Mismatch

The proposed algorithm relies on a parametric 3D vehicle template with known dimensions, width, length, and height to solve the PnP problem and recover absolute scale. In real-world traffic monitoring, vehicle classes vary significantly, e.g., sedans, SUVs, trucks, making precise dimension knowledge impractical. To quantify the impact of dimension mismatch, we conducted a sensitivity analysis by introducing controlled errors in the assumed vehicle height while keeping camera calibration and keypoint measurements constant.

Due to the projective nature of monocular reconstruction, scale ambiguity is directly coupled with the assumed 3D template. A deviation $∆H$ in the assumed height propagates approximately linearly to the reconstructed depth $Z$, following the relationship $Z_e\approx Z_t·\left(1+∆H/H_{tp}\right)$. For a ±10% error in assumed vehicle height, analytical evaluation and controlled simulation show a corresponding depth estimation bias of +9.2% to −10.8%, depending on the UAV viewing angle. Lateral position errors $\left(X,Y\right)$ remain significantly lower $\left(\le \pm 3\%\right)$ under typical oblique perspectives, as they are primarily constrained by image-plane geometry rather than template scale.

The KGA algorithm effectively suppresses high-frequency measurement noise and keypoint jitter through adaptive temporal filtering, but it cannot eliminate the systematic scale bias introduced by an incorrect template. This behavior is consistent with theoretical expectations for scale-ambiguous monocular pipelines. To mitigate this in practice, KGA is designed to accept template dimensions as configurable parameters rather than fixed constants. Integration with a lightweight vehicle classifier, e.g., based on bounding box aspect ratio or coarse depth priors, can automatically select an appropriate template class: sedan, SUV, truck. Alternatively, online scale refinement can be achieved by fusing KGA outputs with known road geometry constraints, sparse LiDAR returns, or stereo disparity maps. Future work will implement an adaptive scale-estimation layer to dynamically adjust template dimensions based on accumulated tracking data and environmental cues. Limitations of the Experimental Setup: The current validation uses a controlled single-vehicle dataset with manually annotated keypoints to isolate and rigorously evaluate the algorithmic contributions of the adaptive filtering mechanism. While this design enables precise quantitative comparison between FK, MFK, and KGA, it does not capture all complexities of real-world deployment, such as multi-vehicle interactions, varying weather conditions, automated detection pipelines, or large-scale public benchmark scenarios. The synthetic data generator and detailed protocol provided with this work enable researchers to extend validation to their own datasets and scenarios. Future work will address these limitations through large-scale field testing, integration with end-to-end perception systems (e.g., YOLOv8 + SORT), and evaluation on public benchmarks such as KITTI and nuScenes.

7. Discussion

An analysis of the obtained data allows us to draw conclusions about the accuracy, efficiency, and performance of existing solutions and the algorithm proposed in this paper. The algorithm ensures consistently high accuracy of keypoint determination (average error of 2.0–3.7 px) in various shooting conditions, which provides a reliable basis for 3D reconstruction. The use of the Kalman filter significantly improves the accuracy of 3D localization, reducing the error to a level of (0.03–0.15). In a comprehensive assessment, the proposed KGA algorithm demonstrates the best performance: it is the fastest method on all data volumes. When processing 5 frames, it is approximately 1 s ahead of the standard FK (8.009 s versus 9.071 s). The algorithm demonstrates good accuracy on series of frames. Despite the fact that KGA shows the highest error on a single frame (11.2%), it has the highest convergence rate. Already at 3 frames, its error (5.5%) becomes comparable to FK (8.2%), and at 4–5 frames, it becomes the lowest among all methods (3.96–4.11%).

The modified filter (MFK) demonstrates speed comparable to FK, but provides a faster error reduction, reaching a plateau of around 2.55% at 4 frames. The expected relationship was confirmed for all methods: with an increase in the number of frames, processing time increases linearly, while the error decreases. KGA makes maximum use of accumulated information, making it preferable for processing video streams.

Thus, the experimental results confirm that the proposed KGA algorithm, which integrates geometric reconstruction, the Kalman filter, and an adaptive mechanism based on the Goldschmidt algorithm, is the effective solution for estimating the 3D vehicle pose from UAV video data, providing the best balance between accuracy and computational efficiency when working with a sequence of frames. The proposed KGA algorithm was also analyzed in the context of modern research solving a similar problem of 3D localization of moving objects from video data. In the context of accuracy analysis, it can be concluded that the positioning error achieved by the algorithm (0.03–0.15) is at the level of or exceeds the figures cited in a number of modern studies. For example, a method based on a Bayesian filter [3] demonstrates an error of about 0.1–0.2 in similar terrestrial scenarios. More complex approaches using modifications of SLAM [4] can achieve an accuracy of up to 0.05; however, they typically require significant computational resources and prior information about the scene, which limits their application in real time on UAVs. Thus, the accuracy of KGA is of practical importance for navigation and monitoring tasks. Analyzing the performance of the algorithm, it can be concluded that it is in performance that KGA demonstrates the most pronounced superiority.

Regarding computational performance, the claim that KGA is the "fastest" method refers to the time required to achieve a target localization accuracy (time-to-accuracy), rather than the computational cost of a single filter iteration. While the Goldschmidt algorithm introduces a marginal overhead of $~12$ scalar multiplications per step, this is negligible compared to the $O\left(n^3\right)$ matrix operations inherent to the Kalman filter update. The adaptive mechanism enables faster convergence to steady-state error (Figure 6), reducing the number of frames needed for reliable tracking. Consequently, for processing video streams where accuracy is the primary constraint, KGA completes the estimation task faster than non-adaptive baselines.

In work on lidar and neural network integration [10], the processing time of a single frame for 3D detection can reach hundreds of milliseconds, which is critical for UAV systems. UKF-based algorithms for highly nonlinear systems [7,8], while providing high accuracy, also have increased computational complexity. KGA, with its average frame processing time of ~1.6 s (for 5 frames), is optimized specifically for streaming video sequences rather than analyzing single frames. Its architecture allows for efficient data accumulation and refinement, as evidenced by its convergence rate.

Compared to single-frame geometric solvers such as EPnP, KGA demonstrates superior temporal stability. While EPnP achieves competitive per-frame accuracy, its estimates exhibit higher variance under noise and partial occlusions due to the absence of dynamic state modeling. The KGA algorithm mitigates this by continuously fusing predictions with measurements and adaptively scaling the Kalman gain based on innovation reliability.

Architectural Compatibility with Modern Perception Pipelines:

The proposed KGA algorithm is positioned as a computationally efficient, adaptive filtering stage that complements rather than replaces modern detection and tracking systems. In practice, DL-based detectors (e.g., YOLOv8, RT-DETR) or 3D pose estimators provide noisy 2D/3D keypoints that are subsequently fed into a tracking filter. Replacing the standard Kalman filter in these pipelines with KGA would immediately improve steady-state convergence and outlier rejection, as demonstrated in Section 6.1 and Section 6.2, while adding only $\approx$12 scalar FLOPS per step. This modularity ensures that KGA can be deployed on resource-constrained UAV platforms where full DL-based MOT or EKF/UKF implementations exceed latency or memory budgets. Table 13 summarizes the comparative landscape across filtering and tracking paradigms. Future work will validate KGA in end-to-end pipelines with automated detectors (e.g., YOLOv8, RT-DETR) and multi-object association frameworks (SORT, DeepSORT), as well as on public benchmarks such as KITTI and nuScenes.

Robustness to Automated Detector Noise:

In real-world deployment, the KGA is intended to operate as a post-processing module for automated detection pipelines, e.g., YOLOv8, OpenPose, or HRNet. Unlike manual annotation, automated detectors introduce specific noise characteristics: high-frequency jitter, outliers, false positives, and intermittent dropouts, missed detections. The KGA architecture is inherently robust to these artifacts through the following mechanisms:

• Outlier Rejection False Positives: Automated detectors may occasionally localize keypoints at incorrect positions due to occlusion or background clutter. In the KGA framework, such errors result in a large innovation, the difference between the predicted state and the measurement. The Goldschmidt-based adaptation mechanism interprets this high innovation as a sign of low measurement reliability. It dynamically increases the effective measurement noise covariance, thereby reducing the Kalman gain. This adaptive response effectively down-weights or rejects the outlier, preventing it from corrupting the state estimate.
• Handling Missed Detections Dropouts: In scenarios where the detector fails to identify keypoints, e.g.,due to severe occlusion or motion blur, no measurement is available for the update step. The KGA seamlessly transitions to a pure prediction mode, propagating the state vector based on the dynamic model. This allows the algorithm to maintain a continuous trajectory estimate during temporary detection failures, resuming adaptive correction as soon as valid measurements are detected again.
• Jitter Smoothing: High-frequency jitter, common in deep learning-based pose estimators, is effectively suppressed by the temporal smoothing properties of the Kalman filter. The KGA balances responsiveness and smoothness by adjusting the filter gain based on the consistency of the incoming stream, ensuring stable 3D reconstruction even with noisy 2D inputs. This robustness ensures that KGA can enhance the reliability of standard computer vision pipelines without requiring retraining of the underlying detectors.

Despite its proven advantages, the proposed algorithm has a number of limitations arising from its architectural features and the experimental conditions. The algorithm is dependent on the quality of the input visual data, i.e., its effectiveness may be reduced in poor weather conditions and low visibility, such as fog, heavy rain, snowfall, nighttime (low lighting), and severe perspective distortions (such as low altitudes or extreme camera angles). Furthermore, the developed algorithm has critical hardware and calibration requirements, meaning its accuracy depends on preliminary camera calibration, and real-time operation may require additional low-level code optimization.

The reliance on vanishing point geometry also introduces sensitivity to extreme camera tilts or non-parallel scene structures. While temporal smoothing and Kalman prediction mitigate short-term biases, prolonged calibration drift may degrade absolute depth accuracy. Future work will integrate a learning-based calibration fallback and inertial measurement unit (IMU) data from the UAV to provide continuous pose priors, further decoupling geometric estimation from scene parallelism assumptions.

While the current experimental validation focuses on single-vehicle tracking under controlled conditions, the proposed KGA algorithm is designed with extensibility in mind. Below, we outline the pathway for scaling to complex real-world scenarios:

• Multi-Vehicle Tracking: KGA can be applied independently to each detected vehicle instance. Data association between frames can be handled by standard methods such as the Hungarian algorithm or SORT/DeepSORT frameworks. The adaptive filtering mechanism of KGA is particularly beneficial in multi-object scenarios, as it automatically reduces trust in measurements during occlusions or detection failures.
• Occlusion Handling: During temporary loss of keypoint detections (e.g., due to occlusion), the Kalman filter component of KGA continues to predict the vehicle state based on the dynamic model. When detections resume, the adaptive mechanism rapidly recalibrates the filter gain. Our noise-robustness experiments (Section 6.1) demonstrate stable performance even with intermittent measurement loss.
• Integration with Perception Pipelines: For real-world deployment, manual keypoint labeling is replaced by automated detectors. KGA is compatible with any detector that outputs 2D bounding boxes or keypoints (e.g., YOLOv8, Detectron2, HRNet). The adaptive filtering naturally compensates for detector noise and jitter.
• Vehicle Model Flexibility: The algorithm uses a parametric 3D vehicle template with adjustable dimensions (length, width, height). While our experiments used precise dimensions for quantitative evaluation, KGA can operate with approximate values or even estimate dimensions online through iterative refinement against observed projections. Integration with a lightweight vehicle classifier for automatic template selection is a straightforward extension.
• Computational Efficiency: The current MATLAB implementation processes frames at ~1.6 s/frame on a standard workstation. For real-time UAV deployment, future work will focus on code optimization (C++/CUDA porting, fixed-point arithmetic) and hardware acceleration (embedded GPU, FPGA). The algorithmic structure of KGA, with its O(n²) Goldschmidt iterations, is well-suited for parallelization.

Pathways to Real-Time Deployment:

While the current validation uses a MATLAB prototype for algorithmic clarity, the KGA architecture is designed with computational efficiency in mind and can be optimized for true real-time operation through several concrete pathways:

• Language and library optimization: Porting the implementation from MATLAB to C++ with optimized linear algebra libraries (Eigen, BLAS, LAPACK) can yield 10–50× speedup for matrix operations, which dominate the computational load.
• Fixed-point arithmetic: The Goldschmidt algorithm operates on scalar values with quadratic convergence, making it well-suited for fixed-point implementation on embedded DSPs or FPGAs. This reduces floating-point unit overhead and memory bandwidth requirements.
• GPU parallelization: The independent Goldschmidt iterations for different measurement channels, as well as the matrix-vector operations in the Kalman update, can be parallelized via CUDA or OpenCL. Benchmark studies of similar adaptive filtering pipelines report 20–100× speedups on embedded GPUs, e.g., NVIDIA Jetson AGX Xavier.
• Multi-resolution processing: A coarse-to-fine strategy—detecting and tracking at reduced resolution, e.g., 640 × 360, then refining only confirmed tracks at full resolution—can reduce per-frame computational cost by ~9× while maintaining accuracy for the regions of interest.
• Algorithmic simplifications: For applications with relaxed accuracy requirements, reducing the number of Goldschmidt iterations from 4 to 2–3 with acceptable precision loss <1% or using approximate matrix inversion methods can further reduce latency.

Based on empirical benchmarks of comparable Kalman-filter implementations, we estimate that a well-optimized C++/CUDA port of KGA could achieve 15–30 ms/frame on modern embedded GPU hardware, meeting the <16.7 ms threshold for 60 fps real-time operation. These optimizations represent engineering efforts rather than fundamental algorithmic limitations, and the core contribution of this paper—the integration of geometric correction, adaptive Kalman filtering, and Goldschmidt-based covariance tuning—remains valid and beneficial across deployment scenarios (Table 15).

Table 15. Hardware-oriented analysis of the advantages of the Goldschmidt algorithm.

Property	Standard Division	Goldschmidt (KGA)	Hardware Benefit
Operation type	Hardware division (20–40 cycles)	3–4 × FMA (≈12 cycles total)	$~60\%$ latency reduction
Branching	May stall pipeline on exception	Branchless iterative loop	Predictable WCET
Convergence	N/A (direct)	Quadratic $({\epsilon }_{\mathrm{i}+1}={{\epsilon }_{\mathrm{i}}}^2)$	Bounded 3–4 iterations
Precision support	FP32/FP64 only	Fixed-point friendly	Suitable for DSP/FPGA
Memory footprint	N/A	Single register for ${\alpha }_{\mathrm{k}}$	No buffer overhead vs. sliding window
Scalability	Sequential	Naturally parallelizable	SIMD/ GPU-friendly

Note: Cycle estimates based on ARM Cortex-A53 architecture. FMA = fused multiply-add. N/A = Not Applicable (the metric is not meaningful for this category).

These extensions represent engineering challenges rather than fundamental algorithmic limitations. The core contribution of this paper, the integration of geometric correction, Kalman filtering, and Goldschmidt-based adaptation, remains valid and beneficial across these deployment scenarios.

To overcome these limitations and expand functionality, future research opportunities include improving the filtering model, replacing the base dynamics model with one that more accurately describes vehicle kinematics, and deep hardware optimization of the algorithm’s computational units. The proposed KGA algorithm is a specialized tool optimal for UAV-based traffic monitoring in areas requiring rapid estimation of vehicle speed and position based on video streams.

8. Conclusions

The algorithm proposed in this paper is designed to accurately and reliably determine the spatial position (3D position) of a moving vehicle based on video data obtained from an unmanned aerial vehicle. Our solution is an iterative algorithm structured in three interconnected stages, each aimed at sequentially addressing key subproblems. Geometric correction and calibration ensure the transition from 2D pixel coordinates to 3D space using camera calibration and scene geometry through a vanishing point. This stage generates a physically meaningful coordinate system, which is a prerequisite for subsequent filtering. State prediction and correction using a Kalman filter smooths noise and predicts trajectories based on a dynamic motion model. This block ensures the stability of the estimate over time and allows for estimating not only the position but also the velocity of an object. Adaptation of filter parameters using the Goldschmidt algorithm is a key innovation that enables dynamic management of confidence in the model and measurements. The adaptation mechanism automatically adjusts the update rate and filter gain depending on the current data quality, which fundamentally distinguishes the algorithm from classic non-adaptive Kalman filter implementations.

Thus, the algorithm not only combines proven methods of projective geometry and optimal filtering but also complements them with a formalized adaptation mechanism. This approach ensures increased accuracy through the combined use of scene geometric constraints and adaptive temporal filtering; computational efficiency is achieved through the rapid convergence of the adaptation procedure based on the Goldschmidt algorithm; and robustness to changing shooting conditions and object motion, as confirmed by experimental results when working with various camera angles and data noise levels. Integrating the Goldschmidt algorithm into the Kalman filter loop reduces the mean square error of the estimation and improves convergence speed, making the proposed method (KGA) effective in both accuracy and computational efficiency compared to standard and modified versions of the Kalman filter (FK, MFK). While the current MATLAB implementation processes frames at $~1.6$ s/frame, the algorithmic structure is designed for hardware optimization: porting to C++/CUDA, fixed-point arithmetic, and embedded GPU acceleration could enable true real-time performance ($<16.7$ ms/frame) in future deployments.