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:
ok+1 = Fkok+ Gkuk+ vk (4.1)
where okis the state vector at time k, Fkis the transition matrix of the system at time k, Gk
is the discrete time gain at time k, ukis the input assumed to be constant over a sampling period, and vkis 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 ukis 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:
ok+1 = F ok+ Γvk (4.2)
where the process noise vkis a zero-mean white acceleration sequence.
Let the Cartesian state vector of the system be:
o = [x, vx, y, vy]T (4.3)
The time-invariant state transition matrix is
F =
· Fx 0 0 Fy
¸
, Fx= Fy=
· 1 t 0 1
¸
(4.4)
4.3 MOVING OBJECT MOTION MODELLING
where t is the sampling period.
Assuming vkis constant during the k-th sampling period t, the increases in the veloc-ity and the position are vkt and vkt2/2 respectively. Therefore, the noise gain is computed by:
Γ =
· Γx
Γy
¸
, Γx= Γy=
· 1
2t2 t
¸
(4.5)
The covariance of the process noise multiplied by the gain is Q =
· Qx 0 0 Qy
¸
(4.6)
where
Qx = Qy = E[ΓvkvkΓ0]
=
· 1
4t4 12t3
1 2t3 t2
¸
σ2v (4.7)
It is suggested that σvshould be of the order of the maximum acceleration magnitude aM 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 vkis the acceleration increment.
Let the Cartesian state vector of the system be:
o = [x, vx, ax, y, vy, ay]T (4.8) The transition matrix is described as:
F =
· Fx 0 0 Fy
¸
, Fx= Fy=
1 t 12t2
0 1 t
0 0 1
(4.9)
The noise gain is
Γ =
· Γx
Γy
¸
, Γx= Γy=
1 2t2
t 1
(4.10)
The covariance of the process noise multiplied by the gain is Q =
· Qx 0 0 Qy
¸
(4.11)
where
Qx = Qy = Γσv2Γ0
=
1
4t4 12t3 12t2
1
2t3 t2 t
1
2t2 t 1
σ2v (4.12)
For the CA model, it is suggested that σvshould be of the order of the magnitude of the maximum acceleration increment over a sampling period, ∆aM .
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/s2. 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/s2. 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.