Drone State Estimation Based on Frame‐to‐Frame Template Matching with Optimal Windows

1. Introduction

Drones can hover or fly while capturing videos from a distance. They can cover large and remote areas. This observation can be made from various viewpoints and altitudes [1,2,3]. They are also cost-effective and do not require highly trained personnel [4]. Flying drones also act as mobile sensing platforms; drones collect data during flight equipped with various sensors [5,6]. Therefore, the position and velocity information of drones are essential for navigation, surveillance, and other high-level tasks. For example, drones can follow a desired trajectory, track multiple targets, and optimize formation in swarm operations.

The drone's position is usually estimated by external sensors such as GPS and internal sensors such as IMU. GPS provides absolute position information but is vulnerable to external conditions [7,8]. IMU provides fast updates and require no external infrastructure, but its estimation performance degrades quickly due to drift errors that accumulate over time [8]. Localization methods using LiDAR or depth cameras have been developed, but they are typically limited to low-altitude or indoor conditions [9,10,11,12]. The RF signal sensor-based UAV navigation often requires pre-installed transmitters for accurate localization [13].

Vision-based localization techniques often rely on object template or feature matching through images [14]. Frame-to-frame template matching is commonly used to estimate displacement between consecutive frames [15]. It can track motion by matching a template from the previous frame to the current frame. Standard template matching works well for small displacements in environments without lighting, rotation, or scale changes [16]. A template is a region of interest (ROI) selected from a fixed area or object in the first frame or a reference image. Template selection may depend on terrain, objects, or explicit features [14,17]. Moving foreground regions were used as automatic template candidates with gaussian mixture modeling of the background [18]. Photometric property-based template selection was developed to choose templates depending on the intensity, contrast, or gradient of pixels [17,19]. Selecting a proper template is critical for accurate motion estimation. As scenes or objects change over time, templates need to be updated however, incorrect updates can lead to accumulated errors that degrade performance [20].

In the paper, the kinematic state of a drone in flight is estimated only from frames captured by the drone. The frame-to-frame template matching using optimal windows is proposed to measure the instantaneous velocity of the drone. The optimal window divides the frame into several regions to minimize the non-uniform spacing of the real coordinates.

Imaging projects 3D space on a 2D plane, and this projection can be modeled using principles of ray optics [21]. The image-to-position conversion converts the integer coordinates of pixels into continuous real-world coordinates [22]. During the conversion, the image size, the camera's horizontal and vertical angular field of view (AFOV), elevation, and tilt angle are assumed to be known. However, this conversion process generates non-uniform spacing in real-world coordinates. When the camera points straight down, the pixel spacing is the most uniform. In [23], an entire frame is set as a template to estimate the drone's speed from the vertical view. However, the spatial and visual information is one-sided, and the surveillance area becomes narrower in the vertical view. The optimal windows are contrived to overcome the non-uniform spacing distortion in the real-world coordinates. The height and location of the optimal windows are obtained from the piecewise linear segments that best fit the image-to-position conversion function in the vertical direction. The split points of the segments are determined so as to minimize the sum of least square errors of the seperate linear regression lines [24,25]. Therefore, multiple templates are independent of the scene and object. No additional process is required for the template update. The matching of each template is performed by minimizing the sum of normalized squared differences [26], and the instantaneous velocity is calculated from the average displacement obtained through multiple template matching.

The drone's kinematic state is estimated based on the measured velocity using a Kalman filter, which adopts a nearly constant acceleration (NCA) model [27]. The state of the Kalman filter is augmented to simultaneously estimate the drone’s state and bias in velocity [28]. Since the computational complexity of frame matching is high, the augmented-state Kalman filter with a zero-order hold scheme [29] on the measurements is adopted for faster processing. The zero-order hold Kalman filter reuses the measurement until new measurements are available.

Figure 1 shows a block diagram of the proposed method. First, the drone's velocity is measured by image-to-position conversion and frame-to-frame template matching using optimal windows. The measured instantaneous velocity is input to the Kalman filter. Next, the drone's state and bias in the velocity are estimated through the augmented Kalman filter.

In the experiment, a mini drone weighing less than 250 g [30] flies along two approximately 150 m long roads and captures video at 30 frame per second (FPS). One road is an urban environment, the other is a suburban environment. The drone starts from a stationary hovering position, accelerates to maximum speed, and continues to fly at a nearly constant speed at an altitude of 40 m and a camera tilt angle of 60 degrees. The frame size is 3840x2160 pixels, and the AFOV of the camera is set to 64 and 40 degrees in the horizontal and vertical directions, respectively. Ten flights were repeated on each road under various traffic conditions. For faster processing, three additional frame matching speeds (10, 3, 1 FPS) are tested using the zero-order hold Kalman filter. The proposed method is shown to achieve an average flight distance errors of 3.07-3.57 m and 1.97-2.39 m for Roads 1 and 2, respectively.

The contributions of this study are as follows: (1) Multiple template selection using optimal windows is proposed. The optimal windows are determined only by the image size, the camera’s AFOV, elevation, and tilt angle. Therefore, the template selection process is independent of the scene or object. (2) The augmented-state Kalman filter is designed to improve the accuracy of the drone’s state. The drone’s flight distance is estimated with high accuracy, resulting in low-meter-level average errors, (3) Real-time processing is possible with the zero-order hold Kalman filter. This method maintains similar error levels even when the frame matching speed is reduced to 1 FPS.

The rest of the paper is organized as follows: the real-world conversion and frame-to-frame template matching with optimal windows are described in Section 2. Section 3 explains drone state estimation with the augmented state Kalman filter. Section 4 presents the experimental results. Discussion and conclusions follow in Section 5 and Section 6, respectively.

2. Drone Velocity Measurement

This section describes how the instantaneous velocity is measured by frame-to-frame template matching with optimal windows.

2.1. Image-to-Position Conversion

The image-to-position conversion [22] applies trigonometry to compute real-world coordinates from image pixel coordinates assuming that the camera's AFOV, elevation, and tilt angles are known and the camera rotates only around the pitch axis. It provides a simple and direct conversion from pixel coordinates to real-world coordinates. However, the non-uniform spacing in the real-world coordinates arises as the tilt angle is larger and the altitude lower. The real-world position vector ${\mathit{x}}_{ij}$ corresponding to the (i, j) pixel is calculated as [22],

$$\begin{gathered} x_{ij}=\left(x_i,y_j\right)\approx \left(d_{{\displaystyle \frac{H}{2}}}·\mathrm{t}\mathrm{a}\mathrm{n}\left[\left(i-{\displaystyle \frac{W}{2}}+1\right){\displaystyle \frac{a_x}{W}}\right], h ·\mathrm{t}\mathrm{a}\mathrm{n}\left[{\theta }_T+\left({\displaystyle \frac{H}{2}}-j\right){\displaystyle \frac{a_y}{H}}\right]\right), \\ i=0,\dots ,W-1, j=0,\dots ,H-1, \end{gathered}$$

(1)

where W and H are the image sizes in the horizontal and vertical directions, respectively, h is the altitude of the drone or the elevation of the camera, $a_x$ and $a_y$ are the camera AFOV in the horizontal and vertical directions, respectively, and ${\theta }_T$ is the tilt angle of the camera. $d_{H/2}$ is the distance from the camera to ($x_{W/2-1}$, $y_{H/2}$,0), which is $\sqrt{y_{H/2}^2+h^2}$. Figure 2 illustrates the coordinate conversion from image to real-world [22].

Figure 3 visualizes the coordinate conversion function in horizontal and vertical directions according to Equation (1): W and H are set to 3840 and 2160 pixels, respectively; $a_x$ and $a_y$ are set to 64° and 40°, respectively; h is set to 40 m; ${\theta }_T$ is set to 60°. The nonlinearity increases rapidly as pixels move away from the center, resulting in non-uniform spacing in real-world coordinates, especially in Figure 3(b). This distortion should be remedied when calculating the actual displacement in the image. In the next subsection, we will see how to overcome these distortions using optimal windows.

2.2. Frame-to-Frame Template Matching with Optimal Windows

The instantaneous velocity is measured at k frame as

$${\mathit{z}}_m\left(k\right)=\left[\begin{array}{c}{z}_{mx}\left(k\right)\\ z_{my}\left(k\right)\end{array}\right]={\displaystyle \frac{1}{T}}{\displaystyle \frac{1}{N_w}}\sum _{n=1 }^{N_w}\left[{{\mathit{c}}_n-\mathit{p}}_n\left(k\right)\right],$$

(2)

$${\mathit{c}}_n=img2pos\left\{\left[\begin{array}{c}{c}_{nx}\\ c_{ny}\end{array}\right]\right\},$$

(3)

$${\mathit{p}}_n\left(k\right)=img2pos\left\{\left[\begin{array}{c}{p}_{nx}\left(k\right)+{\displaystyle \frac{W}{2}}\\ p_{ny}\left(k\right)+{\displaystyle \frac{H}{2}}\end{array}\right]\right\},$$

(4)

where T is the sampling time between two consecutive frames, $N_w$ is the number of optimal windows, or equivalently, the number of templates, ‘img2pos’ denotes the conversion process as in equation (1), and ($c_{nx}, c_{ny})$ is the center of the n-th window in pixel coordinates. In the experiments, $c_{nx}$ is set to W/2, and $c_{ny}\mathrm{i}\mathrm{s}\mathrm{s}\mathrm{e}\mathrm{t}\mathrm{t}\mathrm{o}$the center of the n-th linear segment. ($p_x p_y)$ is the displacement vector in pixel coordinates that minimizes the normalized sum of squared differences as

$$\left[\begin{array}{c}{p}_{nx}\left(k\right)\\ p_{ny}\left(k\right)\end{array}\right]={argmin}_{x,y}\left[{\displaystyle \frac{{\sum _{x\text{'},y\text{'}\in W_n}\left[I\left(x+x\text{'},y+y\text{'};k\right)-TM{P}_n\left(x\text{'}, y\text{'} ;k-1\right)\right]}^2}{\surd \left\{\sum _{x\text{'},y\text{'}\in W_n}{I\left(x+x\text{'},y+y\text{'};k\right)}^2\sum _{x\text{'},y\text{'}\in W_n}{TM{P}_n\left(x\text{'}, y\text{'};k-1\right)}^2\right\}}}\right],$$

(5)

$$TM{P}_n\left(x,y;k\right)=I\left(x,y;k\right), if x,y\in W_n,$$

(6)

where $W_n$ indicates the n-th window, and $I$ is the gray-scaled frame.

The image-to-position conversion function in the vertical direction is approximated by piecewise linear segments. The vertical length of each window is equal to the interval of each segment. The split points between segments are determined so that the sum of the least square errors of the separate linear models is minimized as follows:

$${\widehat{s}}_1,\dots ,{\widehat{s}}_{N_w-1}={argmin}_{s_1,\dots ,s_{N_w-1}}\left[\sum _{n=0}^{N_w-1}\sum _{j=s_n}^{s_{n+1}-1}{{min}_{a_n,b_n}\left[y_j-\left(a_{n}j+b_n\right)\right]}^2\right],$$

(7)

where $s_1,\dots ,s_{N_w-1}$ are $N_w-1$ split points for $N_w$ windows, and $s_0$ and $s_{N_w}$ are equal to 0 and $H$, respectively, and $a_n$ and $b_n$ are the coefficients of the n-th linear regression line. The number of windows is important. If there are too many windows, the sampling points (pixels) of one window may be too small, resulting in inaccurate displacements. If there are too few windows, the uneven spacing cannot be compensated for.

In the experiments, the number of windows is predetermined by 3 in the upper part and 2 in the lower part as it is desirable for the windows to be large and similar in size. The frame was cropped by 180 pixels near the edges to remove distortions that might occur during capture, resulting in optimal windows in an area of ​​3480 x 1800 pixels. Figure 4(a) shows the three linear regression lines of the vertical conversion function in the upper part, and Figure 4(b) shows the two linear regression lines of the lower part. Figure 5 shows 5 optimal windows and 4 split points on ae sample frame.

3. Drone State Estimation

3.1. System Modeling

The following augmented-state NCA model is adopted for the discrete state equation of a drone:

$$\mathit{x}\left(k+1\right)=F\left(T\right)\mathit{x}\left(k\right)+q\left(T\right)\mathit{v}\left(k\right)+q_n\mathit{n}\left(k\right),$$

(8)

$$F\left(T\right)=\left[\begin{array}{c}1 T {\displaystyle \frac{T^2}{2}} 0 0 0 0 0\\ 0 1 T 0 0 0 0 0\\ 0 0 1 0 0 0 0 0\\ 0 0 0 1 0 0 0 0\\ 0 0 0 0 1 T {\displaystyle \frac{T^2}{2}} 0\\ 0 0 0 0 0 1 T 0 \\ 0 0 0 0 0 0 1 0\\ 0 0 0 0 0 0 0 1\end{array}\right], q_v\left(T\right)=\left[\begin{array}{c}{\displaystyle \frac{T^2}{2}} 0\\ T 0\\ 0 0\\ 0 {\displaystyle \frac{T^2}{2}}\\ 0 T\\ 0 0\end{array}\right], q_n=\left[\begin{array}{c}0 0\\ 0 0\\ 1 0\\ 0 0\\ 0 0\\ 0 1\end{array}\right],$$

(9)

where $\mathit{x}\left(k\right)=[x\left(k\right) \dot{x}\left(k\right) {\ddot{x}\left(k\right) b}_x\left(k\right) y\left(k\right) \dot{y}\left(k\right) \ddot{y}\left(k\right) b_y(k){]}^t$ is the state vector of the drone at frame k, $x\left(k\right)$ and $y\left(k\right)$ are positions in the x and y directions, respectively, $\dot{x}\left(k\right)$ and$\dot{ y}\left(k\right)$ are velocities in the x and y directions, respectively, $\ddot{x}\left(k\right)$ and $\ddot{y}\left(k\right)$ are accelerations in the x and y directions, respectively, and $b_x\left(k\right)$ and $b_y\left(k\right)$ are biases in the x and y directions, respectively. $\mathit{v}\left(k\right)=[v_x\left(k\right) v_y\left(k\right){]}^t$is a process noise vector, which is Gaussian white noise with the covariance matrix $Q_v=diag\left([{\sigma }_{vx}^2 {\sigma }_{vy}^2]\right)$, and n$\left(k\right)=[n_x\left(k\right) n_y\left(k\right){]}^T$ is a bias noise vector, which is Gaussian white noise with the covariance matrix $Q_n=diag\left([{\sigma }_{nx}^2 {\sigma }_{ny}^2]\right)$. The measurement equation is as follows

$$\mathit{z}\left(k\right)=\left[\begin{array}{c}{z}_x\left(k\right)\\ z_y\left(k\right)\end{array}\right]=H\mathit{x}\left(k\right)+\mathit{w}\left(k\right),$$

(10)

$$H=\left[\begin{array}{c}0 1 0 1 0 0 0 0\\ 0 0 0 0 0 1 0 1\end{array}\right],$$

(11)

where $\mathit{w}\left(k\right)$ is a measurement noise vector, which is Gaussian white noise with the covariance matrix $R=diag\left(\left[r_x^2{ r}_y^2\right]\right)$.

3.2. Kalman Filtering

The state vector and covariance matrix are initialized, respectively, as follows

$$\widehat{\mathit{x}}\left(0|0\right)=\left[\begin{array}{c}0\\ z_{mx}\left(0\right)\\ b_x\left(0\right)\\ 0\\ z_{my}\left(0\right)\\ b_y\left(0\right) \end{array}\right], P\left(0|0\right)=\left[\begin{array}{c}100000\\ 010000\\ 00P_{x}000\\ 000100\\ 000010\\ 00000P_y\end{array}\right],$$

(12)

where $z_{mx}\left(0\right)$ and $z_{my}\left(0\right)$ are the measurements obtained from equation (2), $b_x\left(0\right)$ and $b_y\left(0\right)$ are the initial biases in x and y directions, respectively, and $P_x$ and $P_x$ are the initial covariances of the bias in x and y directions, respectively. The state and covariance predictions are iteratively computed as

$$\widehat{\mathit{x}}\left(k|k-1\right)=F\widehat{\mathit{x}}\left(k-1|k-1\right),$$

(13)

$$P\left(k|k-1\right)=FP\left(k-1|k-1\right)F^t+q_v\left(T\right)Q_v{q}_v(T{)}^t+q_n{Q}_n{q}_n^t.$$

(14)

Then, the state and covariance are updated as

$$\widehat{\mathit{x}}\left(k|k\right)=\widehat{\mathit{x}}\left(k|k-1\right)+W\left(k\right)\left[{\mathit{z}}_m\left(k\right)-H\widehat{\mathit{x}}\left(k|k-1\right)\right],$$

(15)

$$P\left(k\right|k)=P(k|k-1)-W\left(k\right)S\left(k\right)W(k{)}^T,$$

(16)

where the residual covariance $S\left(k\right)$ and the filter gain $W\left(k\right)$ are obtained as

$$S\left(k\right)=HP\left(k\right|k-1)H^t+R,$$

(17)

$$W\left(k\right)=P\left(k\right|k-1\left)H^{t}S\right(k{)}^{-1}.$$

(18)

When the zero-order hold scheme is applied, the measurement ${\mathit{z}}_m\left(k\right)$ in equation (15) is replaced by ${\mathit{z}}_{zoh}\left(k\right)$ as

$${\mathit{z}}_{zoh}\left(k\right)=\left\{\begin{array}{c}{\mathit{z}}_m\left(1\right), 1\le k<L-1,\\ {\mathit{z}}_m\left(L\right), L\le k<2L-1,\\ .\\ .\\ .\end{array}\right.,$$

(19)

where L-1 is the number of frames before a new frame matching occurs, thus the frame matching speed is frame rate (frame capture speed) divided by L.

4. Results

4.1. Scenario Description

A mini drone weighs less than 250 g (DJI Mini 4K) [30] flies along two different 150-meter long roads and captures videos at 30 FPS with a frame size of 3840x2160 pixels. Figure 6(a) shows a commercially available satellite image of a vehicle road in an urban environment (Road 1) while Figure 6(b) shows a vehicle road in a suburban environment (Road 2). The urban road has more complex backgrounds than the suburban road. The drone starts from a stationary hovering point O, reaches its maximum speed in normal flight mode before Point A, and then continues flying at a nearly constant speed passing near Points A, B, and C. For Road 1 and Road 2, the distances between Point O and Point A are approximately 57 m and 48 m, respectively, the distances between Point O and Point B are approximately 109 m and 100 m, respectively, and the distances between Point O and Point C are approximately 159 m and 150 m, respectively. The flights were repeated 10 times in different traffic conditions on each road. The drone altitude was set to 40 m, and the camera tilt angle was set to 60 degrees. The AFOV was assumed to be 64 degrees and 40 degrees in the horizontal and vertical directions, respectively.

Figure 7(a) and 7(b) show sample video frames when the drone passes near Points O, A, B, and C on Road 1 and 2, respectively. The optical center of the camera is marked with a white ‘+’, which is assumed to be the drone’s position.

4.2. Drone State Estimation

Table 1 shows the parameter values ​​of the augmented-state Kalman filter. The sampling time is set to 1/30 s. the process noise standard deviation is set to 5 m/s² in both x and y directions, the measurement noise standard deviation is set to 3 m/s in both x and y directions, and the bias noise standard deviation is set to 0.01 and 0.1 m/s in x and y directions, respectively. The initial covariance is set to the identical matrix except for the bias factors, which are set to 0.1 m²/s² in both x and y directions. The initial bias in the x direction is set to 0 m/s for both roads while the initial bias in the y direction is set to -1.2 to 0.2 m/s depending on the road traffic conditions and the frame matching speed. The 10 videos of Road 2 are divided into three groups and different initial values ​​are applied as shown in Table 2.

Table 3 and Table 4 show the distance errors from Point O to Point C of 10 videos for Roads 1 and 2, respectively. Distance errors are computed using measured or estimated velocities at various frame matching speeds from 30 to 1 FPS. The average distance error based on the measured velocity is 11.99 m to 17.20 m for Road 1 and 4.95 m to 5.79 m for Road 2. The average distance error based on the estimated velocity for Road 1 is 3.07 m to 3.52 m and the average distance error for Road 2 is 1.97 m to 2.39 m. As the frame matching speed decreases, the average distance errors of the measured velocities increase, but the average distance errors of the estimated velocities remain at a similar level showing the robustness of the proposed system.

Figure 8(a) shows the measured and estimated speeds of Video1 of Road 1 when the frame matching speed is 30 FPS. Figures 8(b) and 8(c) show the biases in the x and y directions, and the actual, measured, and estimated distances to Points A, B, and C. Figure 9, Figure 10 and Figure 11 show the cases where the frame matching speeds are 10, 3, and 1 FPS, respectively. Figure 12, Figure 13, Figure 14 and Figure 15 show the same cases of video 1 of Road 2.

Table 5 and Table 6 show the average distance errors to Points A, B, and C for Road 1 and Road 2, respectively. Table 5 shows a regular pattern, with the error increasing as the distance increases and the frame matching speed decreases. However, Table 6 shows a somewhat irregular pattern in the average error in the estimated velocity as a function of distance or matching speed. This is due to different initial bias values ​​which affect the performance.

Twenty supplementary files are movies in MP4 format, and they are available online. Supplemental material videos S1–S10 show 10 videos capturing Road 1, and Supplemental material videos S11–S20 show another 10 videos capturing Road 2. The optical center of the camera is marked as ‘+’ in blue color. As the drone passes Points A, B, and C, the color of the mark changes to white.

5. Conclusions

A novel frame-to-frame template matching method is proposed. The optimal windows are derived from ray optics principles and a piecewise linear regression model. Multiple templates are obtained by their corresponding optimal windows. Therefore, the templates are scene or object independent and no additional processes are required for template selection and update.

The Kalman filter adopts the NCA model and its state is augmented to estimate the velocity bias of the drone. The zero-order hold method was applied for faster processing. The proposed technique achieves low average flight distance errors even at slow frame matching speeds.

This technique can be useful when external infrastructure is not available such as GPS-denied environments. It could be applied to a variety of fields, including automatic programmed flight or multi ground target tracking using flying drones, which remains a subject of future research.