** Motion Modelling**

**4.3. Moving Object Motion Modelling**

For tracking, not only motion but also other types of information or constraints can be selected and added to the model set. In (Kirubarajan et al., 1998), terrain conditions are used as constraint models and are added to the model set to improve performance of ground target tracking via a VS-IMM algorithm.

**4.2. Robot Motion Modelling**

Robot motion modelling involves kinematics and dynamics, which are the core top-ics in the control literature. This topic has received a great deal of attention over several decades in the car, air, and marine industries. Kinematic equations describe the spatial re-lationships between the vehicle (robot) frame and the world frame. Dynamic equations are derived from Newtonian or Lagrangian mechanics, which explain the force interaction be-tween the robot and its surrounding environment. Research on manned vehicles has been used as the guideline for robot design and modelling. For robots operated on the ground, (Wong, 2001) provides a comprehensive treatment of ground vehicle modelling on both road and off-road terrains. For marine robots, (Fossen, 2002) is the definitive textbook and (Wang, 1996) addresses the modelling and control issues of underwater vehicles. For aerial vehicles, (McLean, 1990) is a good reference.

One of the keys for online robot motion modelling is modelling of motion sensors.

With advanced sensor modelling, accurate relative localization using inexpensive odome-try has been demonstrated to be feasible for mobile robots in (Kelly, 2000) and (Doh et al., 2003).

While perception modelling has been the subject of extensive study, only few results such as (Julier and Durrant-Whyte, 2003) address the role played by the robot motion model in the navigation system. Although robot motion modelling is beyond the scope intended by this dissertation and will not be discussed further, a good treatment of robot motion modelling can be a crucial factor for robots operating at high speeds, on three-dimensional rough terrain, in the sky, or underwater.

**4.3. Moving Object Motion Modelling**

Because of their low computational cost and satisfactory performance, the IMM algo-rithm and its variants (Mazor et al., 1998) have been successfully implemented in many tracking applications for dealing with the moving object motion modelling problem. Since the formulation of the IMM algorithm has been addressed in Section 2.3, we will only de-scribe the motion models used in the IMM algorithm. Because it is very time-consuming and difficult to obtain ground-truth for moving object tracking in our applications, we use synthetic data to examine the algorithms under varying conditions.

**Discrete Time State Space Model**

Just as with the robot motion modelling problem, moving objects can be described by
kinematic and dynamic equations. Motion models used for moving object tracking often
*have a simpler form because of limited data associated with moving objects and limited time*
for motion model learning or adapting in practice. In the tracking literature, moving objects
can be characterized by a simplified discrete time state space model, namely:

**o***k+1* *= F**k***o***k**+ G**k***u***k***+ v***k* (4.1)

**where o***k**is the state vector at time k, F**k**is the transition matrix of the system at time k, G**k*

**is the discrete time gain at time k, u***k*is the input assumed to be constant over a sampling
**period, and v***k*is the discrete time process noise. Note that Equation 4.1 describes a
time-varying discrete time system.

*In most moving object tracking applications, the control input u**k**is unknown and there*
is no sensor measuring the control input directly. One approach for dealing with this
prob-lem is to directly estimate this unknown input using the measurements associated with
moving objects. However, (Fitzgerald, 1980) has showed that it is often not feasible to
obtain an accurate control input estimate if only positional information is available.

Instead, by assuming that a moving object behaves according to one of a finite num-ber of models, multiple model approaches are applied widely and successfully in many maneuvering object tracking applications. In this section, we will describe two of the basic motion models, the constant velocity model and the constant acceleration model.

**The Constant Velocity Model**

The constant velocity (CV) model is shortened from the the piecewise constant white noise constant velocity model, which assumes that the control input of the system can be described as white noise. In other words, the acceleration of the moving object is modelled as white noise.

As described in (Bar-Shalom and Li, 1995), the state equation for this model is:

**o***k+1* **= F o***k***+ Γv***k* (4.2)

**where the process noise v***k*is a zero-mean white acceleration sequence.

Let the Cartesian state vector of the system be:

**o = [x, v***x**, y, v**y*]* ^{T}* (4.3)

The time-invariant state transition matrix is

*F* =

· *F**x* **0**
**0** *F**y*

¸

, *F**x**= F**y*=

· 1 *t*
0 1

¸

(4.4)

4.3 MOVING OBJECT MOTION MODELLING

*where t is the sampling period.*

**Assuming v**_{k}*is constant during the k-th sampling period t, the increases in the *
**veloc-ity and the position are v***k***t and v***k**t*^{2}*/2 respectively. Therefore, the noise gain is computed*
by:

Γ =

· Γ*x*

Γ*y*

¸

, Γ*x*= Γ*y*=

· _{1}

2*t*^{2}
*t*

¸

(4.5)

The covariance of the process noise multiplied by the gain is
*Q =*

· *Q**x* **0**
**0** *Q**y*

¸

(4.6)

where

*Q**x* *= Q**y* *= E[Γv**k**v**k*Γ* ^{0}*]

=

· _{1}

4*t*^{4} ^{1}_{2}*t*^{3}

1
2*t*^{3} *t*^{2}

¸

*σ*^{2}* _{v}* (4.7)

*It is suggested that σ**v*should be of the order of the maximum acceleration magnitude
*a**M* for the constant velocity model.

**The Constant Acceleration Model**

The constant acceleration (CA) model is shortened from the piecewise constant white
noise acceleration model in which the system is assumed to perform accelerations that are
constant over a sampling interval. In this model, the acceleration increment is assumed to
be zero-mean white sequence. The state equation of this model is the same as Equation 4.2
**but the white process noise v***k*is the acceleration increment.

Let the Cartesian state vector of the system be:

**o = [x, v***x**, a**x**, y, v**y**, a**y*]* ^{T}* (4.8)
The transition matrix is described as:

*F* =

· *F**x* **0**
**0** *F**y*

¸

, *F**x**= F**y*=

1 *t* ^{1}_{2}*t*^{2}

0 1 *t*

0 0 1

(4.9)

The noise gain is

Γ =

· Γ*x*

Γ*y*

¸

, Γ* _{x}*= Γ

*=*

_{y}

1
2*t*^{2}

*t*
1

(4.10)

The covariance of the process noise multiplied by the gain is
*Q =*

· *Q**x* **0**
**0** *Q**y*

¸

(4.11)

where

*Q**x* *= Q**y* *= Γσ*_{v}^{2}Γ^{0}

=

1

4*t*^{4} ^{1}_{2}*t*^{3} ^{1}_{2}*t*^{2}

1

2*t*^{3} *t*^{2} *t*

1

2*t*^{2} *t* 1

* σ*^{2}* _{v}* (4.12)

*For the CA model, it is suggested that σ**v*should be of the order of the magnitude of
*the maximum acceleration increment over a sampling period, ∆a**M* .

**The IMM algorithm with the CV and CA Models**

In this section, we will illustrate the performance and the limitation the IMM algo-rithm with the CV and CA models using synthetic data. In all simulations, the sensor plat-form is assumed to be stationary and the moving objects are described by point-features, i.e. no orientation information. The standard deviation of the additive noise in the percep-tion measurements is 0.1 meter.

**Case 1: a constant acceleration maneuver. In this simulation, the initial velocity of the**
*tracked object is 5 m/s and its acceleration is 10 m/s*^{2}. Figure 4.2 shows the tracking
re-sults, the ground truth and the measurements and the performance is satisfying. Figure 4.3
shows the velocity estimates and the probabilities of the CV model and the CA model
dur-ing trackdur-ing. As expected, the CA model dominates the whole trackdur-ing process accorddur-ing
to the probabilities.

10 20 30 40 50 60

0 5 10

x (meter)

y (meter)

true observed filtered

56 56.5 57 57.5 58 58.5 59 59.5 60

4 5 6

x (meter)

y (meter)

**Figure 4.2.** A simulation of the
constant acceleration
maneu-ver. Top: the whole trajectory.

Bottom: a portion of the trajec-tory is enlarged for clarifica-tion.

0 0.5 1 1.5 2 2.5 3

−20 0 20 40

Speed (meter/s)

Time (sec)

true Vx true Vy Vx Vy

0 0.5 1 1.5 2 2.5 3

0.2 0.4 0.6 0.8 1

Porb

Time (sec)

CV Prob CA Prob

**Figure 4.3.** The velocity
esti-mates and the probabilities of
the CV model and the CA
model in the constant
accel-eration maneuver simulation.

Top: the velocity estimates.

Bottom: the probabilities of the CV model and the CA model.

4.3 MOVING OBJECT MOTION MODELLING

**Case 2: a constant velocity maneuver. In this simulation, the velocity of this tracked**
*object is 15 m/s and its acceleration is 0 m/s*^{2}. Figure 4.4 and Figure 4.5 show the simulation
results. At the beginning, the filtered state was overshooting and oscillating. After about
1 second, the system was stably tracked. According the probabilities of the CV model
and the CA model, the CV model did not strongly dominate the tracking process. The
probability difference between the CV model and the CA model is not as big as the constant
acceleration maneuver example. This is because of the model complexity issues, which will
be discussed in Section 4.5.

10 20 30 40 50

0 5 10

x (meter)

y (meter)

true observed filtered

45 45.5 46 46.5 47 47.5 48 48.5 49

4 5 6

x (meter)

y (meter)

**Figure 4.4.** A simulation of the
constant velocity maneuver.

Top: the whole trajectory. Bot-tom: a portion of the trajectory is enlarged for clarification.

0 0.5 1 1.5 2 2.5 3

−20

−10 0 10 20

Speed (meter/s)

Time (sec)

true Vx true Vy Vx Vy

0 0.5 1 1.5 2 2.5 3

0 0.5 1

Porb

Time (sec)

CV Prob CA Prob

**Figure 4.5.** The velocity
esti-mates and the probabilities of
the CV model and the CA
model in the constant velocity
motion simulation.

**Case 3: a turning maneuver. In this simulation, the speed of the tracked object is 15**
*m/s and the angular velocity is 18 degree/s. Figure 4.6 and Figure 4.7 show the satisfying*
results even the turning maneuver is neither the CV maneuver nor the CA maneuver. The
*CV and CA models assume that the motion of the system is independent in the x and y*
directions and the filter can be decoupled with respect to these axes. In the cases that a
moving object performs a coordinated turn, the motion is highly correlated across these
directions and the coordinated turning model can be used for getting better performance
and maneuver classification.

**Case 4: a move-stop-move maneuver. This simulation demonstrates a more **
compli-cated maneuver which consists of the following motion patterns in order: constant velocity,
constant deceleration, stop, constant acceleration and constant velocity. This maneuver is
one of the representative maneuvers for ground moving objects in urban environments. It
seems that Figure 4.8 and Figure 4.9 show the satisfying results.

5 10 15 20 25 30 35 40 5

10 15

x (meter)

y (meter)

true observed filtered

35 36 37 38 39 40

17 17.5 18 18.5 19

x (meter)

y (meter)

**Figure 4.6.** A simulation of the
turning maneuver. Top: the
whole trajectory. Bottom: a
portion of the trajectory is
en-larged for clarification.

0 0.5 1 1.5 2 2.5

0 5 10 15 20

Speed (meter/s)

Time (sec) true Vx

true Vy Vx Vy

0 0.5 1 1.5 2 2.5

0.2 0.4 0.6 0.8 1

Porb

Time (sec)

CV Prob CA Prob

**Figure 4.7.** The velocity
esti-mates and the probabilities of
the CV model and the CA
model in the turning maneuver
simulation.

10 20 30 40

0 5 10

x (meter)

y (meter)

true observed filtered

25 26 27 28 29 30

4.5 5 5.5

x (meter)

y (meter)

**Figure 4.8.** A simulation of the
move-stop-move maneuver.

Top: the whole trajectory. Bot-tom: a portion of the trajectory is enlarged for clarification.

0 1 2 3 4 5

−10 0 10 20

Speed (meter/s)

Time (sec)

true Vx true Vy Vx Vy

0 1 2 3 4 5

0 0.5 1

Porb

Time (sec)

CV Prob CA Prob

**Figure 4.9.** The velocity
esti-mates and the probabilities of
the CV model and the CA
model in the move-stop-move
maneuver simulation.

However, due to uncertainty in the measurements, the velocity estimate did not go to zero when the tracked object was stationary. Figure 4.8 indicates that the estimates from the IMM with CV and CA models moved around the true stationary location instead of converging to the true stationary location. Figure 4.10 shows a simulation in which a stationary object is tracked using a Kalman filter with the CV model. The filtered result overshoots and converges slowly. For SLAM with generic objects, such overshoots and slow convergence degrade inference and learning of the whole process. We will discuss this issue further in the next section.