Motion Prediction for Continued Autonomy

Introduction

Recent hardware developments have rendered dynamic vision – the confluence of computer vision and control–a viable option fora large number of applications, ranging from surveillance and manufacturing to assisting individuals with disabilities. This article discusses one ofthe critical issues currently limiting widespread use of these systems, namely their potential fragility when operating in dense, cluttered environments,and shows that robustness can be substantially enhanced by exploiting the predictive power of dynamic motion models learned from scene data. The articleis organized as follows: Sect. “Definition of the Subject” provides a brief overview ofthe subject. Section “Introduction” illustrates, with a simple example, the robustnesschallenges faced by dynamic vision methods when operating in cluttered, partially stochastic environment, and shows how to address these challengesthrough the use of dynamic motion models. These ideas are further developed in...

Appendix: Background Results on Linear Spaces and Robust System Identification

In this appendix we summarize, for ease of reference, the background results on linear spaces and robust identification used in this chapter.

Linear Spaces

Algebraic structures are instrumental in understanding many problems arising in systems theory from an abstract point of view. In particular these tools are required to formalize and solve the optimal filtering and estimation problems arising in the context of multiframe tracking.

Field

Definition 1

A  field ${ (\mathcal{F},\& ,\star) }$ is an algebraic structure composed of a set ${ \mathcal{F} }$ and two operations ${ \& }$ and ? with the following properties:

• 1. Set ${ \mathcal{F} }$ is closed with respect to ${ \& }$, i.?e. $a,b\in\mathcal{F}\;\Longrightarrow\; (a\& b)\in\mathcal{F}$.

• 2. Operation ${ \& }$ is associative, i.?e. $(a\& b)\& c=a\& (b\& c)=a\& b\& c$ for $a,b,c\in\mathcal{F}$.

• 3. Operation ${ \& }$ is commutative, i.?e. ${ a\& b=b\& a }$ for ${ a,b\in\mathcal{F} }$.

• 4. Set ${ \mathcal{F} }$ contains the neutral element ${ n_\& }$ with respect to ${ \& }$, that is, there exists ${ n_\& }$ such that ${ a\& n_\& =a }$ for all ${ a \in\mathcal{F} }$.

• 5. Set ${ \mathcal{F} }$ contains the inverse element ${ a_\&^I }$ with respect to ${ \& }$, that is, for all ${ a \in \mathcal{F} }$ there exists ${ a^I_\&\in\mathcal{F} }$, such that ${ a\&a_\&^I=n_\& }$.

• 6. Set ${ \mathcal{F} }$ is closed with respect to ?.

• 7. Operation ? is associative.

• 8. Set ${ \mathcal{F} }$ contains the neutral element ${ n_\star }$ with respect to ?.

• 9. Set ${ \mathcal{F} }$ contains the inverse element ${ a_\star^I }$ with respect to ?.

• 10. Operation ? is distributive with respect to ${ \& }$, i.?e. ${ (a\&b)\star c = (a\star c) \& (b\star c) }$ for ${ a,b,c\in\mathcal{F} }$.

An example of a field is the set R of the real numbers, equipped with operations ${ (+,\times) }$ as ${ (\&,\star) }$ respectively. Here ${ n_\&=0 }$, ${ n_\star=1 }$, ${ a^I_\&=-a }$ and ${ a^I_\star=a^{-1} }$ (${ a\neq 0 }$).

Linear Vector Space

Definition 2

A set ${ \mathcal{V} }$ is a  linear vector space over the field ${ (\mathcal{F},+,\times) }$ if and only if the following properties are satisfied (in the sequel the elements of ${ \mathcal{F} }$ and ${ \mathcal{V} }$ will be called scalars and vectors respectively):

• 1. Set ${ \mathcal{V} }$ is closed with respect to +.

• 2. Operation + is associative in ${ \mathcal{V} }$.

• 3. Operation + is commutative in ${ \mathcal{V} }$.

• 4. Set ${ \mathcal{V} }$ contains the neutral element with respect to +.

• 5. Set ${ \mathcal{V} }$ contains the inverse element with respect to +.

• 6. ${ \mathcal{V} }$ is closed with respect to operation × between scalars and vectors.

• 7. Operation × among scalars and vectors is associative in the scalars, i.?e. $(a\times b)\times v=a\times (b\times v)=a\times b\times v$ for ${ a,b\in\mathcal{F} }$ and ${ v\in\mathcal{V} }$.

• 8. Distributive 1: ${ (a+b)\times v = (a\times v) + (b\times v) }$ for ${ a,b\in\mathcal{F} }$ and ${ v\in\mathcal{V} }$.

• 9. Distributive 2: ${ (u+v)\times a = (u\times a) + (v\times a) }$ for ${ a\in\mathcal{F} }$ and ${ u,v\in\mathcal{V} }$.

• 10. Field  ${ \mathcal{F} }$ contains the neutral element of operation × between vectors and scalars, i.?e. ${ n_\times \times v =v }$ for $n_\times \in\mathcal{F}$ and ${ v\in\mathcal{V} }$.

Examples of linear spaces over the field of real numbers are the set of matrices in ${ {R}^{n\times 1} }$ and the set of sequences of real numbers, both equipped with the usual addition and scalar multiplication operations. The former is an example of a finite dimensional space, while the latter could be finite or infinite dimensional depending upon whether finite or infinite sequences are considered.

Metric, Norm and Inner Products

Definition 3

A metric space ${ (\mathcal{V},m(\cdot,\cdot)) }$ is defined in terms of a  linear vector space ${ \mathcal{V} }$ and a real function (the “metric”) ${ m(\cdot,\cdot):\mathcal{V}\times \mathcal{V}\rightarrow {R}_+ }$, satisfying the following conditions:

• 1. ${ m(x,y)\geq 0\quad \forall x,y\in\mathcal{V} }$.

• 2. ${ m(x,y)= 0\quad\Longleftrightarrow\quad x=y }$.

• 3. ${ m(x,y)= m(y,x) \quad\forall x,y\in\mathcal{V} }$.

• 4. ${ m(x,z) \leq m(x,y)+m(y,z) \quad\forall x,y,z\in\mathcal{V} }$.

Here ${ R_+\doteq \left\{ x\in R,\; x\geq 0\right\} }$.

Definition 4

A  normed space ${ (\mathcal{V},\|\cdot\|) }$. is defined in terms of a  linear vector space ${ \mathcal{V} }$ and a real function ${ \|\cdot\|:\mathcal{V}\rightarrow {R}_+ }$ that satisfies the following conditions:

• 1. ${ \|x\|\geq 0\quad \forall x\in\mathcal{V} }$.

• 2. ${ \|x\|= 0\quad\Longleftrightarrow\quad x=\underline 0 }$.

• 3. ${ \|\alpha x\|=|\alpha |\cdot\|x\|\quad\forall x\in\mathcal{V}, \alpha\in\mathcal{F} }$.

• 4. ${ \|x +y\|\leq \|x\| + \|y\|\quad\forall x,y\in\mathcal{V} }$.

Here ${ |\cdot| }$ represents the magnitude of a scalar.

The following are examples of normed spaces:

• 1. The linear space of n-dimensional real vectors, equipped with the norm:

  $$ \|x\|_p \doteq \root p \of {\sum_{i=1}^n |x_i|^p} \quad p\geq 1 $$

  (10)
  $$ \|x\|_\infty \doteq \max_{1\leq i\leq n} |x_i| $$

  (11)

• 2. The linear space of real sequences, equipped with the norm:

  $$ \|x\|_p \doteq \root p \of {\sum_{i=1}^\infty |x_i|^p} \quad p \geq 1 $$

  (12)
  $$ \|x\|_\infty \doteq \max_{i\geq 1} |x_i| $$

  (13)

Robust Identification

The field of system identification concerns itself with mechanisms and algorithms that process finite, partial, and corrupted data to yield abstract mathematical descriptions of real world systems.

Traditional identification approaches [55,56] assume that the data is corrupted by a stochastic process with known statistical properties and that the system to be identified has a prescribed model structure. Most of these identification procedures are based on least squares methods that estimate the parameters of the hypothesized models from the corrupted measurements. In these approaches the only source of uncertainty is the noise in the measurements while the prescribed model is assumed to be an accurate representation of the real system.

In many situations, for example when measurements are known within an accuracy range or when the available statistical information might be questionable, deterministic bounded noise descriptions are a practical and sound alternative to stochastic ones. Using this approach, the problem of system identification can be formulated as finding the sets of parameter values that are consistent with the known noise bounds. A survey of set membership formulations for system identification can be found in [57].

Noise description is only one of the factors affecting the quality of an identified model. Perhaps a more important factor is the unrealistic presumption that a fixed model structure may fully represent the system to be identified: In practice, only partial information of the physical system is available, model parameters might change due to different operation conditions, and real systems are often too complex to be accurately modeled from first principles. These issues are addressed by robust system identification, which departs from traditional approaches by using a deterministic worst-case approach with no prior assumption about the order of the system. Instead, robust identification procedures are based on a priori assumptions on the class of systems and noise and on the a posteriori experimental data. Using this information robust system identification algorithms find nominal models based on the experimental data and worst-case identification error bounds over the set of models defined by the a priori information.

Information Consistency and Diameter of Information

Due to the fact that the assumed a priori information is, in general, a quantification of the engineering common sense or simply a “leap of faith”, there is no guarantee that it will be coherent with the a posteriori experimental data. Thus, robust identification procedures must always first test the consistency of both types of information.

Consistency can be better understood by considering the set of all possible models which could have produced the a posteriori data y, in accordance with the class of systems ${ \mathcal{S} }$ and the measurement noise ${ \eta \in \mathcal{N} }$:

$$ {\mathcal T}(\mathbf{y}) \doteq \{ g \in {\mathcal S} \mid \mathbf{y} = E(g,\eta) , \eta \in \mathcal{N} \} $$

where ${ E(.,.) }$ is the “experiment” operator. Intuitively, the a priori information and the a posteriori experimental data are consistent if there exists at least one element in ${ \mathcal{S} }$ that could have generated the observed experimental data. This concept is formalized in the next definition:

Definition 5

The a priori information ${ (\mathcal{S},\mathcal{N}) }$ is consistent with the experimental a posteriori information y if and only if the set ${ \mathcal{T}(\mathbf{y}) }$ is nonempty.

Once consistency has been established, the computation of a nominal model and a valid model error bound can be attempted. There are two different types of algorithms to accomplish this. The first type of procedures [58,59] are guaranteed to converge, even when the information available is inconsistent. However, they might result on a nominal model outside the consistency set. The second type of procedures, and the type we use in the sequel, are interpolatory algorithms [60]. As we show next, these algorithms are always guaranteed to converge as the information is completed. Moreover, they are optimal to a factor of 2, in the sense that their worst-case error is never larger than twice the minimum achievable error over the set of all identification algorithms.

Worst Case Identification Error

A salient feature of robust identification is its ability to provide worst-case bounds on the identification error. Given an identification algorithm ${ \mathcal{A} }$ mapping the a priori and a posteriori information to candidate nominal model, its local error is defined as follows:

$$ e(\mathcal{A},\mathbf{y})=\sup_{g\in\mathcal{T}(\mathbf{y})}m\left[g,\mathcal{A}(\mathbf{y}, \mathcal{S},\mathcal{N}) \right] $$

(14)

that is, the maximum distance between the identified set and any other plant in the set ${ \mathcal{T}(\mathbf{y}) }$. Note that this error is related to the outcome of a specific experiment y. A global error can be defined by considering the worst–case error over the set of all possible experimental outcomes:

Definition 6

The worst case global error of a given algorithm ${ \mathcal{A}(\mathbf{y},\mathcal{S},\mathcal{N}) }$ is given by:

$$ e(\mathcal{A}) = \sup_{\mathbf{y}\in\mathbf{Y}} e(\mathcal{A},\mathbf{y}) $$

(15)

where Y is the set of all possible experimental data, consistent with sets ${ \mathcal{S} }$ and ${ \mathcal{N} }$.

Next we briefly review how to obtain mathematically tractable bounds for these errors. Recall that the set ${ \mathcal{T}(\mathbf{y})\subset \mathcal{S} }$ is the smallest set of models that are indistinguishable from the view point of the input information. Therefore, roughly speaking, its size gives lower and upper bounds on the identification error defined above. In order to formalize these ideas and obtain computable bounds we need to introduce the following concepts:

Definition 7

The radius and diameter of a subset ${ \mathcal{A} }$ of a metric space ${ (\mathcal{X},m) }$ are

$$ \begin{aligned} r(\mathcal{A}) & = \inf_{x\in \mathcal{X}} \sup_{a \in \mathcal{A}} m(x,a)\\ d(\mathcal{A}) & = \sup_{x,a \in \mathcal{A}} m(x,a)\:. \end{aligned} $$

The radius can be interpreted as the maximum error, measured in the metric ${ m(.) }$, when considering the set ${ \mathcal{A} }$ as represented by a single “central” point (which might not belong to ${ \mathcal{A} }$). The diameter is the maximum distance between any two points in the set. Based on these concepts of radius, we next quantify the “size” of the available information.

Definition 8

The radius and diameter of information are defined as:

$$ \begin{aligned} {\mathcal{R}}(\mathcal{I}) & \doteq \sup_{\mathbf{y} \in \mathbf{Y}} r[\mathcal{T}(\mathbf{y})]\\ \mathcal{D}(\mathcal{I}) & \doteq \sup_{\mathbf{y} \in \mathbf{Y}} d[\mathcal{T}(\mathbf{y})] \end{aligned} $$

where Y is the set of all possible experimental data consistent with the sets ${ \mathcal{S} }$ and ${ \mathcal{N} }$:

$$ \mathbf{Y} \doteq \{E(g,\eta) \mid g \in \mathcal{S}, \eta \in \mathcal{N} \}. $$

The following result gives worst–case bounds of the identification error based on these concepts:

Lemma 1

The worst case identification error defined in (15) satisfies the following inequality:

$$ e(\mathcal{A}) \geq \mathcal{R}(\mathcal{I}) \geq \frac{1}{2} \mathcal{D}(\mathcal{I}) $$

(16)

for any algorithm ${ \mathcal{A} }$. The following upper bound holds:

$$ \mathcal{D}(\mathcal{I}) \geq e(\mathcal{A}_I) $$

(17)

for any interpolation algorithm ${ \mathcal{A}_I }$.

The bounds above are of theoretical importance. For instance ${ \mathcal{R(I)} }$ can be interpreted as an intrinsic error that cannot be decreased by any identification algorithm, unless extra information is added to the problem. On the other hand, these quantities are in general hard to compute. Fortunately, in practically relevant cases, they lead to mathematically tractable problems.

Definition 9

A set ${ \mathcal{A} }$ in a linear space X is called symmetric if and only if there exists an element ${ c\in X }$ such that for any ${ a\in X }$ for which ${ c+a\in \mathcal{A} }$ then ${ c-a\in \mathcal{A} }$. The element c is called the symmetry point of set ${ \mathcal{A} }$.

Lemma 2

If the a priori sets ${ \mathcal{S} }$ and ${ \mathcal{N} }$ are symmetric and convex with respect to 0, and the experiment operator ${ E(g,\eta) }$ is linear with respect to both g and ? then the diameter of information satisfies:

$$ \mathcal{D}(\mathcal{I})=\sup_{\mathbf{y} \in \mathbf{Y}} d\left[\mathcal{T}(\mathbf{y})\right] = d\left[\mathcal{T}(\mathbf{y}_0)\right]\; ,\quad \mathbf{y}_0= E(0,0) $$

(18)

Furthermore,

$$ d\left[\mathcal{T}(\mathbf{y}_0)\right]= 2 \sup_{g\in \mathcal{T}(\mathbf{y}_0)} m(g,0)\:. $$

(19)

Roughly speaking, the result above states that the experiment that yields the least amount of information is the one that results in a null outcome. Moreover, a bound on the worst case identification error is given by twice the maximum distance from any element in ${ \mathcal{T}(\mathbf{y_o}) }$ to the center of symmetry of ${ \mathcal{S} }$.

Time–Domain Based Interpolatory Identification Algorithms

In this section we briefly review the properties of the specific identification algorithm, based on time–domain data, used in this paper to establish the existence of operators with the appropriate features. To this effect we need several preliminary results.

The first lemma considers the problem of the existence of a causal linear discrete-time invariant operator such that the first n terms of its transfer function are given:

Lemma 3 (Carathéodory–Fejér)

Given a matrix valued sequence ${ \left \{ \mathbf{L}_i \right \}_{i=0}^{n-1} }$, there exists a causal, discrete-time, LTI operator ${ L(z) \in \mathcal{B}\mathcal{H}_\infty }$ such that

$$ L(z) = \mathbf{L}_0 + \mathbf{L}_1 z + \mathbf{L}_2 z^2+ \ldots \mathbf{L}_{n-1}z^{n-1} + \ldots $$

(20)

if and only if

$$ (\mathbf{T}_L^n)^\mathrm{T} \mathbf{T}_L^n \leq \mathbf{I} $$

(21)

where I denotes the identity matrix of compatible dimension.

Proof

See for instance Chap. 1 in [61].?

In the sequel we consider operator families of the form ${ \mathcal{S} }$:

$$ \mathcal{S}\doteq \left \{S(z) = H(z)+P(z) \right \} $$

(22)

where operators ${ S(z) }$ are described in terms of a nonparametric component ${ H(z)\in\mathcal{B}\mathcal{H}_\infty(K) }$ and a parametric component ${ P(z) }$. We will further assume that the parametric component ${ P(z) }$ belongs to the following class ${ \mathcal{P} }$ of affine operators:

$$ \mathcal{P} \doteq \{ P(z)=\mathbf{p}^\mathrm{T}\mathbf{G}_p(z), \, \mathbf{p}\in \mathcal{R}^{N_p} \}, $$

(23)

where the ${ N_p }$ components ${ \mathbf{G}_{p_i}(z) }$ of vector ${ \mathbf{G}_p(z) }$ are known, linearly independent, rational transfer functions.

The next lemma gives a necessary and sufficient condition for two finite vector sequences to be related by an operator in the family ${ \mathcal{S} }$.

Lemma 4

Given a scalar K, and two vector sequences ${ (\mathbf{u},\mathbf{y}) }$, there exists an operator ${ S \in \mathcal{S} }$ such that ${ \mathbf{y}=S\mathbf{u} }$ if and only if there exists a vector h satisfying:

$$ \begin{aligned} M(\mathbf{h}) \doteq \begin{bmatrix}\mathbf{I} & (\mathbf{T}_{h}^N)^\mathrm{T} \\ \mathbf{T}_{h}^N & \frac{1}{K^2} \end{bmatrix} \geq 0 \\ \mathbf{y}=\mathbf{T}_{u} \mathbf{P} \mathbf{p}+\mathbf{T}_{u}\mathbf{h} \end{aligned} $$

(24)

where ${ (\mathbf{P})_k\doteq [g_k^1 \; g_k^2 \; \cdots \; g_k^{N_p}] }$, with ${ g_k^i }$ denoting the kth Markov parameter of the ith transfer function ${ G_{p_i}(z) }$, ${ h_k }$ the kth Markov parameter of the nonparametric component ${ H(z) }$, respectively, and the scalar K is an upper bound of the ${ \ell_2 }$ induced norm of ${ H(z) }$.

Moreover, in this case all such operators S can be parametrized in terms of a free parameter ${ Q(z) \in \mathcal{B}\mathcal{H}_\infty }$. In particular, the choice ${ Q(z)=0 }$ leads to the “central” model

$$ S_{\mathrm{central}}(z)=H_{o}(z)+ \mathbf{p}^\mathrm{T}\mathbf{G}_p(z) $$

where an explicit state–space realization of ${ H_{o}(z) }$ is given by:

$$ H_o(z) = \mathbf{C}_H\left(z\mathbf{I}-\mathbf{A}_H\right)^{-1}\mathbf{B}_H + \mathbf{D}_H $$

with

$$\begin{aligned} \mathbf{A}_H=&\left \{ \mathbf{A}-[\mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-} +(\mathbf{A}^\mathrm{T}-\mathbf{I})]^{-1} \mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-}(\mathbf{A}-\mathbf{I})\right \}^{-1} \\ \mathbf{B}_H=& \enskip [\mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-}(\mathbf{A}^\mathrm{T}-\mathbf{A}-\mathbf{I}) - (\mathbf{A}^\mathrm{T}-\mathbf{I})\mathbf{A}]^{-1}\mathbf{C}_{-}^\mathrm{T} \\ \mathbf{C}_H=& \enskip K \mathbf{C}_{+} - K \mathbf{C}_{+} \biggl \{ \mathbf{A}-[\mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-} +(\mathbf{A}^\mathrm{T}-\mathbf{I})]^{-1}\\[-2ex] &\qquad\qquad\qquad\qquad\qquad\qquad\cdot\mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-}(\mathbf{A}-\mathbf{I})\biggr \}^{-1} \\ \mathbf{D}_H=& \enskip K \mathbf{C}_{+} \biggl\{ [ \mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-} +(\mathbf{A}^\mathrm{T}-\mathbf{I})]\\[-2ex] &\qquad\qquad\qquad\qquad\cdot\mathbf{A} - \mathbf{C}_{-}^\mathrm{T}\mathbf{C}_{-}(\mathbf{A}-\mathbf{I}) \biggr\}^{-1}\mathbf{C}_{-}^\mathrm{T}\;,\end{aligned} $$

(25)

and

$$ \begin{aligned} &\mathbf{A}=\left[\begin{array}{cc} 0 & \mathbf{I}_{N\times N} \\ 0 & 0 \end{array} \right], \quad \mathbf{C}_-=[\overbrace{1\quad 0\quad\ldots\quad 0}^{N+1}],\\ &\qquad\qquad\qquad\qquad\qquad\qquad\qquad\quad\mathbf{C}_+=\frac{{\mathbf{h}}^\mathrm{T}}{K}. \end{aligned} $$

(26)

Proof

See Theorem 18.5.2 in [62] and [43].?

Finally, the following corollary addresses the issue that real plants are subject to some unknown but bounded noise as represented in Fig. 12.

Figure 12

Linear operator S with input u and output y corrupted with noise ?

Corollary 1 ([43])

Consider the problem of identifying an operator ${ S \in \mathcal{S} }$ from measurements of its output y to a known input u, corrupted by additive bounded noise ? in a given set ${ \mathcal{N} }$:

$$ y_k=(S*u)_k+\eta_k\:,\quad k=0,1,\dots,N\:. $$

(27)

Then there exist ${ S \in\mathcal{S} }$ that satisfies (27) if and only there exists a pair of vectors ${ (\mathbf{h},\mathbf{p}) }$ such that ${ M(\mathbf{h}) > 0 }$ and ${ \mathbf{y}-\mathbf{T}_{u} \mathbf{P} \mathbf{p}-\mathbf{T}_{u}\mathbf{h}\in\mathcal{N} }$. In that case, one such operator is given ${ S_{\text{central}}=\mathbf{p}^\mathrm{T}\mathbf{G}_p+H_o }$, where ${ H_o }$ has the state–space realization (25).