Adaptive Trajectory Estimation with Power Limited Steering Model under Perturbation Compensation

Trajectory estimation under regional correlations is applied in numerous tasks like dynamic path planning, navigation and tracking. Many previous works get impressive results in the strictly controlled condition with accurate prior statistics and specific dynamic model for certain object. While in a more challenging situation without specific dynamic model and accurate prior statistics, the performance of these methods significantly declines. To estimate the trajectory without specific dynamic model, we propose a general model called the power-limited steering model (PLS), which is a natural combination of instantaneous power and instantaneous angular velocity. As a nonlinear model it needs less parameter to describe the switching of states compared with linear Markov process model. Then we derive the corresponding form in discrete time and compensate the nonlinear effect of perturbation with renormalization group. To overcome the biased prior statistics, the observations drift and linear growing computation in trajectory estimation, we propose the adaptive trajectory estimation (AdaTE) where the online updated statistics and the adaptive truncation time is applied. The experiment of typical trajectory estimation demonstrates that compared with EKF and UKF, AdaTE is more robust to the biased prior statistics and observation drift. Another task shows that with slight modification, AdaTE can be used in path planning that needs obstacle avoidance. Finally, we exhibit its potential application on trajectory optimization in visual tracking.


Introduction
Trajectory estimation plays a critical role from industrial appliances to research areas. It is widely used in numerous tasks like path planning, 1 navigation, 2 visual tracking 3 and simultaneous localization and mapping (SLAM). [4][5][6] Successful trajectory estimation depends on three key aspects: dynamic model, measurement model, and estimation algorithm.
Many previous works 7- 10 have got impressive results under the strictly controlled conditions, where dedicated dynamic model for certain system, 11,12 a precise measurement model, and accurate prior statistics can be obtained. However, in some situations where dedicated dynamic model is unobtainable and prior statistics are inaccurate, these methods may be infeasible. Meanwhile, some sensors may experience observation drift. Without any correction, it would lead to serious biased estimation. In this paper, we try to solve the problems above and embark upon two aspects: the design of dynamic model, and robust estimation algorithm.
To describe the movement of non-cooperative or maneuvering objects for the tasks with randomness such as radar tracking and pedestrian path prediction, many outstanding works have created various general dynamic models for maneuvering object tracking. A large category of them is the random model, which is a combination of state evolution model and random control. It contains 13 : wiener-process acceleration model, 2 Markov process models such as Singer model, 14 and semi-Markov jump process models. 15 These models try to describe the typical states and their switching during maneuver, and get excellent predictions when all of the quantized levels and corresponding probabilities are well designed. But in sever conditions where dedicated dynamic model is unobtainable and prior statistics are inaccurate, they will face two problems. (a) Some models urgently rely on the switching probability between quantized levels, which is a stringent condition in an environment lacking reliable prior information. (b) In most of these models, the movements in orthogonal directions are assumed to be uncoupled with each other, which will weaken the ability of trajectory prediction in many cases such as turning, and split-s maneuver.
Aiming at sever conditions where dedicated dynamic model is unobtainable and prior statistics are inaccurate, we learn from the previous works and propose the power-limited steering model (PLS). It is a natural combination of instantaneous power theory and instantaneous angular velocity theory of Newtonian mechanics. Through the joint action of power and damping, it overcomes the infinite speed problem that occurred in constant mean acceleration model 16 in trajectory prediction. Resort to the strong nonlinearity, PLS needs fewer parameters to describe the switch of typical states compared with Markov jump models.
Estimation algorithm is another emphasis. One famous approach is the Kalman filter (KF) 17 and Kalman smoother 10 for linear dynamic models. To expand the application to nonlinearity, the extended Kalman filter (EKF) and the Unscented Kalman Filter (UKF) 18,19 were developed. But above filters have common defects: a) They are sensitive to the model error and sensitive to the prior statistics; b) Fixed and zeros centered noise model is applied, which leads to the fragility to the observation drift. To overcome these problems, some specifically designed methods have been proposed, such as eXogenous Kalman Filter (XKF) 20 maximum correntropy Kalman filter (MCKF), 21 and the Robust Kalman Filtering (RKF). 22 Each of these methods tries to solve certain problem, but none of them deals with the difficulties in a single light framework. Another famous method is the particle filter [23][24][25][26] Through the resampling strategy, 25 the particle distribution approximates the real state distribution. Compared with Kalman filters, it is much more robust to the biased prior statistics. However, because of the huge computation for maintaining a massive number of candidate trajectories, particle filter is difficult to be applied in real-time estimation.
Different from the filtering approaches, the optimization approaches can produce globally consistent result. A representative illustration is the graph optimization for SLAM. 27,28 However, the basic graph optimization is sensitive to dynamic model and prior statistics. If strongly nonlinear models are invoked with biased covariance matrices, wrong results and divergence may occur. Besides, sparse Cholesky factorization 27 and Krylov subspace methods 29 is usually applied to accelerate the calculation in graph optimization with sparse global correlations named loop closures (some states have direct relationship with the states long time ago). When being applied in a task with only regional correlations, they would bring a lot of unnecessary calculation. To taking advantage of the regional correlations, we have proposed a sparse maximum a posteriori estimation algorithm (sparse MAP), 8 in our previous work. It is efficient in short trajectory estimation. But the linear increasing calculation over time prevents the application to long term estimation. 30 Our motivation of designing estimation algorithm focus on improving the robustness to biased prior statistics while keeping efficiency by adaptively limits the updating range. Based on the previous work, sparse MAP, 8 we propose a specifically designed optimization method called the adaptive trajectory estimation algorithm (AdaTE) with regional correlation hypothesis and PLS model. The process is plotted in Figure 1. Firstly, the trajectory is updated based on the previous estimation and corresponding statistics. Then, the statistics (including transition covariance matrices and observation covariance matrices) is updated based on the newest trajectory and historical observations. Finally, the truncation time confining the range to be updated is calculated based on the difference between previous and updated trajectory. The motivations of these procedures are: a) The adaptation of transition covariance matrices improves the estimation robustness to the model error and prior statistics; b) The correction of observation covariance matrices improves the estimation robustness to the observation drift; c) The truncation time ensures the calculation is restricted to the necessary part, which avoids the linear improvement of calculation.
The main contributions of our article can be summarized as follows: a) A stochastic nonlinear model based on Newton Mechanics is proposed for non-cooperative maneuvering object, named the power limited steering model(PLS), which sustains few adaptive parameters to describe the motion states; b) A novel adaptive trajectory estimation algorithm is proposed, which is robust to the biased prior statistics and observation drift, and can perform online correction of statistical parameters with computational complexity lower than O(n); c) Extensive experiments demonstrate that the AdaTE can be applied in trajectory estimation, local navigation and trajectory optimization for visual tracking successfully.
The overall structure of this article is described as follows. Section "Related works" analyzes the current research work on dynamic models and trajectory estimation algorithms briefly. Section "Power limited steering model" deduces the power limited steering model, which can effectively describe the movement of the target. In addition, the convergence of PLS has also been proved. A novel trajectory estimation algorithm is introduced in section "Adaptive trajectory estimation" in detail, which can accurately and robustly estimate the trajectory. Section "Experiments" shows three verification Experiments, including trajectory estimation of 3D observation, local navigation under random obstacles and trajectory optimization of visual tracking, and section "Conclusion" gives the main conclusion of our work.

Related works
Dynamic model and estimation algorithm are two key factors for trajectory estimation. In this section, we firstly introduce some subclass of dynamic model, their representative members, and their relationship and difference with PLS model. Then we talk about the estimation algorithm and make a contrast of filtering methods with optimization methods.
The assumptions in this paper are grouped as follow: 1) the dedicated dynamic model for particular object cannot be obtained; 2) all of the prior statistics, including transition covariance and observation covariance are inaccurate; 3) the correlation between states is regional, that means the state evolution is a Markov process; 4) part of the observations have drift.

Dynamic model
Some dedicated models11 are designed for cooperative object and system. But they are restricted to dedicated machine under accessible control. For non-cooperative object whose control is unknown to the observer, it is hard to design a dedicated model. That means the trajectory of these objects cannot be accurately forecasted. A popular alternative is describing the movement as a stochastic process with random control. It is known as random model and can be classified into three subclasses: white noise models, Markov process models and Semi-Markov process models. One simplest and representative dynamic model is the Wiener-process acceleration model, 31 where acceleration is assumed to obey the Wiener process. Many following constant acceleration (CA) models are inspired by this work. Lets x, v and a be the position, velocity and acceleration of the object, Setting the state vector as sðtÞ ¼ ½xðtÞ,vðtÞ,aðtÞ� T .
The Wiener-process acceleration model is presented as where u is the control vector and it is an independent process (white noise) of acceleration with power spectral density D. A is the state transition matrix, A ¼ Based on the antitype of Wiener-process, many following works have been proposed, such as the polynomial models, the Singer model, 14 "current" model and semi-Markov jump process model. 15 Their improvement mainly focuses on the distribution of random vector, such as quantized levels of expectations, where a i is the quantized expectation of acceleration,ã is the zero-mean random vector, and zðsÞ, s < t is all of the observations before time t. Another well-known approach is modifying the state transition matrix. However, whatever they are modified, the dynamic models are integration of many linear models for different status. Thus, a large number of quantized levels and corresponding conditional probability based on reliable prior information are required to distinguish different typical movements in a specific model. A typical illustration is the quantized expectations of acceleration a i , i ¼ 1; 2,/, n defined in (3). Unreliable prior information is a challenge for them.
Another impressive work is the generative model for maneuvering target tracking (GMTM)30, where the motive force is divided into the axial force and centripetal force to describe axial acceleration and angular velocity based on Newton mechanics. But the problem is that, to limit the output power given by the product of force with speed, GMTM models the speed-force constraints using conditional Rayleigh distributions. It increases a lot of calculation.
To inherit the advantage and remedy deficiency of GMTM30, we propose the power-limited steering model (PLS). Its main innovation is the combination of instantaneous power theory and instantaneous angular velocity theory from Newton mechanics. Compared with GMTM, PLS is a lot of cheaper in limiting the output power through the power damping interaction. Moreover, PLS is a nonlinear model, thus it needs fewer prior parameters to describe the typical states compared with Markov process models. However, the nonlinearity of PLS leads to the extra difficulty in deriving corresponding discrete time model. It will be specifically discussed in next section of this paper.

Estimation algorithm
The estimation algorithm can be conventionally separated into two categories: filtering approaches and optimization approaches. Normally, filtering approaches result in the expectation of the states at each time under certain distribution, while optimization approaches estimate a trajectory with maximum global probability. The filtering approaches were widely used at early time because they were believed to require fewer computations compared with optimization approaches. Until the optimization framework was found to have sparse associations, it starts to be popular.
A typical category of filtering approach is the Kalman Filter family, includeing Kalman Filter (KF), extended Kalman filter (EKF), Unscented Kalman Filter (UKF), 18,19 Cubature Kalman Filters (CKF) 32,33 and some recent developed methods such as eXogenous Kalman Filter (XKF) 20 and the Maximum Correntropy Kalman filter (MCKF). 21 For the real-time state estimation, they execute a forward propagation: where s i ,i 2 f1,/,kg is the state at time i, Z k is the observations at time k which might contain several independent observations, and Z k ¼ fZ 1 ,/,Z k gis the set of observations from time 1 to time k.
In trajectory estimation, they are known as smoothers to correct the previous states in an inverse time order: In (5), trajectory is estimated as a set of expectations at each moment. The result is not the trajectory with the highest probability but rather a compromise of possibility. If there is a dedicated dynamic model and accurate prior statistics (especially the conditional distribution), it would be easy to obtain an accurate trajectory; otherwise if parts of the priori statistics are inaccurate or even biased, there will be systematic deviation of result. For example, the prior information indicates that the noise of observation is zero-mean while it has drift in fact, the estimated trajectory will shift to one side. Besides, how to set the truncation time to limit the updating range of trajectory is another problem for the filtering approaches. Although some researches worked on this issue, like restricted memory filtering, they still left some problem such as how to adjust the length of memory online.
Different from the filtering approaches, the optimization approaches, such as graph optimization [34][35][36] for SLAM4-6, can be mentioned as finding the trajectory with the highest probability from observations. For a linear model with a simple distribution, we can obtain an analytic solution. However, for nonlinear model or complex distribution that do not have analytic solutions, it's difficult to get an analytical solution. Thus, numerical solutions can be worked out through the Newton-Raphson, BFGS 37 or the gradient descent algorithm. 38 In general, the optimization approaches should have sequential iterative algorithm for online estimation. Because the last estimation is optimized based on the best estimation in the previous time, and the initial estimation has a smooth and small solution space, the result of sequential iteration will be always close to the global optimum. It should be noticed that, if a new observation is inconsistent with the previous estimation, the optimization algorithm often needs extra time to correct the trajectory. Otherwise if we choose to believe the previous estimation, to what extent and on what time we should correct the trajectory? Moreover, sparse Cholesky factorization and Krylov subspace methods 29 are usually applied to accelerate the optimization process. 27 They get excellent performance in optimization with global correlations. Although some outstanding works 29 have reduced their complexity, the direct application in a task only with regional correlations (such as visual tracking) still brings a lot of unnecessary computation.
Both of the listed filtering approaches and optimization approaches have a common problem: they urgently depend on prior statistics. If the prior statistics are incorrect, the result of these algorithms may have extra error. Moreover, if the dynamic model is nonlinear, such as the PLS in this paper, this error may be magnified.
To overcome the problems above, the newly designed AdaTE should have following abilities: (a) be insensitive to prior statistics, (b) can find out the observation outliners and correct their statistics, (c) adaptively limits the updating length of trajectory based on the estimation fluctuation.
Dynamic model is the foundation of trajectory estimation. Before the introduction of AdaTE, we should describe the motion of object with a dynamic model at first.

Power limited steering model
In some dynamic models, the movements in orthogonal directions are assumed to be uncoupled with each other. 39 However, the assumption will weaken the ability of trajectory prediction in many cases like making a turn. We learn from the previous works and propose the power-limited steering model (PLS). It is a nonlinear model based on a natural combination of instantaneous power theory and instantaneous angular velocity theory from Newtonian mechanics. In general, PLS obeys two design philosophies: a) it should overcome the problem of infinite speed in prediction in simplest way; b) it should have fewer prior parameters to describe the typical states compared with Markov process models. Firstly, we will introduce the PLS in continuous time. Then we will derive the corresponding form in discrete time.
Power-limited steering model in continuous time Let x, v and a be the position, velocity and acceleration of the object. The movement of an object can be decomposed into the axial and transverse direction. According to Newtonian mechanics, if the object is driven by power P and receives resistance β and damping α, the axial acceleration is: where a A is the vector of axial acceleration, v is the vector of velocity, kvk is the norm of v, m is the mass of object and p ¼ P m is the power-mass ratio (specific power). It is easy to find that when the power is a constant positive number, the velocity will be asymptotically stabilized to Similar to (10), the acceleration in the transverse direction is given by which is orthogonal to v, where vector c T is the instantaneous angular velocity. The variation of p and c T should be smooth, so we set _ p ¼ u A and _ c T ¼ u T as the axial and transverse controls. As a maneuvering object, the controls have high randomness and low temporal correlation. So u A and u T are modeled as timeindependent and zero-mean random pulses obeying the normal distribution, Where δ t ðsÞ is the delta function, D A and D T are the power spectral density of u A and u T . In practical system, D A and D T can be estimated artificially based on the maneuverability of different maneuvering targets, which brings some estimation error inevitably.
Combining the axial and the transverse movement, the power-limited steering model in continuous time satisfies: The short-term corrected prediction of velocity Noticing (10) is a nonlinear stochastic function, where u A , u T will cause the perturbation of p and c T , which is hard to be solved directly. Based on the assumption that p and c T are constant in a short time, we transform the stochastic differential equation into ordinary differential equations for solving, and then compensate the effect of perturbation.
In differential equation (10), the axial and transverse velocity can be orthogonally separated into two groups: and where u A and u T obeys (8) and (9).
Firstly, we assume that p and c T are constant for a short time τ. Thus, we can ignore the perturbations of p and c T during a short time τ and solve the biased prediction of velocity b vðt þ τÞ from appendix A: where ς 1 and ς 2 are defined in (44), and γ is defined in (45).
However, it is hard to solve the b vðt þ τÞ from (13) directly. Therefore, we use Newton-Raphson method to estimate the arithmetic solution.
Then, after obtaining an estimate of b vðt þ τÞ, we need to add disturbance compensation to eliminate the effect of ignoring p and c T . Based on the strategy of renormalization group, the compensation of axial velocity during time ðt,t þ τÞ can be derived from appendix B, where Δυðt,t þ τÞ is the compensation of axial velocity, μðtÞ ¼ pðtÞ � υ 0 ðαυ 0 þ βÞ, υ 0 ¼ kvðtÞk.

PLS model in discrete time
To match the requirement of the optimization algorithm in next section, the state transition should be written in a linear function in discrete time where s ¼ � x T v T p c T T � T , s k and s kþ1 is the state vector at time k and k þ 1.ε k,kþ1 is the transition error, its integral form in continuous time is: � T is the control vector, u A and u T are defined in (8) and (9) respectively. To work out the Φ k,kþ1 and ε k,kþ1 from velocity evolution (15)- (16), the state transition in continuous time Φðt,t þ τÞ should be solved at first.
Form previous section, the evolution of velocity satisfies: where vðt þ τÞ is the predicted velocity from (15)- (16). From appendix C, after expanding the vðt þ τÞ into a linear function of vðtÞ, pðtÞ and c T ðtÞ, each submatrix of transition matrix Φðt,t þ τÞ ¼ can be obtained from (66)-(67), where I is the identity matrix With the transition matrix, we can analyze the transition error defined in (18).
Because the expectation of velocity perturbation has been compensated in (15), εðt,t þ τÞ will be a zero-mean random vector which satisfies εðt,t þ τÞ ∼ Zf0,Qðt,t þ τÞg. Thus, we only need to work out the transition covariance Qðt,t þ τÞ from (74) in appendix D.
It should be noted that although Φ k,kþ1 implicitly includes the state vector, and the calculation is approximate, the estimation algorithm based on PLS still converges. This is because the transition matrix Φ k,kþ1 is adaptively corrected by estimation algorithm in section "Adaptive trajectory estimation". It can be verified by the experiments in section "Experiments".

Adaptive trajectory estimation
As a maneuvering object, the prior statistics are often inaccurate. This will lead to the extra error of the algorithms which urgently relying on the prior statistics. While the dynamic model is nonlinear, the error may be magnified.
Comprehensively considering other problems, such as the observations drift, the adaptive trajectory estimation algorithm should have following abilities: (a) be insensitive to prior statistics, (b) can find out the outstanding observations and correct their statistics, (c) adjust the truncation time based on the estimation fluctuation. According to these principles, we design the AdaTE for online trajectory estimation. In general, it contains 4 steps: (a) updating the trajectory with sparse maximum a posterior estimation (sparse MAP); (b) updating the transition covariance of different times with a transition error; (c) updating the observation covariance based on analysis of the observation error; (d) update the truncation time based on the difference between previous and latest trajectory.

Updating the trajectory with sparse map
Setting ξ T be the truncation time, S k ¼ fs 1 ,/,s k g be the trajectory of states at time k, Z k ¼ fZ 1 ,/,Z k g be the set of observations from time 1 to time k, the updated trajectory under optimization framework satisfies where S ðξ T : kÞ k 4S k is the part of trajectory during time ½ξ T ,k�. Assuming the observers are independent of each other, the probability of trajectory is Typically, the observable is the position of object. It is linear with the state: where H is the observation matrix, δz is the observation error temporary assumed to obey normal distribution Zf0,Rg, where R has been updated in previous estimation. Assuming the state transition satisfies the PLS model (17), where transition matrix Φ i,iþ1 ,i ¼ 1; 2,/,k � 1 is in the form of (20) and calculated on the previous trajectory S k�1 . And each conditional probability in (22) satisfies: 8 > > < > > : log Maximizing probability (24), the optimal trajectory satisfies the sparse linear equations: where M ðξ T : kÞ k ¼ fM i,j ,i,j ¼ ξ T ,/,kg is a sparse matrix, whose nonzero elements satisfies: B ðξ T : kÞ k ¼ fb i ,i ¼ ξ T ,/,kg is the set of vectors, the elements are (28).
Noticing that the set M ðξ T : kÞ k is a band-diagonal symmetric matrix, equation (25) can be rapidly solved.

Adaptation of transition covariance
For a specific maneuvering target, we can estimate the appropriate prior statistics artificially, but this prior estimate is inaccurate. This will lead to the extra error of the algorithms which urgently relying on the prior statistics. While the PLS is a nonlinear model, the error may be magnified. To be insensitive to the prior statistics, the transition covariance can be updated by a fading memory strategy, where Q i,iþ1 is the transition covariance, respectively, and t m is an empirical hyper-parameter of the memory time, which is fixed during the process. ε ðkþ1Þ i,iþ1 is the transition error defined as: ε Where s are the MAP of trajectory at time k þ 1.
Through updating the transition covariance, the overestimated part of the trajectory in maneuvering can be revised. It improves the robustness to the biased prior statistics.

Adaptation of observation covariance
In most of the observation models, the prior statistics indicate that the noise of observation is zero-mean while it has drift in fact. This will lead to the systematic deviation of estimation. To solve the problem, we use an adaptation strategy to correct the covariance of abnormal observations based on the difference between the real observation and the estimate of trajectory.
First, we define the observation bias: And the corresponding deviation: Where z ðjÞ i is the real observation,Hs i is the estimated observation from trajectory.
Then, the observation whose deviation is greater than 3 times of their RMSE will be considered as abnormal observation, and its covariance will be corrected with Note that under the optimization framework with multiple sensors, not only the covariance of significantly deviated observations but also the covariance of drift observations are corrected in (33). Compared with classical filtering methods, it effectively overcomes the observation drift.

Truncation time
While the trajectory only has regional correlations, the correlation between the observation and the state is inversed to the time interval. This means the latest observations do not relate to the early states. In another word, some earlier states do not need to be updated. Based on this phenomenon, we defined a truncation time to limit the range of trajectory updating. It avoids the linear improvement of calculation in trajectory estimation.
The truncation index is defined as a weighted norm of difference vector between the same states in neighboring time: where s The truncation time is defined as the last moment when all of previous truncation index is smaller than the threshold.
where η is the threshold of truncation index. If ξ T > 0, the part of trajectory before ξ T will be frozen, and only the part after that will be updated. To reserve sufficient observations for the trajectory estimation, the truncation time is bounded by ξ T ≤ k � 10.

Adaptive trajectory estimation algorithm
Summarizing the above steps, the procedures of AdaTE is achieved in Appendix F.
To analyze the real-time performance of AdaTE, we compare the time consumptions of AdaTE algorithm with a naive sparse MAP algorithm 8 in a dataset containing 600 observations. Figure 2 shows the time consumption of two algorithms in trajectory estimation. The result is obvious: compared with the sparse MAP, the AdaTE successfully limits the time consumption.
In this paper, the symbols used are standard, which is shown in Table 1.
Then we will verify the convergence of AdaTE through the experiments in section "Experiments".

Experiments
To evaluation the convergence and performance of AdaTE, we conducted three experiments: (a) typical trajectory estimation on 3D observations, (b) local navigation in random obstacle environment, (c) trajectory optimization for visual tracking. The experiments were performed on a laptop with a 2.5 GHz Intel Core i5 CPU.

Typical trajectory estimation on 3d observations
To verify the convergence and precision of AdaTE, we test it on 3 standard 3D trajectories with different characteristics, where the first trajectory is a cruising route with minimal maneuvers; the second one is a swaying route with frequently varying speed; the third one is a snake-like route with multiple maneuvers and lost observations (from time 250 to 300). For each standard trajectory, there are two kinds of observations: one produced with Gaussian noise and the other with additional partial drift. In all 6 groups of experiments, all tested algorithms share the same and biased prior statistics.
The 3 standard 3D trajectories are pre-designed manually, and for each trajectory, there are several types of common geometric trajectories. For example, the ground truth of cruising route (shown in Figure 3) consists of 2 three quarter circular arcs and 3 straight lines, in which the movement on the middle line is in an accelerated state.
We  Table 2 The observations and the corresponding results of AdaTE are shown in Figures 3, 4

Local navigation in random obstacle environment
The topological path planning is commonly applied in large scale navigation. 40,41 After getting the node orders, the best path between any two nodes should be rapidly found out for local navigation. In an environment with obstacles, for the safety and flexibility, the navigation should be online adjusted according to real-time situation. For the stability, the object ought to have fewer maneuvers while avoiding obstacles. This is a suitable task for AdaTE.
In this task, the object is assigned to avoid the random obstacles and to online update the smooth trajectory to the next two nodes, thus object can safely and punctually arrive at the planned nodes with minimal maneuverability. Figure 6 Setting P be the set of key point nodes, V be the set of obstacles, and t k be the time of arrival at upcoming node, the MAP of the plan can be described as S and The power spectral density of u A D T The power spectral density of u T Q k�1,k The transition covariance The observation covariance t m The memory time of fading memory strategy ξ T The truncation time k � k Norm of a vector or a matrix, which is the L2 norm in this article ½v� × the skew symmetric matrix of v Zfx,σ 2 g Normal distribution with mean x and variance σ 2 OðnÞ The metric of computational complexity  The error used to evaluate the algorithms is the mean normalized RMSE. The RMSE of observation is given as the absolute value while RMSE of each algorithm is the normalized value by that of observation. The bold number in each group is the smallest mean normalized RMSE, and the "Div" denotes that the normalized RMSE is invalid because of the divergence. AdaTE-PLS obtains the minimum error in all groups.
where PðPjS ðk : t k Þ k Þ is the probability of arriving at planed nodes, P a ðVjS ðk : t k Þ k Þ is the probability of successful obstacles avoidance during the time ðk,t k Þ, and P h ðVjS ðiÞ k Þ is the unblocked probability at moment i.
Noticing that the likelihood defined in (37) is an extended form of (22) with an additional unblocked item, the corresponding real-time navigation could be solved by extended AdaTE. The overview of 10 representative moments during online navigation is shown in Figure 7. We can observe that when the object arriving at next node, a new node in plan is taken into consideration and the original path planning is slightly adjusted (compare the pink curve at time 10 with that at time 11). Another detail shows that the path planning at time 34 is adjusted compared with the planning at time 31. The reason is that the original path fails to avoid the obstacle; therefore, at time 34 it was fixed to avoid from the bottom of obstacle (for details, please turn to Figure 8(b)). The evasive maneuvers extend the path and increasing the planning speed, leading to a larger turning radius. A similar situation can be found between time 71 and 73.
The overview of the 3D path cannot display the details of the obstacle avoidance, especially when the object passes obstacle from above or below, the object appearing to be intersecting them. To prevent from misunderstanding, several details of the obstacle avoidance are presented in Figure 8. In general, AdaTE uses minimum maneuverability under the  constraint of the dynamic model to avoid the obstacles while successfully arriving at the planed nodes. In this experiment, the navigation converges in 3 loops of iterations for a single nearby obstacle. And it needs no more than 4 iterations for two obstacles.
In environment with sparsely distributed obstacles, AdaTE has an excellent real time performance by an O(n) computational complexity, where n is the number of nearby obstacles. As a comparison, some powerful obstacle avoidance approaches such as Cooperative Collision Avoidance (CCA) 42 has obvious higher complexity O(2n 2 ). The reason comes from their motivations: AdaTE is an online iterative algorithm, thus the result is adjusted based on   previous plan, while CCA is an offline algorithm without online iteration To test the application of AdaTE in visual tracking, we choose 6 scenes in VOT 2013 with illumination variations (CarDark and Singer1), scale variations (Singer1) and partial blocking (Coke and Tiger1) for the experiment. The fDSST 43 is applied as the basic tracking algorithm, and the mean overlap is chosen as the evaluation index. Parts of the screenshots are presented in Figure 9. It is obvious that in most scenes, the AdaTE achieves a slight improvement in accuracy compared with basic fDSST.
The mean overlaps of fDSST and fDSST-AdaTE is shown in Table 3. In most scenes, fDSST-AdaTE is superior to the   basic fDSST with slight advantage. We find that as a modelbased algorithm, AdaTE is robust to the shaky observations compared with the original tracking algorithm. Compared with applying to a smooth tracking algorithm like DSST, AdaTE is likely to achieve more obvious improvement for a wobbly tracking algorithm such as SAMF.

Conclusion
This paper investigates the online trajectory estimation of non-cooperative or maneuvering under the condition of regional correlations, biased prior statistics and observations drift. And the works focus on two aspects: the dynamic model and robust estimation algorithm.
To estimate the trajectory without the dedicated dynamicpower-limited steering model, which is a natural combination of instantaneous power theory and instantaneous angular velocity theory from Newtonian mechanics. The contribution has two aspects: a) overcoming the infinite speed problem and convergent in prediction; B) sustaining few parameters to describe the switch of typical states.
To achieve robust and efficient trajectory estimation in challenging situations such as the biased prior statistics and the observations drift, we propose the adaptive trajectory estimation algorithm. The contribution of AdaTE is threefold a) the adaptation of transition covariance matrices improves the estimation robustness to the model error and prior statistics; b) the correction of observation covariance matrices improves the estimation robustness to the observation drift; c) the truncation time ensures the calculation is restricted to the necessary part, which avoids the linear improvement of calculation.
To evaluate the performance of AdaTE-PLS in different tasks, we conduct three experiments: a) typical trajectory estimation on 3D observations, b) local navigation over node planning, c) trajectory optimization for visual tracking. By testing on three standard 3D routes with different characteristics, the convergence and precision of AdaTE is verified. Experiment b) and c) demonstrate that with slight modification, AdaTE can be widely applied in tasks such as local navigation under random obstacls, and trajectory optimization in visual tracking.
In the future, we will explore better dynamic model which is simpler, less calculation and more precise for maneuvering object. For example, this article ignores the impact of unknown measurement delays on the system in practical application. The follow-up work can improve the adaptive strategy of observation covariance by referring to the method of observer-based control 44 to solve this problem and further improve the accuracy of the model. Furthermore, we hope to extend the AdaTE algorithm to a wider probability distribution, such as the exponential family, to develop a more significant and general regional optimization method.

Declaration of Conflicting Interests
The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding
The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work where μðtÞ ¼ pðtÞ � υ 0 ðαυ 0 þ βÞ. From (12), we know the angular velocity d dt v kvk is linear with v kvk and ½c T � × , so the u T will not act on the expectation of orientation, orientation doesn't need any compensation.

D. The approximate calculation of transition matrix
To match the requirement of the optimization algorithm in next section, the state transition function should be written in a linear form: where s ¼ � x T v T p c T T � T , s k and s kþ1 is the state vector at time k and k þ 1, ε k,kþ1 is the transition error.

E. The approximate calculation of transition covariance
Because the expectation of velocity perturbation have been compensated in (19), εðt,t þ τÞ will be a zero-mean random vector. Thus εðt,t þ τÞ ∼ Zf0,Qðt,t þ τÞg, where D u is the power spectral density of u, which satisfies, Substituting (59) into (64), the transition covariance will be,