CC BY
17
MSC 93A30, 93E10
DOI: 10.14529/mmp190115
ADAPTIVE ESTIMATION OF A MOVING OBJECT TRAJECTORY USING SEQUENTIAL HYPOTHESIS TESTING
A.V. Tsyganov1, Yu.V. Tsyganova2, A.V. Golubkov1, I.O. Petrishchev1
1Ulyanovsk State Pedagogical University named after I.N. Ulyanov, Ulyanovsk, Russian Federation
2Ulyanovsk State University, Ulyanovsk, Russian Federation
E-mails: [email protected], [email protected], [email protected], [email protected]
The present paper addresses the problem of adaptive estimation of a moving object trajectory and detection of changes in the motion mode. It is supposed that an object moves along a complex trajectory and at known discrete-time instants it may change its motion to one of three possible modes: a uniform straight line motion or a uniform anticlockwise/clockwise circular motion. We propose a new algorithm for adaptive trajectory estimation that combines a hybrid linear stochastic model of an object trajectory with a bank of competitive Kalman filters and a decision rule based on a sequential hypothesis testing. A detailed description of the decision rule and pseudocode of the proposed algorithm are given. The software implementation of the algorithm is made in Matlab. A numerical example of adaptive estimation of the motion of an object along a complex trajectory consisting of nine different pieces is considered. We have conducted computational experiments with different levels of noise in the measurements. The results confirm the effectiveness of the proposed algorithm.
Keywords: adaptive estimation; moving object; sequential hypothesis testing.
Introduction
In this paper, we consider the problem of simultaneous adaptive estimation of a moving object trajectory and detection of changes in the motion mode which is typical for example, for mobile robots [1] or marine vessels [2]. This problem belongs to the class of tracking problems, which are currently of great interest due to their important practical applications.
The estimation of the position and velocity of a maneuvering object has been researched in literature for many decades, see [3]. The well-known approach for solving this problem is the multiple-model (MM) tracking technique. Following [3], the multiple model or hybrid system approach assumes that the system is described by one out of a finite number of models. The known basic approaches to the multiple-model tracking are the static MM estimator, the Dynamic MM Estimator, the GPB1 MM Estimator, the GPB2 MM Estimator, the interacting MM estimator. These algorithms are decision-free, i. e. they do not detect the motion mode, and require to calculate all estimates for each possible model along the whole trajectory.
We suggest replacing a complex and usually nonlinear model of an object movement with a set of linear models for which optimal discrete Kalman filtering may be applied. But to obtain optimal estimates of the object state which allow to track and predict the object movement there is a need to detect changes quickly in the motion mode. The purpose of this paper is to provide the algorithm which efficiently solves this problem. To describe a
moving object trajectory we use a hybrid stochastic linear model proposed in [4] in which separate pieces of the complex trajectory are described by models of one of three possible motion modes: a uniform straight line motion, a uniform anticlockwise circular motion and a uniform clockwise circular motion with a given radius.
Let us suppose that the moments of changes in the motion mode are known. The main idea of the algorithm is to apply at each of these moments the sequential probability ratio test (SPRT) [5] to choose from one of three possible motion modes. The proposed algorithm of adaptive estimation was implemented in MATLAB and proved its efficiency over the set of numerical experiments.
1. Moving Object Trajectory Model
Suppose that the trajectory of an object can be divided into separate pieces on which the movement of the object can be represented using one of the discrete linear stochastic models, each of which describes either a uniform straight line motion or a uniform anticlockwise/clockwise circular motion (left turn/right turn) with a given radius. Then the motion of an object on the entire trajectory is described by the hybrid stochastic model
Xk = Xk-1 + Bi + Gwk-1, i e Z, (1)
where k is a discrete-time moment, i is a motion mode number; x = [x1, x2, x3, x4]T e R4 is a vector of the object motion parameters, in which x1 is a coordinate of the object along axis Ox (m), x2 is velocity vx along axis Ox (m/s), x3 is a coordinate of the object along axis Oy (m), x4 is velocity vy along the axis Oy (m/s). The detailed description of the model (1) matrices can be found in [4]. The proposed hybrid model allows modelling a complex trajectory of the object movement using the algorithm described in [4].
Assuming that only coordinates x1 and x3 are measured (velocities x2 and x4 are not measured), the measurement model is written as zk = Hxk + vk, where H is the corresponding measurements matrix, vk is the coordinates measurement errors, which is a Gaussian white noise with zero mean and a diagonal noise covariance matrix R = diag[pi,p2].
2. Algorithm for Adaptive Estimation
Consider three hypotheses about the modes of the object motion: 1) H0 is an object performs a uniform straight line motion; 2) H1 is an object performs a uniform anticlockwise circular motion with a given radius (left turn); 3) H2 is an object performs a uniform clockwise circular motion with a given radius (right turn) and three Kalman competitive filters F0, F1 and F2 designed, correspondingly, under the assumption of hypothesis H0, H1 and H2. To evaluate likelihood ratios Ak)ij- (k is a moment of time, i, j are hypotheses numbers) we use sequential probability ratio test (SPRT) [5], and to select the optimal filter we use the decision rule described in [6].
The outline of the proposed algorithm in the form of pseudo-code is presented by Algorithm 1. The input data for the algorithm are: x0 is an initial estimate of the state vector, r is a rotation radius, t is a discrete-time step, T is a list of lengths of trajectory pieces, a, ft are error levels for the decision rule.
Algorithm. ATE (Adaptive Trajectory Estimation)
INPUT: xo, r, t, T, a, [. OUTPUT: the estimated trajectory X.
COMPUTATION.
1. A:= ln^, B := In rr£—
a ' 1—a
2. k := 1, x := x0, P := I, qA := 0, prevMoment := 1
3. for i := 1,..., size(T)
4. nextMoment := k + T
5. isChanged := false
6. Iab := {0,1, 2}
7. F := setFilters(x, P, r, t)
8. while (k < nextMoment) && not(isChanged)
9. [x, P, N, S] := makeStep(zk, F, IAB)
10. x:= XqA ,P := PqA
11. Xfc := x
12. A := calculateLambda(N, S, IAB, qA)
13. [isChanged, qA, IAB] := makeDecision(A, IAB, qA)
14. k := k + 1
15. end while
16. if (isChanged)
17. for j := prevMoment,..., k — 1
18. [x, P] := makeStep(zj, F, {qA})
19. Xj := x
20. end for
21. end if
22. for j := k,..., nextMoment — 1
23. [x, P] := makeStep(zj, F, {qA})
24. X- := x
25. end for
26. k := nextMoment
27. prevMoment := nextMoment
28. P := I
29. end for
Algorithm starts by calculating upper and lower thresholds A and B for the decision rule (line 1). Then current time k, estimate x of state vector x, covariance matrix P and index of current filter qA are initialized (line 2). After that algorithm iterates through all trajectory pieces trying to find changes in the motion mode at the beginning of each piece. Note, that k =1 is also considered as the potential moment of change.
Let us consider each iteration of the main loop (lines 3-29) in more detail. At the beginning of each iteration, the next moment of the potential change is calculated, variable isChanged and the set of working filters IAB are initialized. Then the function setFilters() is called which returns the bank of Kalman filters F = (F0, F1, F2) all of which are active. After that algorithm iterates in while loop (lines 8-15) until it finds the change in the motion mode or the next moment of the potential change is reached. At each iteration of this loop function makeStep() is called (line 9) which performs one step of Kalman
filtering procedure for each working filter and current measurement zk and returns sets of estimates X, residuals N, covariance matrices P and E for these filters. Current estimate x of the state vector and its covariance matrix P on each iteration are set to XqA and PqA respectively. After that, the likelihood ratios are calculated and the decision rule is applied (line 12). The function makeDecision() updates the set of working filters at each iteration and returns the new index q^ of the current filter if it finds the change in the motion mode. If the change in the motion mode is found then the algorithm first recalculates estimates from the beginning of the current trajectory piece up to the current moment of time (lines 16-21) and then iterates up to the end of the trajectory piece if it is not yet reached with current filter FqA (lines 22-25) producing estimates x and covariance matrices P otherwise it continues with the next trajectory piece.
The main novelty of this algorithm compared to the one proposed in [6] is that state estimates are recalculated with the filter corresponding to the selected hypothesis (lines 1621). This allows obtaining more accurate state estimates.
3. Numerical Experiments
We would like to substantiate the new adaptive estimation algorithm proposed in Section 2 in practice. For that, we consider the next example.
Example 1. A moving object trajectory is defined by the following scheme: (S, 250), (R, 314, 5), (S, 250), (L, 314, 5), (S, 250), (L, 314, 5), (S, 250), (L, 314, 5), (S, 250) where (S, 250) means straight motion for 250 discrete-time moments and (R/L, 314, 5) means the turn to the right/left for 314 discrete-time moments with radius 5. So, the whole trajectory consists of 9 pieces and the total time of motion is 2506 discrete-time moments. We need to estimate the parameters of the object motion.
First, we have obtained model data of measuring the coordinates of the object as it moves along a given trajectory. We have simulated measurements data for 100 different trajectories using MATLAB. Initial state vector is x0 = [0, 0, 0, 0,25]T. The covariance matrix of the object noise is Q = 0. Consider three levels of uncertainty:
1) R = diag[0,01, 0,01], 2) R = diag[0,1, 0,1], 3) R = diag[1, 1]. (2)
We have performed the following set of numerical experiments. The object motion model (1) is simulated for k = 1,..., 2506 to generate the "exact" state, xexact(tk), and available measurements, zk. Next, the adaptive estimation problem is solved by the proposed algorithm, i. e. we perform the trajectory parameters tracking for estimating unknown state vector xk. The experiment is repeated for 100 times and then RMSE (the root mean square error) in each component of the state vector is calculated as follows:
I M N ~
RMSEj,. = J^ E (Zifc - &ik) > where M = 100> N = 2506' the x\k and are V j=1k=1
the ith entry of the "true" state vector (simulated) and its estimated value obtained in the jth experiment, respectively. Together with the RMSEx., the normalized RMSE (nRMSE) (i.e. ||RMSEx||2) for each level of uncertainty are shown in Table.
Fig. 1 shows the obtained results of modelling and adaptive estimation of a moving object trajectory given in example 1 for the third level of uncertainty (2). The measurements are indicated by grey dots, the calculated estimates of object coordinates
Table
The RMSE in Example 1, M =100 runs
Level RMSEXi nRMSE
X\ X2 X3 X4
i) 2) 3) 0,2273 0,0404 0,2038 0,0377 0,2926 0,0449 0,2399 0,0408 0,3791 0,0575 0,3440 0,0568 0,3103 0,3832 0,5182
Fig. 1. Adaptive estimation of a moving object trajectory
Fig. 2. Mode detection process
are coloured according to detected motion mode: straight motion is painted in red, left turn is painted in blue, and the right turn is painted in green.
Fig. 2 shows a mode detection process for Example 1 given the third level of uncertainty (2). The calculated values of likelihood ratios Ak,ij are coloured according to the corresponding motion mode: straight motion mode is painted in red, left turn mode is painted in blue, and right turn mode is painted in green. We can see which motion mode has been detected on each piece of the trajectory.
Having analyzed the obtained numerical results presented in Table and illustrated in Figure, we make a few conclusions. First, for each component of the state vector x the quality of calculated estimates is good (RMSEXi, i = 1,..., 4 is small enough). Second, the motion modes were detected correctly in all three considered cases, even when the level of the measurements noise is high enough.
Thus, the proposed Algorithm (ATE) of adaptive estimating moving object trajectory allows to detect correctly the motion mode and simultaneously calculate optimal linear estimates of the state vector x. Therefore, it can be recommended for solving practical tracking problems in real-life applications.
Conclusions
In the present paper, a new algorithm ATE for adaptive estimation of a moving object trajectory is proposed. The novelty of this algorithm is that it combines the hybrid linear stochastic model of an object moving along a complex trajectory with Kalman filtering and a sequential hypothesis testing. It is supposed that at known moments of time an object
may change its motion to one of three possible modes. Such an approach may be used, for example for ground and marine objects tracking. The proposed algorithm was implemented in MATLAB and its efficiency was confirmed by multiple numerical experiments. Further, it will be extended to the general case when moments of changes in the motion mode are unknown.
Acknowledgements. This work was supported by the Russian Foundation for Basic Research (grants 16-41-730784, 18-37-00220).
References
1. Kim S., Park J., Lee J. Implementation of Tracking and Capturing a Moving Object Using a Mobile Robot. International Journal of Control, Automation, and Systems, 2005, vol. 3, no. 3, pp. 444-452.
2. Hassani V., Pascoal A.M., Sorensen A.J. A Novel Methodology for Adaptive Wave Filtering of Marine Vessels: Theory and Experiments. Proceedings of the 52nd Annual Conference on Decision and Control (CDC), Florence, Italy, 2013, pp. 6162-6167. DOI: 10.1109/CDC.2013.6760863
3. Bar-Shalom Y., Li X.R., Kirubarajan T. Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software. New Jersey, John Wiley and Sons, 2002.
4. Semushin I.V., Tsyganov A.V., Tsyganova Yu.V., Golubkov A.V., Vinokurov S.D. Modelling and Estimation of a Moving Object Trajectory. Bulletin of the South Ural State University. Series: Mathematical Modelling, Programming and Computer Software, vol. 10, no. 3, pp. 108-119. DOI: 10.14529/mmp170309
5. Wald A. Sequential Analysis. N.Y.: John Wiley and Sons, 1947.
6. Golubkov A.V., Tsyganov A.V., Tsyganova Yu.V. Adaptive Estimation of an Object Motion Parameters Based on the Hybrid Stochastic Model. Journal of Physics: Conferences Series, 2018, vol. 1096, no. 1, 012166 p. DOI: 10.1088/1742-6596/1096/1/012166
Received September 24, 2018
УДК 004.942 DOI: 10.14529/mmp190115
АДАПТИВНОЕ ОЦЕНИВАНИЕ ТРАЕКТОРИИ ДВИЖУЩЕГОСЯ ОБЪЕКТА С ИСПОЛЬЗОВАНИЕМ ПОСЛЕДОВАТЕЛЬНОГО ТЕСТИРОВАНИЯ ГИПОТЕЗ
А.В. Цыганов1, Ю.В. Цыганова2, А.В. Голубков1, И.О. Петрищев1
Ульяновский государственный педагогический университет им. И.Н. Ульянова, г. Ульяновск, Российская Федерация
2Ульяновский государственный университет, г. Ульяновск, Российская Федерация
В статье рассматривается задача адаптивного оценивания траектории движущегося объекта и обнаружения изменения режима движения. Предполагается, что объект движется по сложной траектории и в известные моменты времени может происходить переключение режима движения объекта на один из трех возможных режимов: равномерное прямолинейное движение или равномерное движение против или по часовой стрелке. Разработан новый алгоритм, основанный на сочетании гибридной линейной стохастической модели движения объекта, банке конкурирующих фильтров Калмана и решающем правиле на основе последовательного тестирования гипотез. Приведено
детальное описание решающего правила и предложенного алгоритма в виде псевдокода. Программная реализация алгоритма выполнена на языке Matlab. Рассмотрен численный пример адаптивного оценивания движения объекта по сложной траектории, состоящей из девяти различных участков. Проведены вычислительные эксперименты с различными уровнями помех в измерениях. Полученные результаты подтверждают эффективность предложенного алгоритма.
Ключевые слова: йдаптивное оценивание; движущийся объект; последовательное тестирование гипотез.
Работа выполнена при поддержке РФФИ (проекты 16-41-730784, 18-37-00220). Литература
1. Kim, S. Implementation of Tracking and Capturing a Moving Object Using a Mobile Robot / S. Kim, J. Park, J. Lee // International Journal of Control, Automation and Systems. - 2005. - V. 3, № 3. - P. 444-452.
2. Hassani, V. A Novel Methodology for Adaptive Wave Filtering of Marine Vessels: Theory and Experiments / V. Hassani, A.M. Pascoal, A.J. Sorensen // Proceedings of the 52nd Annual Conference on Decision and Control (CDC), Florence, Italy. - 2013. - P. 6162-6167.
3. Bar-Shalom, Y. Estimation with Applications to Tracking and Navigation: Theory, Algorithms and Software / Y. Bar-Shalom, X.R. Li, T. Kirubarajan. - New Jersey: John Wiley and Sons, 2002.
4. Семушин, И.В. Моделирование и оценивание траектории движущегося объекта / И.В. Семушин, А.В. Цыганов, Ю.В. Цыганова, А.В. Голубков, С.Д. Винокуров // Вестник ЮУрГУ. Серия: Математическое моделирование и программирование. - 2017. -Т. 10, № 3. - С. 108-119.
5. Wald, A. Sequential Analysis / A. Wald. - New York: John Wiley and Sons, 1947.
6. Голубков, А.В. Адаптивное оценивание параметров движения объекта на основе гибридной стохастической модели / А.В. Голубков, А.В. Цыганов, Ю.В. Цыганова // Сборник докладов IV Международной конференции «Информационные технологии и нанотех-нологии» (ИТНТ 2018), Самара, Россия. - 2018. - С. 2064-2074.
Андрей Владимирович Цыганов, кандидат физико-математических наук, доцент, кафедра «Высшая математика», Ульяновский государственный педагогический университет им. И.Н. Ульянова (г. Ульяновск, Российская Федерация), [email protected].
Юлия Владимировна Цыганова, доктор физико-математических наук, доцент, кафедра «Информационные технологии», Ульяновский государственный университет (г. Ульяновск, Российская Федерация), [email protected].
Алексей Владимирович Голубков, аспирант, кафедра «Высшая математика», Ульяновский государственный педагогический университет им. И.Н. Ульянова (г. Ульяновск, Российская Федерация), [email protected].
Игорь Олегович Петрищев, кандидат технических наук, проректор по учебно-методической работе, Ульяновский государственный педагогический университет им. И.Н. Ульянова (г. Ульяновск, Российская Федерация), [email protected].
Поступила в редакцию 24 сентября 2018 г.
