Adaptive estimation of an object motion parameters based on the hybrid stochastic model

This paper examines the problem of estimating parameters of the motion of an object moving along a complex trajectory in the presence of incomplete noisy measurements. This problem is of current interest because of the importance of its practical applications. Examples of such applications are the tracking problems of moving technical devices in robotics, navigation, ground vehicles. We propose a solution to the problem of adaptive estimation of the motion parameters of an object based on the hybrid stochastic model, which is a set of discrete linear stochastic models corresponding to various pieces of the object’s trajectory. The linearity of the models allows us to apply the optimal Kalman filtering to calculate the estimates of the motion parameters of an object. To identify the current motion mode of an object at a known time moments, new algorithm based on the use of the Kalman filters bank and sequential ratio probability test is developed. Numerical experiments confirm the efficiency of the obtained results.


Introduction
We address the problem of adaptive estimation of the parameters of an object motion along a complex trajectory with simultaneous identification of the motion mode. Object motion with different modes is typical for technical objects, for example, for mobile robots [1], for marine vessels [2,3], and also for land vehicles [4]. This problem is difficult because only uncertain noisy data about the object are available. To solve the problem, it is required to detect as soon as possible changes in the motion mode of an object in order to calculate then the optimal estimates of the motion parameters for the current piece of the trajectory.
The problem of parameter estimation of a manoeuvering object has been researched in the literature for many decades, see [5]. It has many impornant applications and many approaches to solution, among them the one well known approach is the multiple-model tracking technique. Nowadays the interest in moving object tracking tasks does not decrease. This is evidenced by great number of publications in this area of research. For example, in [6] authors have presented an algorithm which is able to successfully estimate the ego-velocity of a vehicle equipped with a radar sensor based on stationary raw targets. A cost-effective approach to track moving objects around vehicles using linearly arrayed ultrasonic sensors was proposed in [7]. Paper [8] presents a modified particle filter (PF) algorithm based on elimination of unimportant particles which improving the tracking process. A novel multiple model adaptive complementary filter 2 scheme that has been devised in [9] takes the advantage of multiple model adaptive estimation (MMAE) and the simple structure of linear complementary filter for estimating attitude of a moving vehicle. The proposed scheme is independent of any manually fed parameter and is able to converge to the most accurate filter on the basis of obtained residual error stream from parallel filter blocks. The main contribution of [10] is the proposal of a new tracking algorithm based on Gaussian Mixture Probability Hypothesis Density (GMPHD) filter and the comparison of the different features combination methods. The performance reached with this tracking algorithm makes it a suitable alternative to the available methods, such as Kalman Filter (KF) and Unscentered Kalman Filter (UKF). [11] has proposed a systematic real-time detection and tracking method for an indoor moving sphere using the 3D LiDAR sensor. To solve the problem the Kalman filter and the adaptive particle filter methods are used to estimate the real-time motion state of a spherical object. In [12] authors present an integrated control approach for autonomous driving comprising a corridor path planner that determines constraints on vehicle position, and a linear time-varying model predictive controller combining path planning and tracking in a road-aligned coordinate frame. A unified vehicle tracking and behavior reasoning algorithm presented in [13] can simultaneously estimate the vehicle dynamic state and driver intentions. The multiple model filter based on various behavior models was used to classify the vehicle behavior and estimate the dynamic state of surrounding vehicles. In [14] authors suggest the formula for calculating the turn rate of a maneuvering target. Based on this formula, they estimate the turn rate of a target by using its state vector at a previous time, and its measurement at the current time. By applying the estimation method of the turn rate to the multi-target Bayes (MB) filter, authors develop a MB filter with an adaptive estimation of the turn rate, in order to track multiple maneuvering targets.
In the present paper to simulate an object trajectory we use a hybrid stochastic linear model proposed in [15] in which separate pieces of the trajectory are described by linear discrete models corresponding to the one of different motion modes. This approach to the trajectory modeling allows to replace a complex and nonlinear model of an object movement with a set of linear models for which well-known optimal discrete Kalman filtering algorithm can be applied [16]. But in order to be able to calculate optimal estimates of the object state which allow to track the object and predict its movement there is a need to quickly detect changes in the motion mode. The purpose of this paper is to develop an algorithm which efficiently solves this problem.
Our approach to the problem solution is similar to the other existing approaches for the modeling and estimation of a piecewise trajectory (see for example, [17][18][19]). In contrast to the papers mentioned, the main idea of the approach considered here is to apply the sequential probability ratio test (SPRT) [20,21] at each moment of the motion mode change to identify the new motion mode. Then the appropriate optimal discrete Kalman filter is chosen, which optimally filters the measurements. The new algorithm proposed in this paper was implemented in MATLAB. The efficiency of this algorithm is confirmed by the numerical experiments.

Hybrid stochastic model of a moving object trajectory
Assume that the trajectory of an object can be considered as a sequence of 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.
Let us consider a set of such models. Then the motion of the object on the entire trajectory can be described by a hybrid stochastic model where k is a dicrete-time moment, i is a motion mode number; r i is a given radius of a circular motion (left turn/right turn), x = [x 1 , x 2 , x 3 , x 4 ] T ∈ R 4 is a state vector of the object motion 3 model, in which x 1 is a coordinate x of the object along the axis Ox (m), x 2 is a velocity v x along the axis Ox (m/s), x 3 is a coordinate y of the object along the axis Oy (m), x 4 is a velocity v y along the axis Oy (m/s). Let us suppose that the moments of changes in the motion mode are known.
Model (1) is given by the following matrices: • A uniform straight line motion (the number of motion mode i = 0, r 0 = 0): • A uniform anticlockwise circular motion (left turn) with a given radius r i (the motion mode numbers i = 1, 2, . . . , n): • A uniform clockwise circular motion (right turn) with a given radius r i (the motion mode numbers i = n + 1, n + 2, . . . , 2n): T is a velocity vector at a point with coordinates (x si , y si ) at the moment t s of the motion mode change, parameter ω i > 0 and Thus, the hybrid stochastic model (1) allows to model a complex trajectory of the object movement. In [15] there was developed the algorithm, based on the presented hybrid model, for simulating a trajectory with a change in the motion modes at a known discrete-time moments. Let us consider the example of the trajectory modeling of a moving object. We define the scheme for modeling the trajectory in the form of a three column table TS. Each row of this table corresponds to the separate trajectory piece. The value in the first column of each row describes the motion mode (S for the straight motion, L for the left turn or R for the right turn), the second column contains the length of the trajectory piece and the third column contains a radius of a circular motion. The radius value is not specified for motion mode S. Table 1 contains an example of the moving object trajectory. The trajectory consists of six pieces: 1) turn to the left with the radius of 3 (50 discrete-time moments), 2) turn to the right with the radius of 5 (75 discrete-time moments), 3) straight motion (50 discrete-time moments), 4) turn to the left with the radius of 3 (75 discrete-time moments), 5) straight motion (50 discrete-time moments), 6) turn to the left with the radius of 4 (60 discrete-time moments). The total time of motion is 360 discrete-time moments. Figure 1 shows an example of the simulated trajectory on the Oxy plane. Simulation results have been obtained using MATLAB program [23]. The trajectory pieces of straight motion is painted in red, left turn is painted in blue, and right turn is painted in green. Let us assume that only coordinates x 1 and x 3 are measured, velocities x 2 and x 4 are not measured. Then the measurement model can be written as where H is the measurements matrix, v k is the measurements error, which is a Gaussian white noise with zero mean and diagonal measurement noise covariance matrix R = diag[ρ 1 , ρ 2 ].

Sequential decision making rule based on the bank of Kalman filters
Consider a set of M = 2n + 1 hypotheses about possible motion modes of an object.
Hypothesis H 0 : An object performs a uniform straight line motion.
Hypotheses H i : An object performs a uniform anticlockwise circular motion (left turn) with a given radius r i (i = 1, . . . , n); an object performs a uniform clockwise circular motion (right turn) with a given radius r i (i = n + 1, . . . , 2n).
Consider a set of competitive Kalman filters F = (F 0 , F 1 , . . . , F 2n ), in which each filter is optimal for the corresponding hypothesis H i , i = 0, 1, 2, . . . , 2n. Equations of i-th filter in the bank F are written in the form of two-stage Kalman filter [16]: Each of the following sequences of residuals calculated by the Kalman filter, consists of mutually independent values ν τ,i = z τ − Hx − τ,i , τ = 1, 2, . . . , k, subject to the condition that the corresponding hypothesis H i is true (i = 0, 1, . . . , 2n).
Sequential probability ratio test SPRT [20,21] contains the following algorithm to evaluate the likelihood ratios (i ̸ = j): The mean and variance of sequences (3) for the set of Kalman filters F are given by The values of λ k,ij are then tested against two threshold levels A and B (A > B): We assume that all hypotheses are equiprobable. Then thresholds A and B are chosen in the following way:

Algorithm for adaptive estimation of the object motion parameters
We developed the algorithm 1 to solve the problem of adaptive estimation of the trajectory with simultaneous identification of the current motion mode. It is based on the hybrid stochastic model (1), the sequential decision rule (5) and the concept of the Kalman filter bank.
The input data for the algorithm 1 are: x 0 is an initial estimate of the state vector, τ is a discrete-time step, T is a list of lengths of trajectory pieces, α and β are error levels for the decision rule, r min and r max are the minimal and maximal values of a rotation radius, n is a number of radii. Algorithm 1 starts from calculating upper and lower thresholds A and B for the decision rule (line 1). Then the current time k, state estimatex and index of the current filter q A are initialized (line 2). After that algorithm 1 iterates through all trajectory pieces trying to find changes in the motion mode at the beginning of each piece. The beginning of the first piece (time moment k = 1) is also considered as a possible moment of change in the motion mode.
Let us consider one iteration of the main loop in more detail. At the beginning of each iteration the next moment of the potential change nextMoment is calculated (line 4), variable isChanged and the set of working filters I AB are initialized (line 5). After that the function setFilters() is called which returns the bank of Kalman filters F = (F 0 , F 1 , . . . , F 2n ) (line 6). The filter F 0 corresponds to the straight line motion, filters F 1 , . . . , F n correspond to the circular motion to the left, and filters F n+1 , . . . , F 2n -the circular motion to the right with radii from r min to r max with step r min − r max n − 1 . Then algorithm 1 iterates in while loop (lines 7-13) until it finds the change in the motion mode or the next moment of the potential change is reached. 7 At each iteration of this loop function makeStep() is called (line 8) which performs one step of Kalman filtering procedure for each working filter and current measurement z k and returns state estimatesx, covariance matrices P, values ν and Σ for these filters.
The current estimatex of the state vector and its covariance matrix P on each iteration are set tox q A and P q A respectively (line 9). After that likelihood ratios are calculated (line 10) and the decision rule is applied (line 11). Function makeDecision() updates the set of working filters at each iteration and returns the new index q A of the current filter if it finds the change in the motion mode. If the change in the motion mode is found before the end of the current trajectory piece then the algorithm iterates up to its end with the current filter F q A (lines 14-16) calculating estimatesx and covariance matrices P otherwise it continues with the next trajectory piece.

Numerical Experiments
Consider the trajectory of a moving object, represented by the scheme given in Table 1. We need to estimate the trajectory and detect the motion mode on each piece of the trajectory based on noisy measurements of the coordinates of a moving object.
Using the program [23] developed in MATLAB, one hundred different trajectories were modeled given the initial state vector x 0 = [0, 0, 0, 2] T , the process noise covariance matrix Q = diag[0.001, 0.001] and the measurements with three levels of uncertainty (different values of measurements noise covariance matrix R): We perform the following set of numerical experiments. Each simulated trajectory was firstly evaluated using a standard Kalman filter corresponding to the optimal motion mode and parameters on each piece of the trajectory, which are given in Table 1. Then we evaluated the trajectory with the help of the proposed adaptive estimation algorithm 1 with the identification of the motion mode. For the motion mode identification a bank of Kalman filters was used with the parameters {r min = 1, r max = 10, n = 10}.
The results of estimation are shown in Figures 2, (a) and (b). The modeled 'true' trajectory is shown in light blue, the trajectory estimates are colored in red, blue and green according to detected motion mode. From the Figure 2, (b) it can be seen that the motion mode on the third piece of the trajectory was determined incorrectly (the turn to the right instead of the straight line motion). The average percentage of correctly defined motion modes is 72.5 % for R 1 , 77 % for R 2 and 79.8 % for R 3 .
The operation of the Kalman filter bank on the last piece of the trajectory is shown in Figure 3.
We wish to confirm the efficiency of the proposed estimation algorithm. To do this, we have compared at each discrete-time moment k = 1, . . . , 360 simulated 'true' values of the state vector x exact (t k ) and the obtained estimatesx k , with the help of the calculated value RMSE x (root mean square error) upon the data of all experiments: where M = 100, N = 360, the x j i,exact (t k ) andx +,j i,k are the i-th entry of the 'true' state vector (simulated) and its estimated value obtained in the j-th experiment, respectively. Besides the RMSE x , the normalized nRMSE = ∥RMSE x ∥ 2 has been calculated for each level of uncertainty (see Tables 2 and 3).    Table 4 demonstrates the average decision time (the number of discete-time moments) spent on detecting and identification of the motion mode on each piece of the trajectory by way of adaptive estimation algorithm.  The obtained results confirm the efficiency of the proposed algorithm. The quality of estimation depends, in particular, on the time of the decision making about the motion mode change, the level of measurements noise and, of course, on the correct identification of the motion mode number. So, in most cases with different levels of uncertainty the motion mode number is identified correctly, the turn radii are determined in a worse way. This can be explained by the fact that the parameter r i , which corresponds to the radius, depends on the estimate of the velocity vectorv si at the point t s of a possible change in the motion mode.

Conclusion
In this paper we address the problem of adaptive estimation of an object motion parameters in real time based on the hybrid stochastic model. A new adaptive estimation algorithm is proposed that uses Kalman filtering and a method for early change detection with a sequential hypotheses testing about possible motion modes of an object moving along a complex trajectory. The trajectories consisting of N pieces are considered and on each piece of the trajectory the object can move in accordance with one of the 2n + 1 possible motion modes: straight motion, circular clockwise motion (right turn) and circular anticlockwise motion (left turn) with one of n possible radii. The proposed algorithm was implemented in MATLAB and its performance was confirmed by the results of numerical experiments.
Thus, we showed the principal possibility of solving the adaptive estimation problem of an object motion parameters in real time based on the hybrid stochastic model with identification of M possible motion modes. The limitation of this approach is the knowledge of the moment of possible change in the motion mode. Note that the quality of motion parameter estimates can be improved. Therefore, the further development of this approach to the problem of adaptive estimation of the trajectory will be continued in the following directions: development of new algorithms for adaptive parameters estimation of the motion of an object in real time in the case when the moments of change in the motion mode are unknown (we plan to use results,