Foundations
2.3. Moving Object Tracking
stationary objects. The only way to avoid this quadratically increasing computational re-quirement is to develop suboptimal and approximate techniques. Recently, this problem has been subject to intense research. Approaches using approximate inference, using exact in-ference on tractable approximations of the true model, and using approximate inin-ference on an approximate model have been proposed. These approaches include:
• Thin junction tree filters (Paskin, 2003).
• Sparse extended information filters (Thrun et al., 2002; Thrun and Liu, 2003).
• Submap-based approaches: the Atlas framework (Bosse et al., 2003), compressed filter (Guivant and Nebot, 2001) and Decoupled Stochastic Mapping (Leonard and Feder, 1999).
• Rao-Blackwellised particle filters (Montemerlo, 2003).
This topic is beyond the scope intended by this dissertation. (Paskin, 2003) includes an excellent comparison of these techniques.
Perception Modelling and Data Association
Besides the computational complexity issue, the problems of perception modelling and data association have to be solved in order to accomplish city-sized SLAM. For in-stance, the described feature-based formulas may not be feasible because extracting fea-tures robustly is very difficult in outdoor, urban environments. Data association is difficult in practice because of featureless areas, occlusion, etc. We will address perception mod-elling in Chapter 3 and data association in Chapter 5.
2.3 MOVING OBJECT TRACKING
Formulation of Moving Object Tracking
The robot (sensor platform) is assumed to be stationary for the sake of simplicity. The general formula for the moving object tracking problem can be formalized in the proba-bilistic form as:
p(ok, sk | Zk) (2.30)
where ok is the true state of the moving object at time k, and sk is the true motion mode of the moving object at time k, and Zk is the perception measurement set leading up to time k.
Using Bayes’ rule, Equation 2.30 can be rewritten as:
p(ok, sk | Zk) = p(ok| sk, Zk)p(sk | Zk) (2.31) which indicates that the whole moving object tracking problem can be solved by two stages: the first stage is the mode learning stage p(sk | Zk), and the second stage is the state inference stage p(ok | sk, Zk).
Mode Learning and State Inference
Without a priori information, online mode learning of time-series data is a daunting task. In the control literature, specific data collection procedures are designed for iden-tification of structural parameters of the system. However, online collected data is often not enough for online identification of the structural parameters in moving object tracking applications.
Fortunately, the motion mode of moving objects can be approximately composed of several motion models such as the constant velocity model, the constant acceleration model and the turning model. Therefore the mode learning problem can be simplified to a model selection problem. It is still difficult though because the motion mode of moving objects can be time-varying. In this section, practical multiple model approaches are briefly reviewed such as the generalized pseudo-Bayesian (GPB) approaches and the interacting multiple model (IMM) approach. Because the IMM algorithm is integrated into our whole algo-rithm, the derivation of the IMM algorithm will be described in detail. The multiple model approaches described in this section are identical to those used in (Bar-Shalom and Li, 1988, 1995).
The same problems are solved with switching dynamic models in the machine learning literature (Ueda and Ghahramani, 2002; Pavlovic et al., 1999; Ghahramani and Hinton,
1998). In the cases that the models in the model set are linear, such systems are called jump-linear systems or switching jump-linear dynamic models. However, most of them are batch so that they are not suitable for our applications.
Fixed Structure Multiple Model Approach for Switching Modes. In the fixed structure multiple model approach, it is assumed that the mode of the system obeys one of a finite number of models in which the system has both continuous nodes as well as discrete nodes. Figure 2.14 shows a Dynamic Bayesian Network representing three time steps of an example multiple model approach for solving the moving object tracking problem.
Figure 2.14. A DBN for multiple model based moving object tracking. Clear circles denote hidden continuous nodes, clear squares denotes hidden discrete nodes and shaded circles denotes continuous nodes.
The mode of the moving object is assumed to be one of r possible models which is described by:
sk ∈ {Mj}rj=1 (2.32)
where M is the model set.
In practice the system does not always stay in one mode. Because mode jump or mode switch does occur, the mode-history of the system should be estimated. The mode history through time k is denoted as Sk
Sk = {s1, s2, . . . , sk} (2.33) Given r possible models, the number of possible histories, Mkl, is rkat time k, which increases exponentially with time. Let l be the index of the mode history.
l = 1, 2, . . . , rk (2.34)
The lth mode history, or sequence of modes, through time k is denoted as:
Mkl = {Ml11, Ml22, . . . , Mlkk}
= {Mk−1l , Mlkk} (2.35)
2.3 MOVING OBJECT TRACKING
where liis the model index at time i from the history l and
1 5 li 5 r i = 1, . . . , k (2.36)
Using Bayes’ rule, the conditional probability of the lth mode history Mkl can be ob-tained as:
µlk 4= p(Mkl | Zk)
= p(Mkl | Zk−1, zk)
= p(zk| Mkl, Zk−1)p(Mkl | Zk−1) p(zk| Zk−1)
= η · p(zk| Mkl, Zk−1)p(Mkl | Zk−1)
= η · p(zk| Mkl, Zk−1)p(Mlkk, Mk−1l | Zk−1)
= η · p(zk| Mkl, Zk−1)p(Mlkk| Mk−1l , Zk−1)µlk−1 (2.37) It is assumed that the mode jump process is a Markov process in which the current node depends only on the previous one.
p(Mlkk | Mk−1l , Zk−1) = p(Mlkk| Mk−1l )
= p(Mlkk| Mlkk−1) (2.38) Equation 2.37 can be rewritten as:
µlk = ηp(zk | Mkl, Zk−1)p(Mlkk | Mlkk−1)µlk−1 (2.39) in which conditioning on the entire past history is needed even using the assumption that the mode jump process is a Markov process.
Using the total probability theorem, Equation 2.31 can be obtained by:
p(ok | Zk) =
rk
X
l=1
p(ok| Mkl, Zk)p(Mkl | Zk)
=
rk
X
l=1
p(ok| Mkl, Zk)µlk (2.40)
This method is not practical because an exponentially increasing number of filters are needed to estimate the state. Also even if the modes are Markov, conditioning on the entire past history is needed. In the same way as dealing with the computational complexity of the SLAM problem, the only way to avoid the exponentially increasing number of histories is to use approximate and suboptimal approaches which merge or reduce the number of the mode history hypotheses in order to make computation tractable.
The Generalized Pseudo-Bayesian Approaches. The generalized pseudo-Bayesian (GPB) approaches (Tugnait, 1982) apply a simple suboptimal technique which keeps the histories of the largest probabilities, discards the rest, and renormalizes the probabilities.
In the generalized pseudo-Bayesian approaches of the first order (GPB1), the state estimate at time k is computed under each possible current model. At the end of each cycle, the r hypotheses are merged into a single hypothesis. Equation 2.40 is simplified as:
p(ok| Zk) = Xr j=1
p(ok | Mj, Zk)p(Mj| Zk)
= Xr j=1
p(ok | Mj, zk, Zk−1)µjk
≈ Xr j=1
p(ok | Mj, zk, ˆok−1, Σok−1)µjk (2.41) where the Zk−1is approximately summarized by ˆok−1and Σok−1. The GPB1 approach uses r filters to produce 1 state estimate. Figure 2.15 describes the GPB1 algorithm.
Figure 2.15. The GPB1 algorithm of one cycle for 2 switching models.
In the generalized pseudo-Bayesian approaches of second order (GPB2), the state es-timate is computed under each possible model at current time k and previous time k − 1.
p(ok | Zk) = Xr j=1
Xr i=1
p(ok| Mjk, Mik−1, Zk)p(Mik−1| Mjk, Zk)p(Mjk | Zk) (2.42) In the GPB2 approach, there are r estimates and covariances at time k − 1. Each is pre-dicted to time k and updated at time k under r hypotheses. After the update stage, the r2 hypotheses are merged into r at the end of each estimation cycle. The GPB2 approach uses r2filters to produce r state estimates. Figure 2.16 describes the GPB2 algorithm, which does not show the state estimate and covariance combination stage. For output only, the latest state estimate and covariance can be combined from r state estimates and covariances.
The Interacting Multiple Model Algorithm. In the interacting multiple model (IMM) approach (Blom and Bar-Shalom, 1988), the state estimate at time k is computed under each possible current model using r filters and each filter uses a suitable mixing of the previous model-conditioned estimate as the initial condition. It has been shown that the
2.3 MOVING OBJECT TRACKING
Figure 2.16. The GPB2 algorithm of one cycle for 2 switching models
IMM approach performs significantly better than the GPB1 algorithm and almost as well as the GPB2 algorithm in practice. Instead of using r2filters to produce r state estimates in GPB2, the IMM uses only r filters to produce r state estimates. Figure 2.17 describes the IMM algorithm, which does not show the state estimate and covariance combination stage.
The derivation of the IMM algorithm is described as the following:
Figure 2.17. The IMM algorithm of one cycle for 2 switching models
Similar to Equation 2.40, the Bayesian formula of the IMM-based tracking problem is described as:
p(ok| Zk) T otal P rob.
=
Xr j=1
p(ok| Mjk, Zk)p(Mjk| Zk)
Bayes
=
Xr j=1
p(zk | ok, Mjk, Zk−1)p(ok | Mjk, Zk−1)
p(zk | Mjk, Zk−1) p(Mjk| Zk)
= η
Xr j=1
p(zk| ok, Mjk, Zk−1)p(ok| Mjk, Zk−1)p(Mjk| Zk)
M arkov
= η
Xr j=1
p(zk| ok, Mjk)
| {z }
Update
p(ok| Mjk, Zk−1)
| {z }
Prediction
p(Mjk | Zk)
| {z }
Weighting
(2.43)
where p(Mjk | Zk) is the model probability and can be treated as the weighting of the estimate from the model Mjk. p(ok | Mjk, Zk−1) is the prediction stage and p(zk | ok, Mjk) is the update stage. The final estimate is the combination of the estimates from all models.
The model probability, p(Mjk| Zk), can be calculated recursively as follows:
µjk =4 p(Mjk | Zk)
Bayes
= ηp(zk | Mjk, Zk−1)p(Mjk| Zk−1)
T otal P rob.
= η p(zk| Mjk, Zk−1)
| {z }
Mode Match Xr i=1
p(Mjk| Mik−1, Zk−1)
| {z }
Mode Transition
p(Mik−1| Zk−1)
| {z }
µik−1
(2.44)
The last term on the right hand side is the model probability of the model Miat time k − 1.
The second term on the right hand side is the mode transition probability. Here it is assumed that the mode jump process is a Markov process with known mode transition probabilities.
Therefore,
Pij 4
= p(Mjk| Mik−1, Zk−1)
= p(Mjk| Mik−1) (2.45)
The first term of the right hand side can be treated as mode-matched filtering, which is computed by:
Λjk = p(z4 k | Mjk, Zk−1)
= p(zk | Mjk, ˆok−1, Σok−1) (2.46) To summarize, the recursive formula for computing the model probability is:
µjk = ηΛjk Xr i=1
Pijµik−1 (2.47)
where η is the normalization constant.
The prediction stage of Equation 2.43 can be done as follows:
p(ok | Mjk, Zk−1)
T otal P rob.
=
Xr i=1
p(ok| Mjk, Mik−1, Zk−1)p(Mik−1| Mjk, Zk−1)
T otal P rob.
≈
Xr i=1
Z
p(ok | Mjk, Mik−1, {olk−1}rl=1)dok−1µi|j
Interaction
≈
Xr i=1
Z
p(ok | Mjk, Mik−1, ˆoik−1)dˆok−1µi|j (2.48) The second line of the above equation shows that Zk−1is summarized by r model-conditioned estimates and covariances, which is used in the GPB2 algorithm. The third line shows the key idea of the IMM algorithm which uses a mixing estimate ˆok−1 as the
2.3 MOVING OBJECT TRACKING
input of the filter instead of {olk−1}rl=1. The last term on the right hand side, the mixing probability can be obtained by:
µi|j 4= p(Mik−1| Mjk, Zk−1)
= ηp(Mjk| Mik−1, Zk−1)p(Mik−1| Zk−1)
= ηPijµik−1 (2.49)
where η is the normalization constant. Using the assumption that the mixture estimate is a Gaussian and then approximating this mixture via moment matching by a single Gaussian, the mixed initial condition can be computed by:
o0jk−1 = Xr i=1
ˆ
oik−1µi|j (2.50)
and the corresponding covariance is:
Σo0j
k−1 =
Xr i=1
{Σiok−1+ (ˆo0jk−1− ˆoik−1)(ˆo0jk−1− ˆoik−1}µi|j (2.51)
With the mixed initial conditions, the prediction and update stages can be done with each model using Kalman filtering. Let the estimate and the corresponding covariance from each model be denoted by ˆojk and Σjok respectively. For output purposes, the state estimate and covariance can be combined according to the mixture equations:
ˆ ok =
Xr j=1
ˆ
ojkµjk (2.52)
Σok = Xr j=1
{Σjok+ (ˆojk− ˆok)(ˆojk− ˆok)T}µjk (2.53)
Calculation Procedures of the IMM algorithm
From the Bayesian formula of the moving object tracking problem, one cycle of the the calculation procedures consists of the initialization, prediction, data association and update stages.
Stage 1: Initialization. Figure 2.18 shows the initialization stage of moving object tracking and Figure 2.19 shows the corresponding DBN. In this stage, it is assumed that there are r possible models in the model set, and the prior model probabilities and the mode transition probabilities are given. The mixing probabilities are computed by Equa-tion 2.49 and the mixed initial condiEqua-tions are computed by EquaEqua-tion 2.50 and EquaEqua-tion 2.51.
Figure 2.18. The initialization stage of moving object track-ing.
Figure 2.19. A DBN represent-ing the initialization stage of moving object tracking.
Stage 2: Prediction. Figure 2.20 shows the prediction stage of moving object track-ing and Figure 2.21 shows the correspondtrack-ing DBN. With the mixed initial conditions, each filer use its corresponding motion model to perform prediction individually in the IMM algorithm.
Figure 2.20. The prediction stage of moving object tracking
Figure 2.21. A DBN represent-ing the prediction stage of moving object tracking
Stage 3: Data Association. Figure 2.22 shows the data association stage of moving object tracking and Figure 2.21 shows the corresponding DBN. In this stage, the sensor returns a new measurement zkand each filter use its own prediction to perform data asso-ciation.
Stage 4: Update. Figure 2.24 shows the update stage of moving object tracking and Figure 2.25 shows the corresponding DBN. In this stage, each filter is updated with the associated measurement and then the mode-matched filtering is done by Equation 2.46.
2.3 MOVING OBJECT TRACKING
Figure 2.22. The data associa-tion stage of moving object tracking
Figure 2.23. A DBN represent-ing the data association stage of moving object tracking
The model probabilities are updated by Equation 2.47. For output purposes, the state and covariance can be computed by Equation 2.52 and Equation 2.53.
Figure 2.24. The update stage of moving object tracking
Figure 2.25. A DBN represent-ing the update stage of movrepresent-ing object tracking
Motion Modelling
In the described formulation of moving object tracking, it is assumed that a model set is given or selected in advance, and tracking is performed based on model averaging of this model set. Theoretically and practically, the performance of moving object tracking strongly relates to the selected motion models. Figure 2.26 illustrates the different perfor-mances using different motion models. Given the same data set, the tracking results differ according to the selected motion models. Figure 2.27 illustrates the effects of model set completeness. If a model set does not contain a stationary motion model, move-stop-move object tracking may not be performed well. We will address the motion modelling related issues in Chapter 4.
(a) (b)
Figure 2.26. Model Selection. On the left is the result of tracking using a compli-cated motion model. On the right is the same data using a simple motion model.
(a) (b)
Figure 2.27. Move-stop-move object tracking. On the left is the result of tracking using only moving motion models. On the right is the result of tracking using moving motion models and a stationary motion model.
Perception Modelling and Data Association
Regarding perception modelling, it is assumed that objects can be represented by point-features in the described formulation. In practice this may not be appropriate be-cause of a wide variety of moving objects in urban and suburban areas. In Chapter 3, the hierarchical object based representation for moving object tracking will be described in de-tail. With regard to data association, using not only kinematic information from motion modelling but also geometric information from perception modelling will be addressed in Chapter 5.