Adaptive trajectory estimation with power limited steering model under perturbation compensation

Abstract

Trajectory estimation of maneuvering objects is applied in numerous tasks like navigation, path planning and visual tracking. Many previous works get impressive results in the strictly controlled condition with accurate prior statistics and dedicated dynamic model for certain object. But in challenging conditions without dedicated dynamic model and precise prior statistics, the performance of these methods significantly declines. To solve the problem, a stochastic nonlinear model called the power-limited steering model is proposed to describe the motion of non-cooperative object. It is a natural combination of instantaneous power and instantaneous angular velocity, relying on the nonlinearity to achieve the change of states. And the renormalization group is introduced to compensate the nonlinear effect of perturbation in our model. For robust and efficient trajectory estimation, an adaptive trajectory estimation (AdaTE) algorithm is proposed. By updating the statistics and truncation time online, it corrects the estimation error caused by biased prior statistics and observation drift, while reducing the computational complexity lower than O(n). The experiment of trajectory estimation demonstrates the convergence of AdaTE, and the better robust to the biased prior statistics and the observation drift compared with several typical estimation algorithms. Other experiments demonstrate through slight modification, our method can also be applied to local navigation in random obstacle environment, and 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, ¹ navigation, ² visual tracking ³ and simultaneous localization and mapping (SLAM). ^4–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 ¹³ : wiener-process acceleration model, ² Markov process models such as Singer model, ¹⁴ and semi-Markov jump process models. ¹⁵ 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 ¹⁶ 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) ¹⁷ and Kalman smoother ¹⁰ 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) ²⁰ maximum correntropy Kalman filter (MCKF), ²¹ and the Robust Kalman Filtering (RKF). ²² 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–26 Through the resampling strategy, ²⁵ 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 ²⁷ and Krylov subspace methods ²⁹ 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), ⁸ 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. ³⁰

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, ⁸ 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.

Figure 1.

The process of adaptive trajectory estimation. The first step is updating the trajectory based on the observations and previous estimation with sparse maximum a posteriori estimation (sparse MAP). The second step is updating the transition covariance of different times with a transition error. The third step is updating the observation covariance based on analysis of the observation error. Finally, the truncation time which confines the range to be updated will be worked out based on the difference between previous and updated trajectory.

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, ³¹ where acceleration is assumed to obey the Wiener process. Many following constant acceleration (CA) models are inspired by this work.

Lets $\mathbf{x},\hspace{0.17em}\mathbf{v}$ and $\mathbf{a}$ be the position, velocity and acceleration of the object, Setting the state vector as $\mathbf{s}\left(t\right)={\left[\mathbf{x}\left(t\right),\mathbf{v}\left(t\right),\mathbf{a}\left(t\right)\right]}^T$ .

The Wiener-process acceleration model is presented as

$$\dot{\mathrm{s}}\left(t\right)=\mathbf{As}\left(t\right)+\mathbf{u}\left(t\right)$$

(1)

where $\mathbf{u}$ is the control vector and it is an independent process (white noise) of acceleration with power spectral density $\mathbf{D}$ . $\mathbf{A}$ is the state transition matrix,

$$\mathbf{A}=\left[\begin{array}{ccc}\mathbf{0}& \mathbf{1}& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{1}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}\end{array}\right]$$

(2)

Based on the antitype of Wiener-process, many following works have been proposed, such as the polynomial models, the Singer model, ¹⁴ “current” model and semi-Markov jump process model. ¹⁵ Their improvement mainly focuses on the distribution of random vector, such as quantized levels of expectations,

$$\mathbf{u}\left(t\right)={\displaystyle \sum _{i=1}^n{\overline{\mathbf{a}}}_{i}P\left\{\mathbf{u}\left(t\right)={\overline{\mathbf{a}}}_i|\mathbf{z}\left(s\right),s<t\right\}}+\tilde{\mathbf{a}}$$

(3)

where ${\overline{\mathrm{a}}}_i$ is the quantized expectation of acceleration, $\tilde{\mathrm{a}}$ is the zero-mean random vector, and $\mathbf{z}\left(s\right),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 ${\overline{\mathrm{a}}}_i,i=\mathrm{1,2},\cdots ,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) ²⁰ and the Maximum Correntropy Kalman filter (MCKF). ²¹ For the real-time state estimation, they execute a forward propagation:

$$\begin{array}{l}\text{E}\left[{\mathbf{s}}_k|{\mathrm{Z}}_k\right]=\\ {\displaystyle \int \left[{\displaystyle \int \frac{P\left({\mathbf{Z}}_k|{\mathbf{s}}_k\right)P\left({\mathbf{s}}_k|{\mathbf{s}}_{k-1},{\mathrm{Z}}_{k-1}\right)}{P\left({\mathbf{Z}}_k\right)}d{\mathbf{s}}_{k-1}}\right]{\mathbf{s}}_{k}d{\mathbf{s}}_k}\end{array}$$

(4)

where ${\mathbf{s}}_i,i\in \left\{1,\cdots ,k\right\}$ is the state at time $i$ , ${\mathsf{Z}}_k$ is the observations at time $k$ which might contain several independent observations, and ${\mathsf{Z}}_k=\left\{{\mathbf{Z}}_1,\cdots ,{\mathbf{Z}}_k\right\}$ is 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:

$$\begin{array}{l}\text{E}\left[{\mathbf{s}}_i|{\mathrm{Z}}_k\right]={\displaystyle \int \left[{\displaystyle \int P\left({\mathbf{s}}_{i+1}|{\mathbf{s}}_i,{\mathrm{Z}}_k\right)P\left({\mathbf{s}}_i|{\mathrm{Z}}_{k-1}\right)}d{\mathbf{s}}_{i+1}\right]{\mathbf{s}}_{i}d{\mathbf{s}}_i},\\ i=k-1,\cdots ,1\end{array}$$

(5)

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–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 ³⁷ or the gradient descent algorithm. ³⁸

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 ²⁹ are usually applied to accelerate the optimization process. ²⁷ They get excellent performance in optimization with global correlations. Although some outstanding works ²⁹ 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. ³⁹ 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 $\mathbf{x}\text{,}\hspace{0.17em}\mathbf{v}\hspace{0.17em}\text{and }\mathbf{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 $\beta$ and damping $\alpha$ , the axial acceleration is:

$${\mathbf{a}}_A=-\alpha \mathbf{v}-\beta \frac{\mathbf{v}}{\Vert \mathbf{v}\Vert }+p\frac{\mathbf{v}}{{\Vert \mathbf{v}\Vert }^2}$$

(6)

where ${\mathbf{a}}_A$ is the vector of axial acceleration, $\mathbf{v}$ is the vector of velocity, $\Vert \mathbf{v}\Vert$ is the norm of $\mathbf{v}$ , $m$ is the mass of object and $p=\frac{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 $\frac{-\beta +\sqrt{{\beta }^2+4\alpha p}}{2\alpha }$ .

Similar to (10), the acceleration in the transverse direction is given by

$${\mathbf{a}}_T={\mathbf{c}}_T\times \mathbf{v}={\left[{\mathbf{c}}_T\right]}_{\times }\mathbf{v}$$

(7)

which is orthogonal to $\mathbf{v}$ , where vector ${\mathbf{c}}_T$ is the instantaneous angular velocity.

The variation of $p$ and ${\mathbf{c}}_T$ should be smooth, so we set $\dot{\mathrm{p}}=u_A$ and ${\dot{\mathbf{c}}}_T={\mathbf{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 ${\mathbf{u}}_T$ are modeled as time-independent and zero-mean random pulses obeying the normal distribution,

$$u_A\left(t\right)\sim \mathsf{Z}\left\{0,{\delta }_t\left(s\right)D_A\right\}$$

(8)

$${\mathbf{u}}_T\left(t\right)\sim \mathsf{Z}\left\{\mathbf{0},{\delta }_t\left(s\right){\mathbf{D}}_T\right\}$$

(9)

Where ${\delta }_t\left(s\right)$ is the delta function, $D_A$ and ${\mathbf{D}}_T$ are the power spectral density of $u_A$ and ${\mathbf{u}}_T$ . In practical system, $D_A$ and ${\mathbf{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:

$$\{\begin{array}{l}\dot{\mathbf{x}}=\mathbf{v}\\ \dot{\mathbf{v}}=-\alpha \mathbf{v}-\beta \frac{\mathbf{v}}{\Vert \mathbf{v}\Vert }+p\frac{\mathbf{v}}{{\Vert \mathbf{v}\Vert }^2}-{\left[\mathbf{v}\right]}_{\times }{\mathbf{c}}_T\\ \dot{\mathrm{p}}=u_A\\ {\dot{\mathbf{c}}}_T={\mathbf{u}}_T\end{array}$$

(10)

The short-term corrected prediction of velocity

Noticing (10) is a nonlinear stochastic function, where $u_A$ , ${\mathbf{u}}_T$ will cause the perturbation of $p$ and ${\mathbf{c}}_T$ , which is hard to be solved directly. Based on the assumption that $p$ and ${\mathbf{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:

$$\{\begin{array}{l}\Vert \mathbf{v}\Vert \frac{d\Vert \mathbf{v}\Vert }{dt}=p-\alpha {\Vert \mathbf{v}\Vert }^2-\beta \Vert \mathbf{v}\Vert \\ \dot{\mathrm{p}}=u_A\end{array}$$

(11)

and

$$\{\begin{array}{l}\frac{d}{dt}\frac{\mathbf{v}}{\Vert \mathbf{v}\Vert }={\left[{\mathbf{c}}_T\right]}_{\times }\frac{\mathbf{v}}{\Vert \mathbf{v}\Vert }\\ {\dot{\mathbf{c}}}_T={\mathbf{u}}_T\end{array}$$

(12)

where $u_A$ and ${\mathbf{u}}_T$ obeys (8) and (9).

Firstly, we assume that $p$ and ${\mathbf{c}}_T$ are constant for a short time $\tau$ . Thus, we can ignore the perturbations of $p$ and ${\mathbf{c}}_T$ during a short time $\tau$ and solve the biased prediction of velocity $\widehat{\mathbf{v}}\left(t+\tau \right)$ from appendix A:

$$\{\begin{array}{l}{\left(\frac{\Vert \widehat{\mathbf{v}}\left(t+\tau \right)\Vert +{\varsigma }_1}{\Vert \mathbf{v}\left(t\right)\Vert +{\varsigma }_1}\right)}^{{\varsigma }_1}{\left|\frac{\Vert \widehat{\mathbf{v}}\left(t+\tau \right)\Vert +{\varsigma }_2}{\Vert \mathbf{v}\left(t\right)\Vert +{\varsigma }_2}\right|}^{-{\varsigma }_2}=e^{-\gamma \tau }\\ \frac{\widehat{\mathbf{v}}\left(t+\tau \right)}{\Vert \widehat{\mathbf{v}}\left(t+\tau \right)\Vert }=\exp \left(\tau {\left[{\mathbf{c}}_{\text{T}}\left(t\right)\right]}_{\times }\right)\frac{\mathbf{v}\left(t\right)}{\Vert \mathbf{v}\left(t\right)\Vert }\end{array}$$

(13)

where ${\varsigma }_1$ and ${\varsigma }_2$ are defined in (44), and $\gamma$ is defined in (45).

However, it is hard to solve the $\widehat{\mathbf{v}}\left(t+\tau \right)$ from (13) directly. Therefore, we use Newton-Raphson method to estimate the arithmetic solution.

Then, after obtaining an estimate of $\widehat{\mathbf{v}}\left(t+\tau \right)$ , we need to add disturbance compensation to eliminate the effect of ignoring $p$ and ${\mathbf{c}}_T$ . Based on the strategy of renormalization group, the compensation of axial velocity during time $\left(t,t+\tau \right)$ can be derived from appendix B,

$$\begin{array}{l}\mathrm{\Delta }\upsilon \left(t,t+\tau \right)=\\ -\frac{{\upsilon }_0{D}_A}{2\mu \left(t\right)}\left[\tau -\frac{2{\upsilon }_0^2}{\mu \left(t\right)}\log \frac{{\upsilon }_0^2+\tau \mu \left(t\right)}{{\upsilon }_0^2}+\frac{{\upsilon }_0^2\tau }{{\upsilon }_0^2+\tau \mu \left(t\right)}\right]\end{array}$$

(14)

where $\mathrm{\Delta }\upsilon \left(t,t+\tau \right)$ is the compensation of axial velocity, $\mu \left(t\right)=p\left(t\right)-{\upsilon }_0\left(\alpha {\upsilon }_0+\beta \right)$ , ${\upsilon }_0=\Vert \mathbf{v}\left(t\right)\Vert$ .

Combining (13) and (14), we can get the corrected prediction of axial velocity,

$$\Vert \overline{\mathrm{v}}\left(t+\tau \right)\Vert =\Vert \widehat{\mathrm{v}}\left(t+\tau \right)\Vert +\mathrm{\Delta }\upsilon \left(t,t+\tau \right)$$

(15)

and the direction of velocity,

$$\frac{\overline{\mathrm{v}}\left(t+\tau \right)}{\Vert \overline{\mathrm{v}}\left(t+\tau \right)\Vert }=\exp \left(\tau {\left[{\mathbf{c}}_{\text{T}}\left(t\right)\right]}_{\times }\right)\frac{\mathbf{v}\left(t\right)}{\Vert \mathbf{v}\left(t\right)\Vert }$$

(16)

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

$${\mathbf{s}}_{k+1}={\mathbf{\Phi }}_{k,k+1}{\mathbf{s}}_k+{\mathbf{\epsilon }}_{k,k+1}$$

(17)

where $\mathbf{s}={\left[\begin{array}{cccc}{\mathbf{x}}^{\text{T}}& {\mathbf{v}}^{\text{T}}& p& {\mathbf{c}}_T^{\text{T}}\end{array}\right]}^{\text{T}}$ , ${\mathbf{s}}_k$ and ${\mathbf{s}}_{k+1}$ is the state vector at time $k$ and $k+1$ . ${\mathbf{\epsilon }}_{k,k+1}$ is the transition error, its integral form in continuous time is:

$$\mathbf{\epsilon }\left(t,t+\tau \right)={\displaystyle \underset{0}{\overset{\tau }{\int }}\mathbf{\Phi }\left(t+s,t+\tau \right)\mathbf{u}\left(t+s\right)ds}$$

(18)

where $\mathbf{u}={\left[\begin{array}{cccc}\mathbf{0}& \mathbf{0}& u_A^{\text{T}}& {\mathbf{u}}_T^{\text{T}}\end{array}\right]}^{\text{T}}$ is the control vector, $u_A$ and ${\mathbf{u}}_T$ are defined in (8) and (9) respectively.

To work out the ${\mathbf{\Phi }}_{k,k+1}$ and ${\mathbf{\epsilon }}_{k,k+1}$ from velocity evolution (15)–(16), the state transition in continuous time $\mathbf{\Phi }\left(t,t+\tau \right)$ should be solved at first.

Form previous section, the evolution of velocity satisfies:

$$\mathbf{v}\left(t+\tau \right)=\overline{\mathbf{v}}\left(t+\tau \right)+\delta \mathbf{v}\left(t+\tau \right)$$

(19)

where $\overline{\mathrm{v}}\left(t+\tau \right)$ is the predicted velocity from (15)-(16).

From appendix C, after expanding the $\mathbf{v}\left(t+\tau \right)$ into a linear function of $\mathbf{v}\left(t\right)$ , $p\left(t\right)$ and ${\mathbf{c}}_T\left(t\right)$ , each submatrix of transition matrix

$$\mathbf{\Phi }\left(t,t+\tau \right)=\left[\begin{array}{cccc}\mathbf{I}& {\mathbf{\Phi }}_{12}\left(t,t+\tau \right)& {\mathbf{\Phi }}_{13}\left(t,t+\tau \right)& {\mathbf{\Phi }}_{14}\left(t,t+\tau \right)\\ \mathbf{0}& {\mathbf{\Phi }}_{22}\left(t,t+\tau \right)& {\mathbf{\Phi }}_{23}\left(t,t+\tau \right)& {\mathbf{\Phi }}_{24}\left(t,t+\tau \right)\\ \mathbf{0}& \mathbf{0}& 1& \mathbf{0}\\ \mathbf{0}& \mathbf{0}& \mathbf{0}& \mathbf{I}\end{array}\right]$$

(20)

can be obtained from (66)-(67), where $\mathbf{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), $\mathbf{\epsilon }\left(t,t+\tau \right)$ will be a zero-mean random vector which satisfies $\mathbf{\epsilon }\left(t,t+\tau \right)\sim \mathsf{Z}\left\{\mathbf{0},\mathbf{Q}\left(t,t+\tau \right)\right\}$ . Thus, we only need to work out the transition covariance $\mathbf{Q}\left(t,t+\tau \right)$ from (74) in appendix D.

Let ${\mathbf{\Phi }}_{k,k+1}=\mathbf{\Phi }\left(t_k,t_{k+1}\right)$ , and ${\mathbf{Q}}_{k,k+1}=\mathbf{Q}(t_k,t_{k+1})$ , the state transition in discrete time described in (17) can be obtained.

It should be noted that although ${\mathbf{\Phi }}_{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 ${\mathbf{\Phi }}_{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 ${\xi }_T$ be the truncation time, ${\mathsf{S}}_k=\left\{{\mathbf{s}}_1,\cdots ,{\mathbf{s}}_k\right\}$ be the trajectory of states at time $k$ , ${\mathsf{Z}}_k=\left\{{\mathbf{Z}}_1,\cdots ,{\mathbf{Z}}_k\right\}$ be the set of observations from time 1 to time $k$ , the updated trajectory under optimization framework satisfies

$${\mathsf{S}}_k^{\left({\xi }_T:k\right)}=\arg \max P\left({\mathsf{S}}_k^{\left({\xi }_T:k\right)}|{\mathsf{S}}_{k-1},{\mathsf{Z}}_k\right)$$

(21)

where ${\mathsf{S}}_k^{({\xi }_T:k)}\subseteq {\mathsf{S}}_k$ is the part of trajectory during time $[{\xi }_T,k]$ .

Assuming the observers are independent of each other, the probability of trajectory is

$$P\left({\mathsf{S}}_k|{\mathsf{Z}}_k\right)=P\left({\mathbf{s}}_1\right){\displaystyle \prod _{j=1}^n\frac{P\left({\mathbf{z}}_1^{\left(j\right)}|{\mathbf{s}}_1\right)}{P\left({\mathbf{z}}_1^{\left(j\right)}\right)}}\left\{{\displaystyle \prod _{i=2}^{k}P\left({\mathbf{s}}_i|{\mathbf{s}}_{i-1}\right)}{\displaystyle \prod _{j=1}^n\frac{P\left({\mathbf{z}}_1^{\left(j\right)}|{\mathbf{s}}_1\right)}{P\left({\mathbf{z}}_1^{\left(j\right)}\right)}}\right\}$$

(22)

Typically, the observable is the position of object. It is linear with the state:

$$\mathbf{z}=\mathbf{Hs}+\delta \mathbf{z}$$

(23)

where $\mathbf{H}$ is the observation matrix, $\delta \mathbf{z}$ is the observation error temporary assumed to obey normal distribution $\mathsf{Z}\left\{\mathbf{0},\mathbf{R}\right\}$ , where $\mathbf{R}$ has been updated in previous estimation.

Assuming the state transition satisfies the PLS model (17), where transition matrix ${\mathbf{\Phi }}_{i,i+1},i=\mathrm{1,2},\cdots ,k-1$ is in the form of (20) and calculated on the previous trajectory $S_{k-1}$ . And each conditional probability in (22) satisfies:

$$\{\begin{array}{l}\log P\left({\mathbf{s}}_i|{\mathbf{s}}_{i-1}\right)=-\frac{1}{2}{\Vert {\mathbf{\Phi }}_{i-1,i}{\mathbf{s}}_{i-1}-{\mathbf{s}}_i\Vert }_{{\mathbf{Q}}_{i-1,i}^{-1}}^2\\ \log P\left({\mathbf{z}}_i^{\left(j\right)}|{\mathbf{s}}_i\right)=-\frac{1}{2}{\Vert \mathbf{H}{\mathbf{s}}_i-{\mathbf{z}}_i^{\left(j\right)}\Vert }_{{\mathbf{R}}_{i,j}^{-1}}^2\end{array}$$

(24)

Maximizing probability (24), the optimal trajectory satisfies the sparse linear equations:

$${\mathsf{M}}_k^{\left({\xi }_T:k\right)}{\mathsf{S}}_k^{\left({\xi }_T:k\right)}={\mathsf{B}}_k^{\left({\xi }_T:k\right)}$$

(25)

where ${\mathrm{M}}_k^{\left({\xi }_T:k\right)}=\left\{{\mathbf{M}}_{i,j},i,j={\xi }_T,\cdots ,k\right\}$ is a sparse matrix, whose nonzero elements satisfies:

$${\mathbf{M}}_{i,i}=\{\begin{array}{l}{\mathbf{H}}^{\text{T}}{\displaystyle \sum _{j=1}^n{\mathbf{R}}_{i,j}^{-1}}\mathbf{H}+{\mathbf{\Phi }}_{i,i+1}^{\text{T}}{\mathbf{Q}}_{i,i+1}^{-1}{\mathbf{\Phi }}_{i,i+1}+{\mathbf{Q}}_{i-1,i}^{-1},i={\xi }_T,\cdots ,k-1\\ {\mathbf{H}}^{\text{T}}{\displaystyle \sum _{j=1}^n{\mathbf{R}}_{k,j}^{-1}}\mathbf{H}+{\mathbf{Q}}_{k-1,k}^{-1},\hspace{1em}i=k\end{array}$$

(26)

$${\mathbf{M}}_{i,i-1}={\mathbf{M}}_{i-1,i}^{\text{T}}={\mathbf{Q}}_{i-1,i}^{-1}{\mathbf{\Phi }}_{i-1,i},\hspace{1em}i={\xi }_T+1,\cdots ,k$$

(27)

${\mathrm{B}}_k^{\left({\xi }_T:k\right)}=\left\{{\mathbf{b}}_i,i={\xi }_T,\cdots ,k\right\}$ is the set of vectors, the elements are (28).

$${\mathbf{b}}_i=\{\begin{array}{l}{\mathbf{H}}^{\text{T}}{\displaystyle \sum _{j=1}^n{\mathbf{R}}_{1,j}^{-1}{\mathbf{z}}_1^{\left(j\right)}}+{\mathbf{Q}}_{i-1,i}^{-1}{\mathbf{\Phi }}_{i-1,i}{\mathbf{x}}_{i-1},i={\xi }_T\\ {\mathbf{H}}^{\text{T}}{\displaystyle \sum _{j=1}^m{\mathbf{R}}_{i,j}^{-1}{\mathbf{z}}_i^{\left(j\right)}},i={\xi }_T+1,\cdots ,k\end{array}$$

(28)

Noticing that the set ${\mathsf{M}}_k^{\left({\xi }_T:k\right)}$ 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,

$${\mathbf{Q}}_{i,i+1}:=\frac{t_m{\mathbf{Q}}_{i,i+1}+\tau {\mathbf{\epsilon }}_{i,i+1}^{\left(k+1\right)}{\left[{\mathbf{\epsilon }}_{i,i+1}^{\left(k+1\right)}\right]}^{\text{T}}}{t_m+\tau },i=\mathrm{1,2},\cdots ,k-1$$

(29)

where ${\mathbf{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. ${\mathbf{\epsilon }}_{i,i+1}^{\left(k+1\right)}$ is the transition error defined as:

$${\mathbf{\epsilon }}_{i,i+1}^{\left(k+1\right)}={\mathbf{s}}_{i+1}^{\left(k+1\right)}-{\mathbf{\Phi }}_{k,k+1}{\mathbf{s}}_i^{\left(k+1\right)},,i=\mathrm{1,2},\cdots ,k-1$$

(30)

Where ${\mathbf{s}}_i^{\left(k+1\right)}$ and ${\mathbf{s}}_{i+1}^{\left(k+1\right)}$ 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:

$$\delta {\mathbf{z}}_i^{\left(j\right)}={\mathbf{z}}_i^{\left(j\right)}-\mathbf{H}{\mathbf{s}}_i$$

(31)

And the corresponding deviation:

$$e_z\left({\mathbf{z}}_i^{\left(j\right)}\right)={\Vert \delta {\mathbf{z}}_i^{\left(j\right)}\Vert }_{{\mathbf{R}}_{i,j}^{-1}}$$

(32)

Where ${\mathbf{z}}_i^{\left(j\right)}$ is the real observation, $\mathbf{H}{\mathbf{s}}_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

$${\mathbf{R}}_{i,j}=\frac{t_m{\mathbf{R}}_{i,j}+\tau \delta {\mathbf{z}}_i^{\left(j\right)}\delta {\mathbf{z}}_i^{\left(j\right)\text{T}}}{t_m+\tau }$$

(33)

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:

$${\theta }_i^{\left(k\right)}={\Vert {\mathbf{s}}_i^{\left(k\right)}-{\mathbf{s}}_i^{\left(k-1\right)}\Vert }_{\mathbf{\psi }},i=\mathrm{2,3},\cdots ,k$$

(34)

where ${\mathbf{s}}_i^{\left(k\right)}$ and ${\mathbf{s}}_i^{\left(k-1\right)}$ are the i-th state of the trajectory estimated at time $k$ and time $k-1$ , and $\mathbf{\psi }$ is a diagonal matrix quantize the weight of each dimension of $\mathbf{s}$ .

The truncation time is defined as the last moment when all of previous truncation index is smaller than the threshold.

$${\xi }_T=\max \left\{t|\forall i\le t,{\theta }_i^{\left(k\right)}<\eta \right\}$$

(35)

where $\eta$ is the threshold of truncation index.

If ${\xi }_T>0$ , the part of trajectory before ${\xi }_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 ${\xi }_T\le 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 ⁸ 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.

Figure 2.

The efficiency of AdaTE and naive sparse MAP. The time consumption of sparse MAP linearly increases with the number of observations, while the time consumption of AdaTE fluctuates around 17 ms (η is set to 0.00001, the number of observer is set to one).

In this paper, the symbols used are standard, which is shown in Table 1.

Table 1.

Standard symbols in this paper.

Symbols	Description
$u_A$	The axial control variable
${\mathbf{u}}_T$	The transverse control variable
$D_A$	The power spectral density of $u_A$
${\mathbf{D}}_T$	The power spectral density of ${\mathbf{u}}_T$
${\mathbf{Q}}_{k-1,k}$	The transition covariance
${\mathbf{R}}_{i,j}$	The observation covariance
$t_m$	The memory time of fading memory strategy
${\xi }_T$	The truncation time
$\Vert \cdot \Vert$	Norm of a vector or a matrix, which is the L2 norm in this article
${\left[\mathbf{v}\right]}_{\times }$	the skew symmetric matrix of $\mathbf{v}$
$\mathsf{Z}\left\{\overline{x},{\sigma }^2\right\}$	Normal distribution with mean $\overline{x}$ and variance ${\sigma }^2$
$O\left(n\right)$	The metric of computational complexity

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.

Figure 3.

The observations and results of AdaTE based on cruising route. The first row shows the observations under different conditions, while the second row presents the results of trajectory estimation corresponding to the observations above. The coloring from red to blue is consistent with the time order. Groups (a) do not contain Observation drift, and Groups (b) contain Observation drift. In addition, we provide the 3D graph and top view in each group to show the 3D trajectory intuitively.

We choose EKF, UKF and sparse MAP as contrast algorithms. For the EKF and UKF, there are two different results with constant acceleration model (CA) and PLS model. For sparse MAP, the applied dynamic model is constant turning model (CT). Estimation error is measured with the normalized RMSE defined as $\frac{{\text{RMSE}}_{\text{result}}}{{\text{RMSE}}_{\text{observation}}}$ .

The calculation of ${\text{RMSE}}_{\text{result}}$ and ${\text{RMSE}}_{\text{observation}}$ is based on the error between them and the ground truth. Table 2

Table 2.

Statistics of the RMSE in the trajectory estimation task.

Methods	Cruise		Swaying		Snake-like
Observation drift	No	Yes	No	Yes	No	Yes
MAP-CT	0.164	0.115	0.179	0.117	0.257	0.135
EKF-CA	0.136	0.107	0.156	0.110	0.208	0.113
EKF-PLS	Div	Div	Div	Div	Div	Div
UKF-CA	0.133	0.106	0.152	0.110	0.204	0.113
UKF-PLS	0.142	0.113	0.230	0.124	0.201	0.135
AdaTE-PLS	0.128	0.056	0.152	0.045	0.169	0.086
Observation RMSE	24.78	58.89	23.65	91.25	14.19	28.02

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.

The observations and the corresponding results of AdaTE are shown in Figures 3, 4 and 5, where the figures on the first row show the observations under different conditions, and the figures on the second row are the estimated trajectory corresponding to the observations above. Observation drift is added to Groups (b) of Figures 3, 4 and 5. It should be noted that the AdaTE produces smooth and accurate estimation in every listed situations, even both of the missing observation and observation drift occurs in group (e) and (f).

Figure 4.

The observations and results of AdaTE based on swaying route. The first row shows the observations under different conditions, while the second row presents the results of trajectory estimation corresponding to the observations above. The coloring from red to blue is consistent with the time order. Groups (a) do not contain Observation drift, and Groups (b) contain Observation drift. In addition, we provide the 3D graph and top view in each group to show the 3D trajectory intuitively.

Figure 5.

The observations and results of AdaTE based on snake-like route. The first row shows the observations under different conditions, while the second row presents the results of trajectory estimation corresponding to the observations above. The coloring from red to blue is consistent with the time order. Groups (a) do not contain Observation drift, and Groups (b) contain Observation drift. In addition, we provide the 3D graph and top view in each group to show the 3D trajectory intuitively.

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

Figure 6.

The normalized RMSE of algorithms in each group of trajectory estimation, where (a)-(f) is the order of group. The bars represent the mean normalized RMSE of each algorithm, while the error lines represent the range decided by standard deviations. Note that the EKF-PLS has a divergent result in each group, and the normalized RMSE is beyond the range of the table.

Setting $\mathbf{P}$ be the set of key point nodes, $\mathbf{\Omega }$ 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

$${\mathbf{S}}_k^{\left(k:t_k\right)}=\underset{{\mathbf{X}}_k^{\left(k:t_k\right)}}{\arg \max }\mathrm{ℒ}\left({\mathbf{S}}_k^{\left(k:t_k\right)};{\mathbf{S}}_{k-1}^{\left(k:t_{k-1}\right)},\mathbf{P},\mathbf{\Omega }\right)$$

(36)

where

$$\begin{array}{l}\mathrm{ℒ}\left({\mathbf{S}}_k^{\left(k:t_k\right)};{\mathbf{S}}_{k-1}^{\left(k:t_{k-1}\right)},\mathbf{P},\mathbf{\Omega }\right)=\\ \log P\left({\mathbf{S}}_k^{\left(k:t_k\right)}|{\mathbf{S}}_{k-1}^{\left(k:t_{k-1}\right)}\right)+\log P\left(\mathbf{P}|{\mathbf{S}}_k^{\left(k:t_k\right)}\right)+\log P_a\left(\mathbf{\Omega }|{\mathbf{S}}_k^{\left(k:t_k\right)}\right)\end{array}$$

(37)

and

$$P_a\left(\mathbf{\Omega }|{\mathbf{S}}_k^{\left(k:t_k\right)}\right)={\displaystyle \prod _{i=k}^{t_k}\left(1-P_h\left(\mathbf{\Omega }|{\mathbf{S}}_k^{\left(i\right)}\right)\right)}$$

(38)

where $P\left(\mathbf{P}|{\mathbf{S}}_k^{\left(k:t_k\right)}\right)$ is the probability of arriving at planed nodes, $P_a\left(\mathbf{\Omega }|{\mathbf{S}}_k^{\left(k:t_k\right)}\right)$ is the probability of successful obstacles avoidance during the time $\left(k,t_k\right)$ , and $P_h\left(\mathbf{\Omega }|{\mathbf{S}}_k^{\left(i\right)}\right)$ 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.

Figure 7.

Overview of the 3D navigation at 8 representative moments, where path planning is adjusted online according to the position of next nodes and real time obstacles. The images on the top line from left to right are overviews at time 11, 21, 31, and 34. The bottom images are overviews at time 41, 51, 61, and 73. The dark blue curve is the historical path, and the purple curve is the local navigation plan. The dark blue ellipsoids are the covariance of key points; the wathet blue region around the key points is the nearby region within two standard derivations. The dark red ellipsoids are the restricted area of the obstacles, while the yellow regions around are the danger areas.

Figure 8.

The regional details of obstacle avoidance at 4 representative moments, where subgraphs (a)-(d) correspond to time 11, 34, 61 and 73. Each subgraph shows part of details from 4 different views, where the corresponding viewing angle is indicated at the top left. The dark blue curve is the historical path, and the purple curve is the planned path.

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) ⁴² has obvious higher complexity O(2n ² ). 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 ⁴³ 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.

Figure 9.

Some screenshots in visual tracking experiments, including the result of original fDSST (blue box), the result estimated by fDSST-AdaTE (red box), and the ground truth (yellow box). Note that in most of the images, fDSST-AdaTE obtaines a slightly better result than fDSST. Trajectory optimization for visual tracking.

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 model-based 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.

Table 3.

Mean overlap on eight scenes from vot 2013 dataset.

	fDSST	fDSST-AdaTE	Increase, %
Vot-cardark	0.7422	0.7507	1.16
Vot-coke	0.5589	0.5604	0.27
Vot-singer1	0.8042	0.8149	1.33
Vot-subway	0.6880	0.6914	0.50
Vot-tiger1	0.7337	0.7349	0.17
Vot-walking	0.7257	0.7333	1.05

A comparison of fDSST and fDSST-AdaTE in visual tracking task on 8 scenes from the VOT 2013 dataset. The index for algorithm evaluation is mean overlap. The table shows that in most scenes, fDSST-AdaTE has a better result compared with fDSST.

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 ⁴⁴ 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.