Filter Kalman for solving the problem of coordinates uav

Arefyev N.N.

UDK 623.7

N. N. AREFYEV

FILTER KALMAN FOR SOLVING THE PROBLEM OF COORDINATES UAV

Belarusian National Technical University

Unmanned aerial vehicles (UAVs) are increasingly used in military and scientific research. Some miniaturized UAVs rely entirely on the global positioning system (GPS) for navigation. GPS is vulnerable to accidental or deliberate interference that can cause it to fail. It is not unusual, even in a benign environment, for a GPS outage to occur for periods of seconds to minutes. For UAVs relying solely on GPS for navigation such an event can be catastrophic. This article proposes an extended Kalman filter approach to estimate the location of a UAV when its GPS connection is lost, using inter-UAV distance measurements Increasing the accuracy of coordinate's determination is one of the most crucial tasks of the modern UAV navigation. This task can be solved by using different variants of integration of navigation systems. One of the modern variants of integration is the combination of GPS/GLONASS-navigation with the extended Kalman filter, which estimates the accuracy recursively with the help of incomplete and noisy measurements. Currently different variations of extended Kalman filter exist and are under development, which include various number of variable states [1]. This article will show the utilization efficiency of extended Kalman filter in modern developments.

Keywords: Kalman filter, GPS, coordinates UAV, mathematical modeling, aerial vehicle, visual odometry, projective geometry, control, navigation, integration.

Theory analysis

The integration of observation channels in control systems of objects subjected to perturbations and measurement errors of the motion is based on on the observations control theory started in the early 1960s. The first works on this topic were based on the simple Kalman filter property, namely: the possibility of determining the root-mean-square estimation error in advance, without observations, by solving the Riccati equation for the error covari-ance matrix [2]. The development of this methodology allowed solving problems with a combination of discrete and continuous observations for stochastic systems of discrete-continuous type. At the same time, methods were developed for solving problems with constraints imposed on the composition of observations, temporal and energy constraints both on separate channels and on aggregate. For a wide class of problems with convex structure, necessary and sufficient conditions for optimality were obtained, both in the form of dynamic programming equations and the generalized maximum principle, which opens the possibility of a numerical solution. The tasks of integrating surveillance and control systems for UAVs open a new wide field of application of the observation control

methods, especially when performing autonomous flight tasks. One of the most important problems is the detection of the erroneous operation of individual observation subsystems, in which the solution of navigational tasks should be redistributed or transferred to backup subsystems or other systems operating on other physical principles [3].

A typical example: navigation through satellite channels such as global positioning system (GPS), which is quite reliable in simple flight conditions, but in a complex terrain (mountains, gorges), it is necessary to use methods to determine your position with the help of other systems based, for example, on landmarks observed either with optoelectronic cameras, or radar.

Here the serious problem of converting the signals of these systems into data suitable for navigation arises. The human-operator copes with this task on the basis of training. That is the serious problem in computer vision area and it is one of the mainstream in the UAV autonomous flight. Meanwhile, the prospects for creating artificial intelligence systems of this level for UAV applications are still far from reality.

At the same time, the implementation of simple flight tasks, such as either access to the aerial

survey area or tracking the reference trajectory the organization of data transfer in conditions of limited time and energy storage and even landing, are quite accessible for performing UAVs in the autonomous mode with reliable navigation aids.

Unmanned aerial, land-based and underwater-based vehicles that perform the autonomous missions use, as a rule, an on-board navigation system supplemented by sensors of various physical nature. At the same time, unlike remote control systems in which these sensors present information in the form as operator-friendly as possible, the measurement results should be converted into the input signals of the control system, which requires other approaches. This is especially evident in the example of an optical or optoelectronic surveillance system, whose purpose in remote control mode is to provide the operator with the best possible image of the surrounding terrain. At the same time, in an autonomous flight, the observing system should be able to search for the characteristic objects in the observed landscape and give the control system their coordinates and estimate the distances between them. Of course, the issue of providing an exellent image and determining the metric properties of the observed images are connected, and in no case cancel one another. However, what a human operator does automatically basing on a sufficiently high-quality image of the terrain, the readings of other sensors and undoubtedly on previous experience, the control system algorithm must do by using data from video and other systems, with the same accuracy as the human operator [5].

Mathematical Model Construction

In the given example we will talk about the UAV horizontal motion, in other words, we will look through the problem of 2d localization [2]. In our case, this is justified by the facts that for many situations that are practically encountered, the UAV can remain at about the same height. This supposition is widely used to simplify the modeling of aircraft dynamics [2]. Dynamic UAV model is given by the following system of equations:

x(t) = v(t )cos h(t), y (t) = v(t) sin h(t), h (t) = w(t), w (t) = ew (t),

v(t) = ev (t),

(1)

where (x(t), y(t)} are the UAV coordinates in horizontal plane as the function of time, h(t) is the UAV direction, w(t) is the UAV angular velocity, and v(t) is the UAV actual velocity, ew(.) and ev(.) functions are constant.

They are mutually independent with the certain covariances E[ew(t) sw(<S)] and E[ev(t) sv(<S)] equal to Qw5(t - 5) and Qv5(t - 5) respectively and are used for modeling of the UAV acceleration caused by the wind, pilots' maneuvers, etc. Qu and Qv values are the derivatives of the UAV maximum angular velocity and measured values of the UAV linear velocity variation 5 is the Kronecker sign.

The given system of equations is approximate because of nonlinearity of the model and the noise. The simplest way of approximation here is the approximation by Euler method. The discrete model of the dynamic UAV motion system is shown below.

xk+1 = xk + vkDtk cos hk,

yk+1 = yk + vk Dtk sin hk,

hk+1 =hk +wk Dtk,

wk+1 = wk +ew,k,

(2)

vk+1 =vk

where 0k = [xk, yk, hb wk, vk] is the discrete state vector of the Kalman filter, which allows one to approximate the value of the continuous state vector. Dtk is the time interval between k and k + 1 measurements. {ew, k} and {ev, k} is the sequences of values of white Gaussian noise with zero mean value.

The covariance matrix for the first sequence is as follows [11]:

E{ew, ifiw, j} = QwDtk5kj

Analogically, for the second sequence is

E{ev, kPv, j} = QvDtk5kj

Performing the corresponding substitutions in the system equations (2), we obtain:

wk+1 = wk wk^k

vk+1

= Vu +

k +Yv,W Dtk

(3)

The {Tw, k} and {^V, k} sequences are mutually independent. They are also the sequences of values of white Gaussian noise with zero mean value and the Qw and Qv covariance matrixes respectively. The advantage of this configuration

is that is shows the discrete noise changing in suit, we have the following discrete dynamic the interval between each measurement. As a re- model [8]:

(

qk+1 -

1 0 0 0 1 0 0 0 1 0 0 0 0 0 0

0 0

Dtk 1 0

Dtkcos hkЛ Dtksin hk 0 0 1

f [qk ]

The equation for dk is as follows:

xk - xo )2 +(yk - yo )2

dk —

h\Qt

'-■d ,Ъ

(5)

xt+1 — xt

- atdt + 5,

(6)

Figure 1. Coordinate transformation. XYZ is the geocentric coordinate system and X'Y'Z' is the local coordinate system

e*

л

0 0 0

DL

0 0 0 0

(g л

I w,k g v,k

(4)

where x and y are the UAV coordinates at the k-time and ed,k is the Gaussian sequence of random parameters with zero mean value which is used to specify the error.

The given sequence is supposed to be independent from {Sw, k} and {ev, J.

The equations (3) and (4) serve as a basis for assessing the location of the UAV, where the coordinates are obtained with the help of the Extended Kalman filter. Simulating the failure of navigation systems regarding this type of filter shows its significant effectiveness [9].

To be clearer we will give a simple example. Let an UAV fly at uniformly accelerated speed, with some constant a-acceleration.

Uk

where x is the UAV coordinate at the t - time and 5 is a random value.

During the flight, the following GPS data were collected: time, geocentric latitude, geocentric longitude, altitude time of arrival, pulse width, signal frequency and amplitude. The longitude, latitude and height (LLH) coordinates recorded by the UAV are not well suited for navigation and tracking problems because linear motion becomes non-linear in these coordinates. In comparison, a local coordinate system whose X and Y axes are in the local horizon and Z axis points to the local Zenith is much better suited and is the industry standard. Therefore, the geocentric latitude and longitude location information is first converted into a local coordinate system, which is shown in Figure 1. X'Y'Z' is the local coordinate system. The origin of the local coordinate system is randomly chosen to be the starting location of UAV. In the figure, j and g are the geocentric latitude and longitude respectively. j is the geodetic latitude [12]. Refer to [13] for a detailed description on how to convert the LLH coordinates into local coordinates.

Let us suppose that we have a GPS-sensor, which receives the data about the UAV location. Let us perform the result of the modeling of this process through MATLAB.

Simulation

At the present time, the UAV are not equipped to determine inter-UAV distances; hence, as indicated in the introduction, loss of a GPS connection is likely currently to be fatal. For this reason, the real world data we use is entirely data obtained when UAV do actually have a GPS connection. From this data, we are able to simulate loss of a GPS connection and acquisition of inter-UAV distance measurements in the following way. A cer-

200 400 600 800 1000 1200 1400 1600 1800

Number of GPS Measurements

Figure 2. Time interval between adjacent GPS measurements for UAV

Table 1. Steps of the simulations

Simulation Е(х - х) <r(i — х) Ei» ~ У) °{у -«/) E(d) a(d)

1 10.5836 46.3087 -10.9167 38.1720 38.8275 48.2183

2 8.7661 45.6333 -11.7453 37.2149 37.2011 47.9382

3 10.9220 47.6161 • 11.8613 38.5942 39.2297 49.7761

4 10.1834 62.1998 -12.3481 40.1211 39.0115 64.9048

5 8.7462 50.1151 -12.1389 39.2946 39.4348 52.1936

6 9.2793 45.4631 -10.8837 37.9597 38.9082 46.8867

7 9.8450 55.4412 -12.0954 39.5770 40.1785 57.1734

8 7.2842 46.6786 -12.2274 36.2379 38.4231 47.0972

9 9.2419 45.9712 -12.3221 37.8227 39.2994 47.2921

10 8.6380 52.5742 -12.3697 39.9224 40.1153 54.5532

Average 9.3490 49.8001 -11.8909 38.4917 39.0629 51.6034

tain time series of intervals of synthetic GPS outage is postulated. During these intervals, inter-UAV distances are synthesized at discrete instants of time. This is done by taking the actual GPS measurements, determining the corresponding in-ter-UAV distance, and then adding on to the resulting value a Gaussian random variable with zero mean and standard deviation of 10m. This is delivered to the algorithm as a (synthesized) in-ter-UAV distance, and the Kalman filter is run with this data. Validation occurs by comparing the estimated UAV tracks delivered by the Kalman filter with those in the original real- world data, where GPS measurements are actually available, so that actual UAV tracks are known [14].

In the simulations, we assume that UAV has a GPS connection most of the time but it may temporarily lose the GPS connection for up to 54s (see Figure 2).

In the simulation, the first two state variables of the initial state vector are chosen to be the ini-

tial GPS location of UAV and other state variables are chosen randomly. The initial value of P is chosen based on an empirical estimate as P = diag{1000 1000 0.3 0.01 1}. It is found that generally the choice of P has little impact on the filter performance; however a very large deviation of P from its true value does cause the divergence of the filter. The value of Q is chosen based on an empirical estimate as Q = diag{0.0003 10}. The choice of Q is critical for the filter performance and Q should be chosen carefully based on an in-depth understanding of the UAV dynamics. The value of R is chosen to be 100. In real applications, the value of R can be obtained via a priori calibration of the distance measurement equipment. The distance measurement can be obtained by a simple round trip timing mechanism [10].

The UAV location obtained from GPS is used as the «true location» of the UAV. The path of UAV starts from the rectangular on the right side of the figure. Apparently, the estimated location

Figure 3. Variation of error in x with Time

Figure 4. Variation of error in y with Time

has larger error on this part of the figure. As time evolves, the estimated location gradually converges to the true location, which is evidenced by much less deviation from the true location on the left side of the figure. Figure 3 and 4 shows the variation of error. As shown in the Table, both the estimate of x and the estimate ofy have a bias. As a reference, the value of x varies within the range of [-5000, 1000] and the value of y varies within the range of [-6000, 3000]. Therefore, the value of the bias is comparatively small. However, both

biases have a fairly consistent trend in all ten simulations [13].

iНе можете найти то, что вам нужно? Попробуйте сервис подбора литературы.

Figure 5 shows the effectiveness of filtering by Kalman algorithm. However, in a real situation, signals often have non-linear dynamics and abnormal noise. In such cases, the extended Kal-man filter is used. In case, if the noise variances are not too large (i.e. the linear approximation is adequate) the application of the extended Kalman filter yields the solution of the problem with high accuracy. But in case if the noise is not Gaussian

х

mm

Figure 5. Filtering the sensor reading with the help of Kalman filter

the extended Kalman filter mustn't be applied. In this case, a partial filter is usually used, which uses numerical methods for taking integrals based on Monte Carlo methods with Markov chains.

Partial Filter

Let us perform one of the algorithms, which develop the ideas of the extended Kalman filter -a partial filter. Partial filtering is a non-optimal filtering method that works when performing a Monte Carlo join on a set of particles that represent the probability distribution of the process. Here a particle is an element taken from the a priori distribution of the estimated parameter. The main idea of the partial filter is that the large amount of particles can be used to represent the distribution estimate. The larger the number of particles is used, the more precise the set of particles will represent the a priori distribution. The particle filter is initialized by placing N of particles in it from the a priori distribution of the parameters that we want to estimate. The filtering algorithm involves running these particles through a special system, and then weighing it using information obtained from measuring these particles. The resulting particles and associated masses represent the posterior distribution of the evaluation process. The cycle is repeated for each new measurement and the weights of the particles are updated to represent the subsequent distribution. One of the main problems of the traditional method of particle filtering is that as a result such an

approach usually has several particles with a huge weight unlike most others with a very light weight. It leads to the filtering instability [6]. This problem can be solved by introducing a sampling frequency, where N of new particles are taken from a distribution composed of old particles. The result of the evaluation is obtained by getting a sample of the mean value of the particle set. If we have several independent samples, the mean sample will be an accurate estimate of the mean value that determines the final variance.

Even if the particle filter is not optimal, then as the number of particles tends to infinity, the efficiency of the algorithm approaches the Bayesian estimation rule. That is why it is recommended to have the possible number of particles to get the best result. Unfortunately, this leads to a strong increase in the computational complexity and therefore forces us to seek a compromise between accuracy and calculation speed. So the number of particles should be chosen based on the requirements for the accuracy estimation problem. Another important factor for the operation of the particle filter is the restriction on the sampling frequency. As mentioned earlier, the sampling frequency is an important parameter of the particle filtering and without it, the algorithm eventually becomes degenerate. The idea is that if the weights are distributed too unevenly and the sampling threshold is soon reached, then the low-weight particles are discarded, and the remaining set forms a new probabilistic density for which

new samples can be taken. Choosing a sampling frequency threshold is a rather difficult task, because too high frequency causes excessive filter sensitivity to noise, and too low one gives a large error. Another important factor is the probability density [11].

In general, the particle filter algorithm shows a good productivity of location calculation for stationary targets and in the case of relatively slow moving targets with unknown acceleration dynamics. Generally, the particle-filtering algorithm is more stable than the extended Kalman filter, and is less prone to the degeneration and serious failures. In cases of nonlinear, non-Gaussian distribution, this filtering algorithm shows a very good accuracy in determining the target location while the extended Kalman filtering algorithm cannot be used under such conditions. One of the disadvantages of this approach is its higher complexity compared with the extended Kalman filter, as well as the fact that it is not always obvious how to select the parameters for this algorithm correctly [12].

Optimal estimation of a random process

In contrast to common approaches based on consideration as a criterion for optimizing the minimum of the mean square error of estimation, in this case the maximum of a posteriori probability density of the evaluated process is considered as an optimization criterion. The a priori probability density of the evaluated process is initially considered to be a Gaussian differentiable function, which allows it to be expanded in a Taylor series without using in the intermediate transformations the characteristic functions and decomposition into harmonics. For small time intervals, the probability density of the measurement error vector is defined by definition also Gaussian with zero mathematical expectation. This makes it possible to obtain a mathematical expression for the discrepancy function characterizing the deviation of the values of the real measurement of the process from its mathematical model. To determine the optimal a posteriori estimate of the state vector, it is assumed that this estimate corresponds to its mathematical expectation, the maximum of the posterior probability density. This makes it possible to derive the Stratonovich-Kouchner equation on the basis of the Bayesian formula for the a priori and a posteriori probability density. The use of

the Stratonovich-Kushner equation for various types and values of the drift vector and the diffusion matrix of the Markov stochastic process makes it possible to solve various problems of filtering, identification, smoothing, and forecasting the state of the system for both continuous and discrete systems. The discrete realization of the developed continuous algorithms for a posteriori estimation allows obtaining specific discrete algorithms for the implementation in the on-board computer of a mobile robotic system [7].

Prospective researches in this field

The use of the Kalman filter model, similar to what we have shown, can be seen in [3], where it is used to improve the characteristics of the complex system (GPS + computer vision model for comparison with the geographic base), and the satellite navigation equipment failure situation is simulated. With the help of the Kalman filter the results of the system operation in case of failure were significantly improved (for example, the error in determining the altitude was reduced by about half, and the errors in determining the coordinates along different axes were reduced by almost 9 times). The analogical use of the Kalman filter is also shown in [4].

The problem, which is interesting from the point of view of the method set, is solved in [1]. There is also used a Kalman filter with 5 states, with some differences in the construction of the model. The result obtained exceeds the result of the given model given [5] due to the use of additional means of integration, (photo and thermal images are used). The application of the Kalman filter in this case makes it possible to reduce the error in determining the spatial coordinates of a given point to a value of 5.5 m.

Conclusion

As a conclusion, we can note that the use of the Kalman filter in the UAV location systems is practiced in many modern developments. There is a huge number of variances and aspects of such use up to simultaneous application of several similar filters with different factors of states. One of the most promising trends in Kalman filter development is working on creating a modified filter, where the errors of will be represented by color noise, which will make it even more valuable for solving real problems. The great interest in this

area is a partial filter by means of which it is possible to filter non-Gaussian noise. The given variety and the tangible results in increasing accuracy, especially in case of the failure of standard satel-

lite navigation systems, are the main factors of the impact of this technology on various scientific fields related to the development of accurate and fault-tolerant navigation systems for various aircrafts.

REFERENCES

1. Makarenko G. K., Aleshechkin A. M. Investigation of the filtering algorithm in determining the coordinates of an object by the signals of satellite radio navigation systems / / Doklady TUSURa [Reports of TUSUR]. - 2012. - No. 2 (26). -pp. 15-18 (in Russian).

2. Miller, B. M. Optimal control of observations in the filtering of diffusion processes // I, II. Autom. Remote Control 1985, p. 46, p. 207-214, p. 745-754

3. Bar-Shalom Y., Li X. R., Kirubarajan T. Estimation with Applications to Tracking and Navigation // Theory Algorithms and Software. - 2001. - Vol. 3. - pp. 10-20.

4. Miller, B. M.; Runggaldier, W. J. Optimization of observations: A stochastic control approach. SIAM J. Control Op-tim. 1997, p. 35, p. 1030-1052.

5. Andreev, K. A.; Rubinovich, E. P. UAV Guidance When Tracking a Ground Moving Target with Bearing-Only Measurements and Digital Elevation Map Support // Southern Federal University, Engineering Sciences: Rostov Oblast, Russia, 2015; pp. 185-195. (In Russian).

6. Bassem I S. Vision based Navigation (VBN) of Unmanned Aerial Vehicles (UAV) // UNIVERSITY OF CALGARY. -2012. - Vol. 1. - pp. 100-127.

7. Lobaty A. A., Yacina Y. F., Arefyeu N. N. Optimal estimation of random processes on the criterion of maximum a posteriori probability// Belarussian National Technical University. - 2016.

8. Conte G., Doherty P. An Integrated UAV Navigation System Based on Aerial Image Matching // Aerospace Conference. - 2008. -Vol. 1. - pp. 3142-3151.

9. Guoqiang M., Drake S., Anderson B. Design of an extended kalman filter for uav localization // In Information, Decision and Control. - 2007. - Vol. 7. - pp. 224-229.

10. Ponda S. S. Trajectory Optimization for Target Localization Using Small Unmanned Aerial Vehicles // Massachusetts institute of technology. - 2008. - Vol. 1. - pp. 64-70.

11. Wang J., Garrat M., Lambert A. Integration of gps/ins/vision sensors to navigate unmanned aerial vehicles // IAPRS&SIS. - 2008. - Vol. 37. - pp. 963-969.

12. Tsukanov A. V. Development of a software package for constructing the optimal trajectory of UAV motion. - 40 p.

13. Chepurny I. V. Dynamics of the flight of aircraft: training. allowance / I. V. Chepurny - Komsomolsk-on-Amur: FG-BOU VPO «KnAGTU», 2014. - 112 p.

14. Nathan A. WPF 4. Detailed guidance. - Trans. from English. - SPB.: Symbol-Plus, 2011. - 880 p.: Ill.

ЛИТЕРАТУРА

1. Макаренко Г. K., Алешечкин A. M. Исследование алгоритма фильтрации при определении координат объекта по сигналам спутниковых радионавигационных систем / / Доклады ТУСУРа [Доклады из ТУСУР]. - 2012. - No. 2 (26). - 15-18 с.