UAV Control on the Basis of 3D Landmark Bearing-Only Observations

Abstract

The article presents an approach to the control of a UAV on the basis of 3D landmark observations. The novelty of the work is the usage of the 3D RANSAC algorithm developed on the basis of the landmarks’ position prediction with the aid of a modified Kalman-type filter. Modification of the filter based on the pseudo-measurements approach permits obtaining unbiased UAV position estimation with quadratic error characteristics. Modeling of UAV flight on the basis of the suggested algorithm shows good performance, even under significant external perturbations.

1. Introduction

Modern UAV’s navigation systems use the standard elements of INS (inertial navigation systems) along with GPS, which permits correcting the bias and improving the UAV localization, which are necessary for resolving mapping issues, targeting and reconnaissance tasks [1]. The use of computer vision as a secondary or a primary method for autonomous navigation of UAVs has been discussed frequently in recent years, since the classical combination of GPS and INS systems cannot sustain autonomous flight in many situations [2]. UAV autonomous missions usually need so-called data fusion, which is a difficult task, especially for standard INS and vision equipment. It is clear that cameras provide visual information in a different form, inapplicable to UAV direct control, and therefore, one needs an additional on-board memory and special recognition algorithms.

1.1. Visual-Based Navigation Approaches

Several studies have demonstrated the effectiveness of approaches based on motion field estimation and feature tracking for visual odometry [3]. Vision-based methods have been proposed even in the context of autonomous landing management [2]. In [4], visual odometry based on geometric homography was proposed. However, the homography analysis uses only 2D reference points coordinates, though for the evaluation of the current UAV altitude, the 3D coordinates are necessary. All such approaches presume the presence of some recognition system in order to detect the objects nominated in advance. Examples of such objects can be special buildings, crossroads, tops of mountains, and so on. The principal difficulties are the different scale and aspect angles of observed and stored images, which leads to the necessity of huge template libraries in the memory of the UAV control system. Here, one can avoid this difficulty, because of the usage of another approach based on the observation of so-called feature points [5] that are scale and aspect angle invariant. For this purpose, the technology of feature points [6] is used. In [7], the approach based on the coordinate correspondence of the reference points observed by the on-board camera and the reference points on the map loaded into the UAV’s memory before the mission start had been suggested. During the flight, these maps are compared to the frame of the land, directly observed with the help of an on-board video camera. As a result, one can detect the current location and orientation without time-error accumulation. These methods are invariant to some transformations, and they are noise-stable, so that predetermined maps can be different in scale, aspect angle, season, luminosity, weather conditions, etc. This technology appeared in [8]. The contribution of this work is the usage of a modified unbiased pseudo-measurements filter for bearing-only observations of some reference points with known terrain coordinates.

1.2. Kalman Filter

In order to obtain metric data from visual observations, one needs first to make observations from different positions i.e., triangulation and then use nonlinear filtering. However, all nonlinear filters either have unknown bias [9] or are very difficult for on-board implementation, like the Bayesian-type estimation [10,11]. Approaches for position estimation based on bearing-only observations had been analyzed long ago, especially for submarine applications [12] and nowadays for UAV applications [1].

A comparison of different nonlinear filters for bearing-only observations in the issue of ground-based object localization [13] shows that the EKF (extended Kalman filter), the unscented Kalman filter, the particle filter and the pseudo-measurement filter give almost the same level of accuracy, while the pseudo-measurement filter is usually more stable and simple for on-board implementation. This observation is in accordance with older results [12], where all of these filters were compared in the issue of moving object localization. It has been mentioned that all of these filters have bias, which makes their use in data fusion issues rather problematic [14]. The principle requirement for such filters in data fusion is the non-biased estimate with the known mean square characterization of the error. Among the variety of possible filters, the pseudo-measurement filter can be easily modified to satisfy the data fusion demands. The idea of such nonlinear filtering was developed by V.S. Pugachev and I. Sinitsyn in the form of so-called conditionally-optimal filtering [15], which provides the non-biased estimation within the class of linear filters with the minimum mean squared error. In this paper, we develop such a filter (the so-called pseudo-measurement Kalman filter (PKF)) for the UAV position estimation and give the algorithm for path planning along with the reference trajectory under external perturbations and noisy measurements.

1.3. Optical Absolute Positioning

Some known aerospace maps of a terrain in a flight zone are loaded into the aircraft memory before the start of a flight. During the flight, these maps are compared to the frame of the land, directly observed with the help of an on-board video camera. For this purpose, the technology of feature points [6] is used. As a result, one can detect the current location and orientation without time-error accumulation. These methods are invariant to some transformations and are also noise-stable, so that predetermined maps can vary in height, season, luminosity, weather conditions, etc. Furthermore, from the moment of the previous plane surveying, the picture of this landscape can be changed due to human and natural activity. All approaches based on the capturing of the objects assigned in advance presume the presence of some on-board recognition system in order to detect and recognize such objects. Here, we avoid this difficulty by using the observation of feature points [5] that are scale and aspect angle invariant. In addition, the modified pseudo-measurements Kalman filtering (PKF) is used for the estimation of UAV positions and the control algorithm.

1.4. Outline of the Approach and the Article Structure

One of the principal parts of this research is an approach to the estimation of the UAV position on the basis of the bearing-only observations. The original filter that uses the idea of pseudo-measurements had been suggested in reference [16] for the case of the azimuth bearing of the terrain objects nominated in advance. In reference [17], this approach had been extended to the case of two angle measurements, namely azimuth and elevation. However, the usage of this approach as a real navigation tool needs huge on-board memory and a sophisticated recognition algorithm, since the template and in-flight observed images, even of the same object, are rather different due to the changes of illuminance, the altitude of flight and the aspect angles. That is why the method based on the observation of feature points looks more attractive for in-flight implementation. In reference [7], an algorithm joining together the feature points approach and modified PKF had been suggested, though for 2D feature point localization, while the more advanced 3D localization had been suggested in references [18,19], which are the shortened versions of the methodology presented in this article.

In this work, we use a computer simulation of a UAV flight and on-board video camera imaging. The simulation program is written in MATLAB. The type of feature points is ASIFT, realized in OpenCV (Python) [20]. Feature points in this model work as in a real flight, because the images for the camera model and for the template images were transformed by projective mapping and created by observations from different satellites.

The next section presents the original RANSAC algorithm for 3D feature point localization. Section 3 and Section 4 give the description of PKF, providing the unbiased estimation of the UAV position with the estimate of quadratic errors. Section 5 describes the locally optimal control algorithm for tracking the reference trajectory on the basis of PKF estimation of the UAV position. In Section 6, we give a new approach to the RANSAC robustness with the use of the UAV motion model. Section 7 presents the modeling results, and Section 8 is the conclusions.

2. Random Sample Consensus for Isometry

At every step, the algorithm deals with two images of a 3D landscape. An example of the landscape used for modeling is shown in Figure 1.

Figure 1. (a) Image $I_c$ of the on-board camera of the UAV; (b) template image loaded in the UAV memory in advance.

Figure 1. (a) Image $I_c$ of the on-board camera of the UAV; (b) template image loaded in the UAV memory in advance.

The first image $I_c$ is obtained from the on-board UAV camera, the position of which is unknown and should be estimated. The second image $I_m$ was taken previously from a known position and uploaded into the UAV memory. The ASIFT method is used for both images to detect feature points, which are specified in pixels:

$$c_i=\left|\begin{array}{c}{c}_{xi}\\ c_{yi}\end{array}\right|$$

and:

$$m_i=\left|\begin{array}{c}{m}_{xi}\\ m_{yi}\end{array}\right|$$

and calculates their descriptors. The correspondence between images is constructed by using these descriptors, and thereby, the feature points are combined in pairs. However, many of these pairs are wrong, and therefore, these pairs are considered as outliers or they are not. The result of ASIFT correspondence is shown in Figure 2.

The Earth coordinate system is the Cartesian coordinate system, which is rigidly connected with the Earth. Therefore, the algorithm uses a 3D terrain map of the area from which the image $I_m$ was taken and over which the UAV flies. Therefore, one can determine the coordinates of the points:

$$r_i=\left|\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right|$$

which generated $m_i$ points in the Earth coordinate system. However, if i corresponds to the pair of points that is not an outlier, then the point $r_i$ also generates a point $c_i$ in the UAV camera.

Figure 2. (a) Image $I_c$ of the on-board camera of the UAV; (b) template image loaded in the UAV memory in advance.

Figure 2. (a) Image $I_c$ of the on-board camera of the UAV; (b) template image loaded in the UAV memory in advance.

Another Cartesian coordinate system is rigidly connected with the UAV camera. The axis of the camera is parallel to the axis z. The transformation from the Earth coordinate system to the UAV coordinate system has the form:

$$r^{\prime }=A(r-b)$$

where b represents the coordinates of the camera in the Earth coordinate system and A is the orthogonal ($A{A}^T=I$) rotation matrix defining the orientation of the UAV camera. Then, the points $r_i$ in the camera coordinate system are:

$$r_i^{\prime }=A(r_i-b)$$

To define the relation between $r_i$ and the feature points $c_i$, one can use the model of the camera obscura. This model gives a central projection on the plane:

$$c_i=\left|\begin{array}{c}\frac{{\rho }_{xi}}{{\rho }_{zi}}\\ \frac{{\rho }_{yi}}{{\rho }_{zi}}\end{array}\right|$$

where:

$$\left|\begin{array}{c}{\rho }_{xi}\\ {\rho }_{yi}\\ {\rho }_{zi}\end{array}\right|={\rho }_i=K{r}_i^{\prime }=KA(r_i-b)$$

where K is a known calibration matrix of the camera.

Thus, the task is to estimate A and b on the basis of known $c_i,$ $r_i,$ $K.$ The minimum number of feature point pairs needed to solve this task is three.

One can give the solution of the problem under the assumption that there are just three pairs:

$$i=\{1,2,3\}$$

Points $r_i^{\prime }$ form a triangle in the space with the following sides:

$${\rho }_1=\left|\right|r_2^{\prime }-r_3^{\prime }{\left|\right|}_2,{\rho }_2=\left|\right|r_3^{\prime }-r_1^{\prime }{\left|\right|}_2,{\rho }_3=\left|\right|r_1^{\prime }-r_2^{\prime }{\left|\right|}_2$$

Meanwhile, due to the rectilinear propagation of light, each point $r_i^{\prime }$ lies on the beam $r^{\prime }=a_{i}t$, where t is a scalar parameter, and:

$$a_i=K^{-1}\left|\begin{array}{c}{c}_{xi}\\ c_{yi}\\ 1\end{array}\right|$$

In order to find $r_i^{\prime }$, we have to determine parameters $t_i,$ $i=\{1,2,3\}$ that satisfy the system of quadratic equations:

$$\left\{\begin{array}{c}{(a_2{t}_2-a_3{t}_3)}^T(a_2{t}_2-a_3{t}_3)={\rho }_1^2\hfill \\ {(a_3{t}_3-a_1{t}_1)}^T(a_3{t}_3-a_1{t}_1)={\rho }_2^2\hfill \\ {(a_1{t}_1-a_2{t}_2)}^T(a_1{t}_1-a_2{t}_2)={\rho }_3^2\hfill \end{array}\right.$$

For the given $t_1$, this system may be either solved analytically or has no solution. A determination of $t_1$ can be done numerically, for example by the bisection method.

Finally, one can obtain the coordinates of three points on the Earth’s surface in the camera coordinate system $r_i^{\prime }$ and, at the same time, in the Earth coordinate system $r_i$. The connection between them is: $r_i^{\prime }=A(r_i-b)$. Since A is the orthogonal matrix, then $y=Ax$ implies ${\left|\right|y\left|\right|}_2={\left|\right|x\left|\right|}_2$; thereby:

$$r_i^{\prime T}{r}_i^{\prime }={(r_i-b)}^T(r_i-b)$$

Therefore, we have eliminated A and obtained the problem of finding the intersection of three spheres, which can be solved analytically. This problem may have two solutions; one of them will be rejected later. When b has been found, solutions for A are as follows:

$$A=\left|\begin{array}{ccc}{r}_1^{\prime }& r_2^{\prime }& r_3^{\prime }\end{array}\right|{\left|\begin{array}{ccc}{r}_1-b& r_2-b& r_3-b\end{array}\right|}^{-1}$$

Therefore, there are two options, and only one of them is correct:

$$\begin{array}{c}\left|\begin{array}{c}{a}_{11}\\ a_{21}\\ a_{31}\end{array}\right|=\left|\begin{array}{c}{a}_{12}\\ a_{22}\\ a_{32}\end{array}\right|\times \left|\begin{array}{c}{a}_{13}\\ a_{23}\\ a_{33}\end{array}\right|\mathrm{or}\left|\begin{array}{c}{a}_{11}\\ a_{21}\\ a_{31}\end{array}\right|=-\left|\begin{array}{c}{a}_{12}\\ a_{22}\\ a_{32}\end{array}\right|\times \left|\begin{array}{c}{a}_{13}\\ a_{23}\\ a_{33}\end{array}\right|\hfill \end{array}$$

If the first one is correct, then the second one corresponds exactly to the turnover.

As a result, A and b have been found by using three pairs of feature points and the height map. However, this approach alone is not suitable as the final solution, due to the following problems:

• The method gives either knowingly false solution or no solution at all if among the three points there are outliers.
• There is a strong dependence on the noise in the feature points’ location.

Both problems may be solved with random sample consensus (RANSAC) [21,22]. From the general selection of points, one needs to select N times a subsample of size three. For each subsample $j=\{1,2,\cdots ,N\}$, one can calculate $A_j$ and $b_j$, which allows one to simulate the generation of all feature points on the UAV camera:

$$c_{ji}=\left|\begin{array}{c}\frac{{\rho }_{jxi}}{{\rho }_{jzi}}\\ \frac{{\rho }_{jyi}}{{\rho }_{jzi}}\end{array}\right|,{\rho }_j=K{r}_{ji}^{\prime }=K{A}_j(r_{ji}-b_j)$$

Then, one can evaluate which points are the outliers by the threshold $s_{ji}=\mathbf{1}\left(\right||c_i-c_{ji}{\left|\right|}_2<d)$, where d is the threshold. Here, $s_{ji}=0$ means that projection of the i-th point on the j-th point is counted as an outlier, otherwise $s_{ji}=1.$ The answer will be the following:

$$A=A_J,b=b_J,J=arg\underset{j}{max}\sum _i{s}_{ji}$$

Therefore, we really solve the problem of outliers. Next, we find the required number of N subsamples of a size of three, such that among them, there will be at least one subsample without outliers with probability $p.$ Let the proportion of outliers be $1-w$. It is easy to see that [21]:

$$N\left(p\right)=\frac{log(1-p)}{log(1-w^3)}$$

In the case when $w=\frac{1}{2}$: $N(0.9999)\approx 69$, which shows the high efficiency of algorithm. After that, the points marked as outliers are removed from consideration. The clarification of the response is made by the numerical solution of the following optimization problem on the set of remaining points:

$$\{A^{*},b^{*}\}=arg\underset{A,b}{min}\sum _i\left|\right|c_i-c_i(A,b){\left|\right|}_2^2$$

Thereby, the second problem of noise reduction may be solved. However, one can use a more advanced procedure, which takes into account the motion model. A more stable solution may be obtained with the aid of so-called robust RANSAC [23]; the idea is to use predicted values of $(A,b)$ for the preliminary rejection of outliers from the pairs of observed feature points. Therefore, if on the k-th step of the filtering procedure, the values $(A_k,b_k)$ have been obtained, one can use the following values on the $(k+1)$-th step:

$$(A_{k+1},b_{k+1})=(A_k,{\widehat{b}}_{k+1})$$

(1)

where ${\widehat{b}}_{k+1}$ is the predicted estimate of the UAV attitude obtained on the basis of the PKF estimate. The filtering approach is described in Section 5.

3. Filtering Problem Statement

The problem of bearing-only filtering is considered to determine the coordinates of the UAV, which can observe some objects with known coordinates. These objects can be either the well recognizable objects or a network of radio-beacon stations with a well-specified frequency and known coordinates. In this work, the function of beacons is performed by the feature points determined with the aid of the RANSAC algorithm. The UAV has the standard set of INS devices, which enables it to perform the flight with some degree of accuracy, which, however, is not sufficient for mission completion.

3.1. Model of the UAV’s Motion

We assume that a UAV motion is described by three coordinates $(X\left(t_k\right),Y\left(t_k\right),Z\left(t_k\right))$ and velocities $(V_x\left(t_k\right),V_y\left(t_k\right),V_z\left(t_k\right)).$ At times $t_k=k\Delta t,k=1,2,...$, these coordinates satisfy the following equations:

$$\mathbf{X}\left(t_{k+1}\right)=F\mathbf{X}\left(t_k\right)+B\mathbf{a}\left(t_k\right)+\mathbf{W}\left(t_k\right)$$

(2)

where:

$$\mathbf{X}\left(t_k\right)={(X\left(t_k\right),Y\left(t_k\right),Z\left(t_k\right),V_x\left(t_k\right),V_y\left(t_k\right),V_z\left(t_k\right))}^T$$

is the vector of state-velocities,

$$\mathbf{a}\left(t_k\right)={(a_x\left(t_k\right),a_y\left(t_k\right),a_z\left(t_k\right))}^T$$

is the vector of accelerations, which we consider as controls,

$$\mathbf{W}\left(t_k\right)={(0,0,0,W_x\left(t_k\right),W_y\left(t_k\right),W_z\left(t_k\right))}^T$$

is the vector of current perturbations, modeling the turbulence components of the wind and the autopilot errors, as well. The matrices A and B are equal:

$$F=\left(\begin{array}{cccccc}1& 0& 0& \Delta t& 0& 0\\ 0& 1& 0& 0& \Delta t& 0\\ 0& 0& 1& 0& 0& \Delta t\\ 0& 0& 0& 1& 0& 0\\ 0& 0& 0& 0& 1& 0\\ 0& 0& 0& 0& 0& 1\end{array}\right)$$

$$B=\left(\begin{array}{ccc}\frac{\Delta t^2}{2}& 0& 0\\ 0& \frac{\Delta t^2}{2}& 0\\ 0& 0& \frac{\Delta t^2}{2}\\ \Delta t& 0& 0\\ 0& \Delta t& 0\\ 0& 0& \Delta t\end{array}\right)$$

and stochastic Equation (2) describes a controlled and perturbed UAV motion.

3.2. Measurements

Assume that $(X_i,Y_i,Z_i)$ are the coordinates of the i-th reference point and ${\varphi }_i\left(t_k\right),{\lambda }_i\left(t_k\right)$ are the bearing angles on that point. The measuring scheme is shown in Figure 3.

Figure 3. Scheme of the measurements of bearing angles. $X_i,Y_i,Z_i$ are the coordinates of the i-th feature point; $\lambda \left(t_k\right),\varphi \left(t_k\right)$ are the elevation and azimuth bearing angles, measured at the moment $t_k.$

Figure 3. Scheme of the measurements of bearing angles. $X_i,Y_i,Z_i$ are the coordinates of the i-th feature point; $\lambda \left(t_k\right),\varphi \left(t_k\right)$ are the elevation and azimuth bearing angles, measured at the moment $t_k.$

At moment $t_k$, these angles satisfy the relations:

$$\begin{array}{c}\frac{{\displaystyle Y_i+{\epsilon }_k^y-Y\left(t_k\right)}}{{\displaystyle X_i+{\epsilon }_k^x-X\left(t_k\right)}}{I}_i\left(t_k\right)=tan{\varphi }_i\left(t_k\right)+{\epsilon }_k^{\varphi }\hfill \\ \\ \frac{{\displaystyle Z_i+{\epsilon }_k^z-Z\left(t_k\right)}}{{\displaystyle \sqrt{{(X_i+{\epsilon }_k^x-X\left(t_k\right))}^2+{(Y_i+{\epsilon }_k^y-Y\left(t_k\right))}^2}}}{I}_i\left(t_k\right)=tan{\lambda }_i\left(t_k\right)+{\epsilon }_k^{\lambda }\hfill \end{array}$$

(3)

where ${\epsilon }_k^x\sim \mathcal{WN}(0,{\sigma }_x^2),$ ${\epsilon }_k^y\sim \mathcal{WN}(0,{\sigma }_y^2),$ ${\epsilon }_k^z\sim \mathcal{WN}(0,{\sigma }_z^2),$ ${\epsilon }_k^{\varphi }\sim \mathcal{WN}(0,{\sigma }_{\varphi }^2),$ ${\epsilon }_k^{\lambda }\sim \mathcal{WN}(0,{\sigma }_{\lambda }^2)$ are uncorrelated random variables with zero means and variances ${\sigma }_x^2,$ ${\sigma }_y^2,$ ${\sigma }_z^2,$ ${\sigma }_{\varphi }^2,{\sigma }_{\lambda }^2$, defined as errors in the measurement of the coordinates of the i-th reference point and of the tangents of angles ${\varphi }_i\left(t_k\right),{\lambda }_i\left(t_k\right)$ and forming the white noise sequences.

Remark 1. 

In the majority of works based on the method of pseudo-measurements, another model is used. It assumes the measurements of the angles with Gaussian errors (see [12,14] and most of the successive works). However, in the real definition of the object position in the image or in the matrix of sensors, the system measures the distance between the object image and the center of the sensor, that is the tangent of the bearing angle. This simple observation allows one to find the unbiased estimate of the UAV coordinates.

One can rewrite Equation (3) for angle ${\lambda }_i\left(t_k\right)$ as follows:

$$\frac{{\displaystyle Z_i+{\epsilon }_k^z-Z\left(t_k\right)}}{{\displaystyle Y_i+{\epsilon }_k^y-Y\left(t_k\right)}}sin{\varphi }_i\left(t_k\right)I_i\left(t_k\right)=\frac{{\displaystyle sin{\lambda }_i\left(t_k\right)}}{{\displaystyle cos{\lambda }_i\left(t_k\right)}}+{\epsilon }_k^{\lambda }$$

(4)

Remark 2. 

The indicator function $I_i\left(t_k\right)=1$ if at $t_k$ the bearing of the i-th reference point occurs, and $I_i\left(t_k\right)=0$ otherwise. For convenience, we assume that $I_i\left(t_k\right)=1.$

Therefore, at the moment $t_k$, the UAV control system determines the angles ${\varphi }_i\left(t_k\right)$ and ${\lambda }_i\left(t_k\right)$, related to the coordinates of the UAV $(X\left(t_k\right),Y\left(t_k\right),Z\left(t_k\right))$ as follows:

$$\begin{array}{c}(Y_i+{\epsilon }_k^y-Y\left(t_k\right))cos{\varphi }_i\left(t_k\right)-(X_i+{\epsilon }_k^x-X\left(t_k\right))sin{\varphi }_i\left(t_k\right)\hfill \\ ={\epsilon }_k^{\varphi }(X_i+{\epsilon }_k^x-X\left(t_k\right))cos{\varphi }_i\left(t_k\right)\hfill \\ \\ (Z_i+{\epsilon }_k^z-Z\left(t_k\right))sin{\varphi }_i\left(t_k\right)cos{\lambda }_i\left(t_k\right)-(Y_i+{\epsilon }_k^y-Y\left(t_k\right))sin{\lambda }_i\left(t_k\right)\hfill \\ ={\epsilon }_k^{\lambda }(Y_i+{\epsilon }_k^y-Y\left(t_k\right))cos{\lambda }_i\left(t_k\right)\hfill \end{array}$$

(5)

4. Modified Kalman Filtering on the Basis of Pseudo-Measurements

4.1. Linear Measurements Model

The idea of the pseudo-measurement method is to separate in Equation (5) the observable and non-observable values, which gives the following observation equations:

$$\begin{array}{c}{m}_k^{\varphi }=Y_{i}cos{\varphi }_i\left(t_k\right)-X_{i}sin{\varphi }_i\left(t_k\right)=Y\left(t_k\right)cos{\varphi }_i\left(t_k\right)-X\left(t_k\right)sin{\varphi }_i\left(t_k\right)\hfill \\ -{\epsilon }_k^{y}cos{\varphi }_i\left(t_k\right)+{\epsilon }_k^{x}sin{\varphi }_i\left(t_k\right)+{\epsilon }_k^{\varphi }(X_i+{\epsilon }_k^x-X\left(t_k\right))cos{\varphi }_i\left(t_k\right)\hfill \\ \\ m_k^{\lambda }=Z_{i}sin{\varphi }_i\left(t_k\right)cos{\lambda }_i\left(t_k\right)-Y_{i}sin{\lambda }_i\left(t_k\right)=Z\left(t_k\right)sin{\varphi }_i\left(t_k\right)cos{\lambda }_i\left(t_k\right)-Y\left(t_k\right)sin{\lambda }_i\left(t_k\right)\hfill \\ -{\epsilon }_k^{z}sin{\varphi }_i\left(t_k\right)cos{\lambda }_i\left(t_k\right)+{\epsilon }_k^{y}sin{\lambda }_i\left(t_k\right)+{\epsilon }_k^{\lambda }(Y_i+{\epsilon }_k^y-Y\left(t_k\right))cos{\lambda }_i\left(t_k\right)\hfill \end{array}$$

(6)

where $X_i,Y_i,Z_i$ represent the coordinates of the i-th feature point determined with the aid of the RANSAC algorithm and ${\varphi }_i,{\lambda }_i$ are the corresponding observable bearing angles measured by the system. Thus, the left-hand side of Equation (6), that is $(m_k^{\varphi },m_k^{\lambda })$, corresponds to the observable values, whereas the right-hand side containing the coordinates of the UAV corresponds to the non-observable ones. The aim is to estimate the coordinates and velocities of the UAV on the basis of linear observation Equation (6) and the motion model Equation (2). Therefore, the measurement vector has the following form:

$$\begin{array}{c}{m}_k=\left(\begin{array}{c}{m}_k^{\varphi }\hfill \\ m_k^{\lambda }\hfill \end{array}\right)=\left(\begin{array}{c}Y\left(t_k\right)cos{\varphi }_i\left(t_k\right)-X\left(t_k\right)sin{\varphi }_i\left(t_k\right)-{\epsilon }_k^{y}cos{\varphi }_i\left(t_k\right)+{\epsilon }_k^{x}sin{\varphi }_i\left(t_k\right)\hfill \\ +{\epsilon }_k^{\varphi }(X_i+{\epsilon }_k^x-X\left(t_k\right))cos{\varphi }_i\left(t_k\right)\hfill \\ \\ Z\left(t_k\right)sin{\varphi }_i\left(t_k\right)cos{\lambda }_i\left(t_k\right)-Y\left(t_k\right)sin{\lambda }_i\left(t_k\right)-{\epsilon }_k^{z}sin{\varphi }_i\left(t_k\right)cos{\lambda }_i\left(t_k\right)\hfill \\ +{\epsilon }_k^{y}sin{\lambda }_i\left(t_k\right)+{\epsilon }_k^{\lambda }(Y_i+{\epsilon }_k^y-Y\left(t_k\right))cos{\lambda }_i\left(t_k\right)\hfill \end{array}\right)\hfill \end{array}$$

(7)

Thereby, we obtain the system Equation (7) of linear measurement equations, though the noise variance depends on non-observable coordinates. By using V.S. Pugachev’s method [15], one can obtain the unbiased estimate and the variance with the aid of a prediction-correction filter [24]. Moreover, we do not need to assume the Gaussian distribution of errors that is not valid in bearing observations with optical-electronic cameras with discrete image sensors.

4.2. Prediction-Correction Estimation

Assume that at the moment $t_k$, we have unbiased estimates $\widehat{\mathbf{X}}\left(t_k\right)$, such that:

$$\begin{array}{c}E(\widehat{\mathbf{X}}\left(t_k\right))=\mathbf{X}\left(t_k\right)\hfill \end{array}$$

(8)

with the following matrix of the mean-square errors:

$$\begin{array}{c}\widehat{P}\left(t_k\right)=E\left\{(\widehat{\mathbf{X}}\left(t_k\right)-\mathbf{X}\left(t_k\right)){(\widehat{\mathbf{X}}\left(t_k\right)-\mathbf{X}\left(t_k\right))}^T\right\}\hfill \\ \\ =\left(\begin{array}{cccccc}{\widehat{P}}^{xx}\left(t_k\right)& {\widehat{P}}^{xy}\left(t_k\right)& ...& ...& ...& {\widehat{P}}^{x{V}_z}\left(t_k\right)\\ {\widehat{P}}^{xy}\left(t_k\right)& {\widehat{P}}^{yy}\left(t_k\right)& ...& ...& ...& {\widehat{P}}^{y{V}_z}\left(t_k\right)\\ {\widehat{P}}^{xz}\left(t_k\right)& {\widehat{P}}^{yz}\left(t_k\right)& ...& ...& ...& {\widehat{P}}^{z{V}_z}\left(t_k\right)\\ {\widehat{P}}^{x{V}_x}\left(t_k\right)& {\widehat{P}}^{y{V}_x}\left(t_k\right)& ...& ...& ...& {\widehat{P}}^{V_x{V}_z}\left(t_k\right)\\ {\widehat{P}}^{x{V}_y}\left(t_k\right)& {\widehat{P}}^{y{V}_y}\left(t_k\right)& ...& ...& ...& {\widehat{P}}^{V_y{V}_z}\left(t_k\right)\\ {\widehat{P}}^{x{V}_z}\left(t_k\right)& {\widehat{P}}^{y{V}_z}\left(t_k\right)& ...& ...& ...& {\widehat{P}}^{V_z{V}_z}\left(t_k\right)\end{array}\right)\hfill \end{array}$$

(9)

Problem 1. 

Find the unbiased estimates $\widehat{\mathbf{X}}\left(t_{k+1}\right)$ and matrix $\widehat{P}\left(t_{k+1}\right)$ on the basis of estimates at the moment $t_k,$ $m_k$, the known position of the i-th observable object $(X_i,Y_i,Z_i)$ and the UAV’s motion parameter Equation (2). These estimates must satisfy Equation (8) and give the matrix Equation (9) for the moment $t_{k+1}$.

4.2.1. Prediction

The prediction is obtained by assuming that at the moment $t_{k+1}$, the values of $\varphi \left(t_{k+1}\right),$ $\lambda \left(t_{k+1}\right)$ will be known:

$$\begin{array}{c}\tilde{\mathbf{X}}\left(t_{k+1}\right)=F\widehat{\mathbf{X}}\left(t_k\right)+B\mathbf{a}\left(t_k\right)\hfill \\ \\ {\tilde{m}}_{k+1}=\left(\begin{array}{c}{\tilde{m}}_{k+1}^{\varphi }\\ {\tilde{m}}_{k+1}^{\lambda }\end{array}\right)=\left(\begin{array}{c}I\left(t_{k+1}\right)(\tilde{Y}\left(t_{k+1}\right)cos\varphi \left(t_{k+1}\right)-\tilde{X}\left(t_{k+1}\right)sin\varphi \left(t_{k+1}\right))\\ I\left(t_{k+1}\right)(\tilde{Z}\left(t_{k+1}\right)sin\varphi \left(t_{k+1}\right)cos\lambda \left(t_{k+1}\right)-\tilde{Y}\left(t_{k+1}\right)sin\lambda (t_{k+1}))\end{array}\right)\hfill \end{array}$$

(10)

Assuming that the motion perturbations and the UAV position are uncorrelated, we obtain:

$$\begin{array}{c}\tilde{P}\left(t_{k+1}\right)=\left(\begin{array}{cccccc}{\tilde{P}}^{xx}\left(t_{k+1}\right)& {\tilde{P}}^{xy}\left(t_{k+1}\right)& ...& ...& ...& {\tilde{P}}^{x{V}_z}\left(t_{k+1}\right)\\ {\tilde{P}}^{xy}\left(t_{k+1}\right)& {\tilde{P}}^{yy}\left(t_{k+1}\right)& ...& ...& ...& {\tilde{P}}^{y{V}_z}\left(t_{k+1}\right)\\ {\tilde{P}}^{xz}\left(t_{k+1}\right)& {\tilde{P}}^{yz}\left(t_{k+1}\right)& ...& ...& ...& {\tilde{P}}^{z{V}_z}\left(t_{k+1}\right)\\ {\tilde{P}}^{x{V}_x}\left(t_{k+1}\right)& {\tilde{P}}^{y{V}_x}\left(t_{k+1}\right)& ...& ...& ...& {\tilde{P}}^{V_x{V}_z}\left(t_{k+1}\right)\\ {\tilde{P}}^{x{V}_y}\left(t_{k+1}\right)& {\tilde{P}}^{y{V}_y}\left(t_{k+1}\right)& ...& ...& ...& {\tilde{P}}^{V_y{V}_z}\left(t_{k+1}\right)\\ {\tilde{P}}^{x{V}_z}\left(t_{k+1}\right)& {\tilde{P}}^{y{V}_z}\left(t_{k+1}\right)& ...& ...& ...& {\tilde{P}}^{V_z{V}_z}\left(t_{k+1}\right)\end{array}\right)\hfill \end{array}$$

(11)

where the elements of this matrix are:

$$\begin{array}{c}{\tilde{P}}^{xx}\left(t_{k+1}\right)={\widehat{P}}^{xx}\left(t_k\right)+2{\widehat{P}}^{x{V}_x}\left(t_k\right)\Delta t+{\widehat{P}}^{V_x{V}_x}\left(t_k\right)\Delta t^2\hfill \\ {\tilde{P}}^{yy}\left(t_{k+1}\right)={\widehat{P}}^{yy}\left(t_k\right)+2{\widehat{P}}^{y{V}_y}\left(t_k\right)\Delta t+{\widehat{P}}^{V_y{V}_y}\left(t_k\right)\Delta t^2\hfill \\ {\tilde{P}}^{zz}\left(t_{k+1}\right)={\widehat{P}}^{zz}\left(t_k\right)+2{\widehat{P}}^{z{V}_z}\left(t_k\right)\Delta t+{\widehat{P}}^{V_z{V}_z}\left(t_k\right)\Delta t^2\hfill \\ {\tilde{P}}^{xy}\left(t_{k+1}\right)={\widehat{P}}^{xy}\left(t_k\right)+{\widehat{P}}^{x{V}_y}\left(t_k\right)\Delta t+{\widehat{P}}^{y{V}_x}\left(t_k\right)\Delta t+{\widehat{P}}^{V_x{V}_y}\left(t_k\right)\Delta t^2\hfill \\ {\tilde{P}}^{xz}\left(t_{k+1}\right)={\widehat{P}}^{xz}\left(t_k\right)+{\widehat{P}}^{x{V}_z}\left(t_k\right)\Delta t+{\widehat{P}}^{z{V}_x}\left(t_k\right)\Delta t+{\widehat{P}}^{V_x{V}_z}\left(t_k\right)\Delta t^2\hfill \\ {\tilde{P}}^{yz}\left(t_{k+1}\right)={\widehat{P}}^{yz}\left(t_k\right)+{\widehat{P}}^{y{V}_z}\left(t_k\right)\Delta t+{\widehat{P}}^{z{V}_y}\left(t_k\right)\Delta t+{\widehat{P}}^{V_y{V}_z}\left(t_k\right)\Delta t^2\hfill \\ {\tilde{P}}^{x{V}_x}\left(t_{k+1}\right)={\widehat{P}}^{x{V}_x}\left(t_k\right)+{\widehat{P}}^{V_x{V}_x}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{x{V}_y}\left(t_{k+1}\right)={\widehat{P}}^{x{V}_y}\left(t_k\right)+{\widehat{P}}^{V_x{V}_y}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{x{V}_z}\left(t_{k+1}\right)={\widehat{P}}^{x{V}_z}\left(t_k\right)+{\widehat{P}}^{V_x{V}_z}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{y{V}_x}\left(t_{k+1}\right)={\widehat{P}}^{y{V}_x}\left(t_k\right)+{\widehat{P}}^{V_x{V}_y}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{y{V}_y}\left(t_{k+1}\right)={\widehat{P}}^{y{V}_y}\left(t_k\right)+{\widehat{P}}^{V_y{V}_y}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{y{V}_z}\left(t_{k+1}\right)={\widehat{P}}^{y{V}_z}\left(t_k\right)+{\widehat{P}}^{V_y{V}_z}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{z{V}_x}\left(t_{k+1}\right)={\widehat{P}}^{z{V}_x}\left(t_k\right)+{\widehat{P}}^{V_x{V}_z}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{z{V}_y}\left(t_{k+1}\right)={\widehat{P}}^{z{V}_y}\left(t_k\right)+{\widehat{P}}^{V_y{V}_z}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{z{V}_z}\left(t_{k+1}\right)={\widehat{P}}^{z{V}_z}\left(t_k\right)+{\widehat{P}}^{V_z{V}_z}\left(t_k\right)\Delta t\hfill \\ {\tilde{P}}^{V_x{V}_x}\left(t_{k+1}\right)={\widehat{P}}^{V_x{V}_x}\left(t_k\right)+{\sigma }_{\mathbf{X}}^2\hfill \\ {\tilde{P}}^{V_y{V}_y}\left(t_{k+1}\right)={\widehat{P}}^{V_y{V}_y}\left(t_k\right)+{\sigma }_{\mathbf{Y}}^2\hfill \\ {\tilde{P}}^{V_z{V}_z}\left(t_{k+1}\right)={\widehat{P}}^{V_z{V}_z}\left(t_k\right)+{\sigma }_{\mathbf{Z}}^2\hfill \\ {\tilde{P}}^{V_x{V}_y}\left(t_{k+1}\right)={\widehat{P}}^{V_x{V}_y}\left(t_k\right)\hfill \\ {\tilde{P}}^{V_x{V}_z}\left(t_{k+1}\right)={\widehat{P}}^{V_x{V}_z}\left(t_k\right)\hfill \\ {\tilde{P}}^{V_y{V}_z}\left(t_{k+1}\right)={\widehat{P}}^{V_y{V}_z}\left(t_k\right)\hfill \end{array}$$

(12)

Note that ${\sigma }_{\mathbf{X}}^2$ is not the same as ${\sigma }_x^2$ and similarly for the other indices.

Then, the following values ${\tilde{P}}^{xm}\left(t_{k+1}\right),$ ${\tilde{P}}^{ym}\left(t_{k+1}\right),$ ${\tilde{P}}^{zm}\left(t_{k+1}\right),$ ${\tilde{P}}^{V_{x}m}\left(t_{k+1}\right),$ ${\tilde{P}}^{V_{y}m}\left(t_{k+1}\right),$ ${\tilde{P}}^{V_{z}m}\left(t_{k+1}\right),$ ${\tilde{P}}^{mm}\left(t_{k+1}\right)$ should be calculated using the following relations:

$$\begin{array}{c}\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\hfill \\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\hfill \end{array}\right)\hfill \\ =\left(\begin{array}{c}(Y\left(t_{k+1}\right)-\tilde{Y}\left(t_{k+1}\right))cos{\varphi }_i\left(t_{k+1}\right)-(X\left(t_{k+1}\right)-\tilde{X}\left(t_{k+1}\right))sin{\varphi }_i\left(t_{k+1}\right)\hfill \\ -{\epsilon }_{k+1}^{y}cos{\varphi }_i\left(t_{k+1}\right)+{\epsilon }_{k+1}^{x}sin{\varphi }_i\left(t_{k+1}\right)+{\epsilon }_{k+1}^{\varphi }(X_i+{\epsilon }_{k+1}^x-X\left(t_{k+1}\right))cos{\varphi }_i\left(t_{k+1}\right)\hfill \\ (Z\left(t_{k+1}\right)-\tilde{Z}\left(t_{k+1}\right))sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-(Y\left(t_{k+1}\right)-\tilde{Y}\left(t_{k+1}\right))sin{\lambda }_i\left(t_{k+1}\right)\hfill \\ -{\epsilon }_{k+1}^{z}sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)+{\epsilon }_{k+1}^{y}sin{\lambda }_i\left(t_{k+1}\right)+{\epsilon }_{k+1}^{\lambda }(Y_i+{\epsilon }_{k+1}^y-Y\left(t_{k+1}\right))cos{\lambda }_i\left(t_{k+1}\right)\hfill \end{array}\right)\hfill \end{array}$$

and the identities:

$$\begin{array}{cc}& X_i-X\left(t_{k+1}\right)=X_i-\tilde{X}\left(t_{k+1}\right)-(X\left(t_{k+1}\right)-\tilde{X}\left(t_{k+1}\right))\hfill \\ & Y_i-Y\left(t_{k+1}\right)=Y_i-\tilde{Y}\left(t_{k+1}\right)-(Y\left(t_{k+1}\right)-\tilde{Y}\left(t_{k+1}\right))\hfill \\ & Z_i-Z\left(t_{k+1}\right)=Z_i-\tilde{Z}\left(t_{k+1}\right)-(Z\left(t_{k+1}\right)-\tilde{Z}\left(t_{k+1}\right))\hfill \end{array}$$

where we consider that the position of the i-th object is known and use the non-correlatedness of ${\epsilon }_{k+1}^x,$ ${\epsilon }_{k+1}^y,$ ${\epsilon }_{k+1}^z,$ ${\epsilon }_{k+1}^{\varphi },$ ${\epsilon }_{k+1}^{\lambda }$ and differences $(X\left(t_{k+1}\right)-\tilde{X}\left(t_{k+1}\right)),$ $(Y\left(t_{k+1}\right)-\tilde{Y}\left(t_{k+1}\right))$ and $(Z\left(t_{k+1}\right)-\tilde{Z}\left(t_{k+1}\right)).$

Finally, we get:

$$\begin{array}{c}{\left[{\tilde{P}}^{xm}\left(t_{k+1}\right)\right]}^T=E\left[(X\left(t_{k+1}\right)-\tilde{X}\left(t_{k+1}\right))\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\end{array}\right)\right]\hfill \\ =\left(\begin{array}{c}{\tilde{P}}^{xy}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{xx}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)\\ {\tilde{P}}^{xz}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{xy}\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\end{array}\right),\hfill \end{array}$$

(13)

$$\begin{array}{c}{\left[{\tilde{P}}^{ym}\left(t_{k+1}\right)\right]}^T=E\left[(Y\left(t_{k+1}\right)-\tilde{Y}\left(t_{k+1}\right))\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\end{array}\right)\right]\hfill \\ =\left(\begin{array}{c}{\tilde{P}}^{yy}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{xy}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)\\ {\tilde{P}}^{yz}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{yy}\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\end{array}\right),\hfill \end{array}$$

(14)

$$\begin{array}{c}{\left[{\tilde{P}}^{zm}\left(t_{k+1}\right)\right]}^T=E\left[(Z\left(t_{k+1}\right)-\tilde{Z}\left(t_{k+1}\right))\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\end{array}\right)\right]\hfill \\ =\left(\begin{array}{c}{\tilde{P}}^{yz}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{xz}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)\\ {\tilde{P}}^{zz}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{yz}\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\end{array}\right),\hfill \end{array}$$

(15)

$$\begin{array}{c}{\left[{\tilde{P}}^{V_{x}m}\left(t_{k+1}\right)\right]}^T=E\left[(V_x\left(t_{k+1}\right)-{\tilde{V}}_x\left(t_{k+1}\right))\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\end{array}\right)\right]\hfill \\ =\left(\begin{array}{c}{\tilde{P}}^{y{V}_x}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{x{V}_x}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)\\ {\tilde{P}}^{z{V}_x}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{y{V}_x}\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\end{array}\right),\hfill \end{array}$$

(16)

$$\begin{array}{c}{\left[{\tilde{P}}^{V_{y}m}\left(t_{k+1}\right)\right]}^T=E\left[(V_y\left(t_{k+1}\right)-{\tilde{V}}_y(t_{k+1}))\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\end{array}\right)\right]\hfill \\ =\left(\begin{array}{c}{\tilde{P}}^{y{V}_y}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{x{V}_y}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)\\ {\tilde{P}}^{z{V}_y}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{y{V}_y}\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\end{array}\right),\hfill \end{array}$$

(17)

$$\begin{array}{c}{\left[{\tilde{P}}^{V_{z}m}\left(t_{k+1}\right)\right]}^T=E\left[(V_z\left(t_{k+1}\right)-{\tilde{V}}_z\left(t_{k+1}\right))\left(\begin{array}{c}{m}_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi }\\ m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda }\end{array}\right)\right]\hfill \\ =\left(\begin{array}{c}{\tilde{P}}^{y{V}_z}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{x{V}_z}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)\\ {\tilde{P}}^{z{V}_z}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{y{V}_z}\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\end{array}\right).\hfill \end{array}$$

(18)

In a similar way, we calculate:

$$\begin{array}{c}{({\tilde{P}}^{mm}\left(t_{k+1}\right))}^{-1}\hfill \\ ={\left[E\left(\begin{array}{cc}{(m_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi })}^2& (m_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi })(m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda })\\ (m_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi })(m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda })& {(m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda })}^2\end{array}\right)\right]}^{-1}\hfill \end{array}$$

(19)

Therefore:

$$\begin{array}{c}E\left[{(m_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi })}^2\right]\hfill \\ ={\tilde{P}}^{yy}\left(t_{k+1}\right){cos}^2{\varphi }_i\left(t_{k+1}\right)-{\tilde{P}}^{xy}\left(t_{k+1}\right)sin2{\varphi }_i\left(t_{k+1}\right)\hfill \\ +{\tilde{P}}^{xx}\left(t_{k+1}\right){sin}^2{\varphi }_i\left(t_{k+1}\right)+{\sigma }_y^2{cos}^2{\varphi }_i\left(t_{k+1}\right)+{\sigma }_x^2{sin}^2{\varphi }_i\left(t_{k+1}\right)+{\sigma }_{\varphi }^2({(X_i-\widehat{X}\left(t_k\right))}^2\hfill \\ +{\tilde{P}}^{xx}\left(t_{k+1}\right)+{\sigma }_x^2){cos}^2{\varphi }_i\left(t_{k+1}\right)\hfill \\ \\ E\left[(m_{k+1}^{\varphi }-{\tilde{m}}_{k+1}^{\varphi })(m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda })\right]\hfill \\ ={\tilde{P}}^{yz}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)\hfill \\ -{\tilde{P}}^{yy}\left(t_{k+1}\right)cos{\varphi }_i\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{xz}\left(t_{k+1}\right){sin}^2{\varphi }_i\left(t_{k+1}\right)cos{\lambda }_i\left(t_{k+1}\right)\hfill \\ +{\tilde{P}}^{xy}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)sin{\lambda }_i\left(t_{k+1}\right)\hfill \\ \\ E[{(m_{k+1}^{\lambda }-{\tilde{m}}_{k+1}^{\lambda })}^2]\hfill \\ ={\tilde{P}}^{zz}\left(t_{k+1}\right){sin}^2{\varphi }_i\left(t_{k+1}\right){cos}^2{\lambda }_i\left(t_{k+1}\right)-{\tilde{P}}^{yz}\left(t_{k+1}\right)sin{\varphi }_i\left(t_{k+1}\right)sin2{\lambda }_i\left(t_{k+1}\right)\hfill \\ +{\tilde{P}}^{yy}\left(t_{k+1}\right){sin}^2{\lambda }_i\left(t_{k+1}\right)+{\sigma }_z^2{sin}^2{\varphi }_i\left(t_{k+1}\right){cos}^2{\lambda }_i\left(t_{k+1}\right)+{\sigma }_y^2{sin}^2{\lambda }_i\left(t_{k+1}\right)+{\sigma }_{\lambda }^2({(Y_i-\widehat{Y}\left(t_k\right))}^2\hfill \\ +{\tilde{P}}^{yy}\left(t_{k+1}\right)+{\sigma }_y^2){cos}^2{\lambda }_i\left(t_{k+1}\right)\hfill \end{array}$$

4.2.2. Correction

After getting the measurements at the moment $t_{k+1}$, one can obtain the estimate of the UAV position at this moment. Therefore, the solution of Problem 1 has the form:

$$\begin{array}{c}\widehat{\mathbf{X}}\left(t_{k+1}\right)=\tilde{\mathbf{X}}\left(t_{k+1}\right)+\tilde{\mathbf{P}}\left(t_{k+1}\right){({\tilde{P}}^{mm}\left(t_{k+1}\right))}^{-1}(m_{k+1}-{\tilde{m}}_{k+1})\hfill \end{array}$$

(20)

and the matrix of the mean square errors is equal to:

$$\begin{array}{c}\widehat{\mathbf{P}}\left(t_{k+1}\right)=\tilde{P}\left(t_{k+1}\right)-\tilde{\mathbf{P}}\left(t_{k+1}\right){({\tilde{P}}^{mm}\left(t_{k+1}\right))}^{-1}\tilde{\mathbf{P}}{\left(t_{k+1}\right)}^T\hfill \end{array}$$

(21)

where:

$$\tilde{\mathbf{P}}\left(t_{k+1}\right)={({\tilde{P}}^{xm}\left(t_{k+1}\right),{\tilde{P}}^{ym}\left(t_{k+1}\right),...,{\tilde{P}}^{V_{z}m}\left(t_{k+1}\right))}^T$$

5. Robust Filtering on the Basis of the UAV Motion Model

The RANSAC method calculates the rotation matrix and the coordinates of the camera $\{A^{*},b^{*}\}$ in the Earth coordinate system with some minor error. However, the RANSAC method can give quite the wrong answer, called an outlier. It could happen, for example, if the frames $I_c$ and $I_m$ do not depict common objects. We provide further a method that makes a decision about whether $\{A^{*},b^{*}\}$ is an outlier or not. This problem has been considered in relation to the exclusion of outliers in the RANSAC-type procedures [25,26]. Here, we use the modification of the robust RANSAC [23] based on PKF for bearing-only observations [17].

After the prediction step of the Kalman filter, one can estimate the UAV (camera) position and the matrix of the mean square errors:

$$\tilde{\mathbf{X}}=\tilde{\mathbf{X}}\left({\mathbf{t}}_{\mathbf{k}+\mathbf{1}}\right),\tilde{\mathbf{P}}=\tilde{\mathbf{P}}\left({\mathbf{t}}_{\mathbf{k}+\mathbf{1}}\right).$$

Like in [26], one can suppose that the corresponding probability density is Gaussian. The reason is that the PKF gives the best linear estimates obtained until the current time $t.$ This estimate is the sum of uncorrelated random variables with almost the same variations, at least on the short intervals preceding the current time. It gives the opportunity to approximate the probability density distribution by the Gaussian one. Further extension of the robust RANSAC technique is based on the prior distribution of the UAV attitude. The approach has been developed in [23,27] on the basis of the extended Kalman filter (EKF). However, the estimate given by the EKF has an unknown bias and, of course, does not give the posterior Gaussian distribution. Therefore, the PKF, which gives an unbiased estimate, looks more preferable under the hypothesis of the posterior Gaussian distribution. Therefore, at the $(k+1)$-th step, the posterior distribution of $r_i^{{}^{\prime }}$ corresponding to an inlier is assumed to be Gaussian, that is according to Equation (1):

$$\mathcal{N}(A_k(r_i-\tilde{\mathbf{X}}\left(t_{k+1}\right)),A_k(\tilde{\mathbf{P}}\left(t_{k+1}\right)+P_{rr})A_k^T)$$

where $P_{rr}$ is the covariance matrix of the landmarks localization. Further, in the estimation algorithm, the pair $\{A_k,\tilde{\mathbf{X}}\left(t_{k+1}\right)\}$ is considered as an outlier at the confidence level $95\%$ if:

$$\parallel r_i^{{}^{\prime }}-A_k(r_i-\tilde{\mathbf{X}}\left(t_{k+1}\right))\parallel \ge 2*Sp[A_k(\tilde{\mathbf{P}}\left(t_{k+1}\right)+P_{rr})A_k^T]$$

Otherwise, the correction step is based on $\{A_k,\tilde{\mathbf{X}}\left(t_{k+1}\right)\}.$ Of course, all such nonlinear conjectures need confirmation on the basis of statistical modeling, which is one of the results of this article. One can observe the performance of robust filtering in the following figures. Figure 4 shows the correspondence between feature points obtained without a UAV motion model. Next, Figure 5 shows the correspondence established on the basis of the UAV motion model. The number of outliers reduces substantially.

Figure 4. Correspondence between (a) (image $I_c$ of the on-board camera of the UAV) and (b) (template image loaded in the UAV memory in advance) feature points found without the UAV motion model. One can observe chaotic correspondence, which gives a huge number of outliers.

Figure 4. Correspondence between (a) (image $I_c$ of the on-board camera of the UAV) and (b) (template image loaded in the UAV memory in advance) feature points found without the UAV motion model. One can observe chaotic correspondence, which gives a huge number of outliers.

Figure 5. Correspondence between (a) (image $I_c$ of the on-board camera of the UAV) and (b) (template image loaded in the UAV memory in advance) feature points found with the aid of RANSAC based on the UAV motion model. The number of outliers reduces substantially.

Figure 5. Correspondence between (a) (image $I_c$ of the on-board camera of the UAV) and (b) (template image loaded in the UAV memory in advance) feature points found with the aid of RANSAC based on the UAV motion model. The number of outliers reduces substantially.

6. Control of the UAV

Control of a UAV that ensures its motion along the reference trajectory may be determined on the basis of the standard deterministic linear-quadratic approach [28]. However, the problem of control on the basis of bearing-only observation is different from the standard one. It should be underlined that this problem is a non-linear one and cannot be solved by the standard way. The problem of the optimal control for the system Equation (2) is the stochastic one with incomplete information and does not have an explicit solution. However, for practical reasons, one can simplify it by considering the locally optimal control. Here, we discuss the following problem:

Problem 2. 

Find the locally optimal controls $a_x\left(t_k\right),$ $a_y\left(t_k\right)$ and $a_z\left(t_k\right)$ aimed to keep the motion of the UAV along the reference trajectory.

Assume that we have some reference trajectory ${\mathbf{X}}_{nom}\left(t_k\right).$

Therefore, at the moment $t_{k+1}$, we have to minimize the following expressions:

$$\begin{array}{cc}& E_1=E{\{{(X\left(t_{k+1}\right)-X_{nom}\left(t_{k+1}\right))}^2+(V_x\left(t_{k+1}\right)-V_{x_{nom}}\left(t_{k+1}\right))\Delta t)}^2\}\to \underset{a_x\left(t_k\right)}{min}\hfill \\ & E_2=E{\{{(Y\left(t_{k+1}\right)-Y_{nom}\left(t_{k+1}\right))}^2+(V_z\left(t_{k+1}\right)-V_{z_{nom}}\left(t_{k+1}\right))\Delta t)}^2\}\to \underset{a_y\left(t_k\right)}{min}\hfill \\ & E_3=E{\{{(Z\left(t_{k+1}\right)-Z_{nom}\left(t_{k+1}\right))}^2+(V_z\left(t_{k+1}\right)-V_{z_{nom}}\left(t_{k+1}\right))\Delta t)}^2\}\to \underset{a_z\left(t_k\right)}{min}\hfill \end{array}$$

Let us consider the components of the $E_1$ expression:

$$\begin{array}{cc}& X\left(t_{k+1}\right)-X_{nom}\left(t_{k+1}\right)=X\left(t_k\right)-\widehat{X}\left(t_k\right)-(X_{nom}\left(t_k\right)-\widehat{X}\left(t_k\right))\hfill \\ & +(V_x\left(t_k\right)-{\widehat{V}}_x\left(t_k\right))\Delta t-(V_{x_{nom}}\left(t_k\right)-{\widehat{V}}_x\left(t_k\right))\Delta t+(a_x\left(t_k\right)-a_{x_{nom}}\left(t_k\right))\frac{\Delta t^2}{2}\hfill \\ \\ & V_x\left(t_{k+1}\right)-V_{x_{nom}}\left(t_{k+1}\right)=V_x\left(t_k\right)-{\widehat{V}}_x\left(t_k\right)-(V_{x_{nom}}\left(t_k\right)-{\widehat{V}}_x\left(t_k\right))+(a_x\left(t_k\right)-a_{x_{nom}}\left(t_k\right))\Delta t\hfill \\ & +W_x\left(t_k\right)\hfill \end{array}$$

Then, we square these components and take the derivative of the sum with respect to $a_x\left(t_k\right)$ given that some components are uncorrelated:

$$\begin{array}{cc}& E\{(X\left(t_k\right)-\widehat{X}\left(t_k\right))(X_{nom}\left(t_k\right)-\widehat{X}\left(t_k\right))\}=0\hfill \\ & E\{(V_x\left(t_k\right)-{\widehat{V}}_x\left(t_k\right))(V_{x_{nom}}\left(t_k\right)-{\widehat{V}}_x\left(t_k\right))\}=0\hfill \end{array}$$

Finally, we get:

$$\begin{array}{c}{a}_x\left(t_k\right)=a_{x_{nom}}\left(t_k\right)+\frac{2(X_{nom}\left(t_k\right)-\widehat{X}\left(t_k\right))}{5\Delta t^2}+\frac{6(V_{x_{nom}}\left(t_k\right)-{\widehat{V}}_x\left(t_k\right))}{5\Delta t}\hfill \end{array}$$

(22)

We take into account that the acceleration of the UAV has limitations $[a_{x_{min}},a_{x_{max}}]$, so the control acceleration has the form:

$$a_x^c\left(t_k\right)=\left\{\begin{array}{cc}\hfill a_{x_{min}}& if{a}_x\left(t_k\right)<a_{x_{min}},\hfill \\ \hfill a_x\left(t_k\right)& if{a}_{x_{min}}\le a_x\left(t_k\right)\le a_{x_{max}},\hfill \\ \hfill a_{x_{max}}& if{a}_x\left(t_k\right)>a_{x_{max}}.\hfill \end{array}\right.$$

Similarly, we obtain the expressions for $a_y^c\left(t_k\right)$ and $a_z^c\left(t_k\right).$ Thus, we get the following solution of Problem 2 [19]:

$$\begin{array}{c}\widehat{\mathbf{a}}\left(t_k\right)={\widehat{\mathbf{a}}}_{nom}\left(t_k\right)+\frac{2}{5\Delta t^2}({\widehat{\mathbf{X}}}_{nom}\left(t_k\right)-\widehat{\mathbf{X}}\left(t_k\right))+\frac{6}{5\Delta t}({\widehat{\mathbf{V}}}_{nom}\left(t_k\right)-\widehat{\mathbf{V}}\left(t_k\right))\hfill \end{array}$$

(23)

where:

$$\begin{array}{c}\widehat{\mathbf{a}}\left(t_k\right)={(a_x\left(t_k\right),a_y\left(t_k\right),a_z\left(t_k\right))}^T\hfill \\ \widehat{\mathbf{X}}\left(t_k\right)={(X\left(t_k\right),Y\left(t_k\right),Z\left(t_k\right))}^T\hfill \\ \widehat{\mathbf{V}}\left(t_k\right)={(V_x\left(t_k\right),V_y\left(t_k\right),V_z\left(t_k\right))}^T\hfill \end{array}$$

and similarly for the nominal trajectory components.

7. Experimental Results

In this section, we give the results of the algorithm’s modeling. The UAV is virtually flying over the landscape shown in Figure 1. This image has been obtained from Google Maps, and for verification of the algorithm, the image of the same region obtained from another Bing satellite was used. Therefore, these two images modeled the preliminary downloaded template and the image obtained by the on-board camera. The result of virtual flight experiment is shown in Figure 6.

Figure 6. Blue dots corresponds to the reference trajectory and black dots to the real path. Blue squares show the localization of the terrain areas corresponding to the template images, and red squares show the moments where the estimates of the UAV positions have been obtained and assumed to be reliable according to the robust RANSAC algorithm described in Section 5.

Figure 6. Blue dots corresponds to the reference trajectory and black dots to the real path. Blue squares show the localization of the terrain areas corresponding to the template images, and red squares show the moments where the estimates of the UAV positions have been obtained and assumed to be reliable according to the robust RANSAC algorithm described in Section 5.

8. Results and Discussion

In the modeling of the control algorithm, we use the UAV moving approximately with a velocity of 50 m/s, though the change of the altitude is assumed to be rather substantial. The control algorithm takes into account the constraints imposed on linear acceleration and angular velocities. The software developed for modeling may be used in a real on-board navigation system, as well. Moreover, the filtering algorithm based on unbiased estimation may be easily incorporated with the INS, since it gives also the unbiased square error estimates, which opens the way to correct data fusion. The quality of tracking for $x,y,z$ components is shown below in Figure 7, Figure 8 and Figure 9, respectively. In all of these figures, blue dots correspond to the reference trajectory, black dots to the real path and red squares to the moments, where the estimates of the UAV positions have been obtained and assumed to be reliable according to the robust RANSAC algorithm. One can observe that in the “measurement” areas, the algorithm estimates the coordinates with high accuracy, and the control provides the tracking with high accuracy, as well.

Figure 7. Tracking of the x-coordinate.

Figure 7. Tracking of the x-coordinate.

Figure 8. Tracking of the y-coordinate.

Figure 8. Tracking of the y-coordinate.

Figure 9. Tracking of the z-coordinate.

Figure 9. Tracking of the z-coordinate.

Figure 10. Averaged standard deviation of the position estimation error. The limit value in the observation areas is close to 24.5 m. One can see that in the areas of no observations, the SD monotonically increases.

Figure 10. Averaged standard deviation of the position estimation error. The limit value in the observation areas is close to 24.5 m. One can see that in the areas of no observations, the SD monotonically increases.

The high accuracy is in accordance with the standard deviation (SD) theoretically calculated from the PKF. The value of the averaged standard deviation, which is the square root of $P_{xx}\left(t\right)+P_{yy}\left(t\right)+P_{zz}\left(t\right)$, is shown in Figure 10 below.

9. Conclusions

The main result of the work is the new algorithm of the UAV control based on the observation of the landmarks in a 3D environment. The new RANSAC based on the UAV motion model permits one to exclude the huge number of outliers and, by that, to provide the reliable set of data for the estimation of the UAV position on the basis of the novel non-biased PKF algorithm. This work is just the beginning of the implementation of this approach in the navigation of UAVs during long-term autonomous missions.