12 IMM-UKF Algorithm and IMM-EKF Algorithm For
12 IMM-UKF Algorithm and IMM-EKF Algorithm For
discussions, stats, and author profiles for this publication at: https://www.researchgate.net/publication/262270675
CITATIONS READS
0 72
3 authors, including:
Some of the authors of this publication are also working on these related projects:
All content following this page was uploaded by Daoud Berkani on 30 May 2016.
The user has requested enhancement of the downloaded file. All in-text references underlined in blue are added to the original document
and are linked to publications on ResearchGate, letting you access and read them immediately.
3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ 7
&RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH
convenient sizes. The continuous time state equation (2) is The output equations are as follows (trajectory parameter are
linear time invariant and independent of M’s trajectory, except considered time-invariant):
on the sizes of s ( .) and p ( .) . Moreover, it may be shown
Straight Line: z (k ) = x(k ) = d cos α + s (k ) sin α + v(k ) (3)
∆
that the fundamental matrix F involved its exact discretization y(k ) d sinα − s(k ) cos α
at the period T takes the form
( AT )
i
∆
F = exp ( AT ) = ∑ i =0
n −1
. s (k )
i! Circle:
∆ x ( k ) x0 + R 0 cos R 0 (4)
z ( k ) = = + v( k )
The dynamic and measurement noises are supposed to be
y( k ) y 0 + R 0 sin s( k )
stationary, white and Gaussian, non inter-correlated with R0
known covariances.
with s(k ) distance covered by the target and v(⋅) zero mean
A. Canonical motion equations Gaussian noise, E {v (k) } = 0, E {v (k) ⋅ v T (k) } = R δ k , k ′
The point M is supposed to move on straight or circular
trajectories at constant or uniformly time-varying speed 2) State Equations
(constant speed or constant acceleration). Those motions
1 T
belong to the set of the possible behaviours of a non- s(k +1) = s(k ) + w s (k )
holonomic robot whose wheels are driven at constant Constant velocity 0 1 (5)
velocities or accelerations. p(k + 1) = p(k ) + wp (k )
1) Output equations: One minimal description of a straight
line is defined by the vector p= (α ,d ) shown in figure 1(a),
T
Constant acceleration
which is related to Plucker coordinates. Concerning a circular
trajectory one minimal description is defined by the T2
vector p = ( R0, x0 , y0 ) Τ shown in figure 1(b). The origin of 1 T
2
curvilinear abscissa is uniquely defined from those
s( k + 1) = 0 1 T s ( k ) + w s (k ) (6)
parameterizations.
0 0 1
p(k + 1) = p(k ) + wp (k )
d s=0
s
α where the random vectors
0 Direction of
(
x(0) = s(0) , p(0)
T T T
) , w(⋅) = (w (⋅) , w (⋅) )
s
T
p
T T
and v(⋅)
M increasing are jointly Gaussian and:
(a) Line E{x(0)} = x̂(0), E{w( k )} = 0, E{v ( k )} = 0,
x(0) − x̂(0) x(0) − x̂(0) T
s0
E w( k ) w( k ′) = diag (P (0 ), Qδ k ,k ′ , Rδ k ,k ′ )
v( k ) v ( k ′)
M
s ( k )
and s( k ) = s( k ) vector of the point M dynamics
y0 + s=0
s(k )
0
x0 R0 p(k) trajectory parameters vector
A. The Unscented Transform At first the state vector is augmented with the process and
According to Julier and Uhlmann in [5, 6], the unscented noise terms this leads to an na = n + q dimensional vector.
transform is based on the intuition that it is simpler to x (k )
approximate a Gaussian distribution than it is to approximate x a (k ) = (11)
an arbitrary nonlinear function. A set of vectors, selected to be w (k )
representative of the probability distribution, are chosen so The process model is now a function of x a ( k ) ,
that their mean and covariance are respectively x and P .
xx
The nonlinear function is applied to each point; the result is a x ( k + 1) = f x a ( k ) , u ( k ) , k (12)
set of transformed points with the statistics y and Pyy . The
selection of these vectors is not arbitrarily done but according The unscented transform needs to 2n a +1 vectors which are
to a deterministic algorithm. selected from:
The n-dimensional random variable x with mean and
covariance respectively x and P is approximated by 2n+1
xx x̂ ( k k )
x̂ a ( k k ) =
weighted points given by: 0
q x1
χ0 = x κ and (13)
W0 =
(n + κ ) Ρ(k k) Ρxw (k k)
Ρa (k / k) = .
χi = x + ( (n + κ ) Ρ xx ) i Wi =
1 i = 1......n (7) Ρxw (k k) Q(k)
2(n + κ )
(
χi+n = x − (n + κ )Ρxx ) i Wi + n =
1 The Kalman filter algorithm computes on two principles
phases:
2(n + κ )
- Prediction of the state vector and its covariance matrix
- Estimation of the state vector and its covariance matrix
Where κ ∈R, ( ( n+κ ) Pxx )i is the i th row or column of the by updating the prediction with the current
measurement.
matrix square root of ( n+κ ) Pxx and Wi is the weight which
2n The prediction phase using the unscented transform is as
is associated with the i th point, note that ∑ Wi = 1 .
i =0 follow:
The transformation procedure occurs in three steps:
1. The set of vectors are created by applying equation (7)
1. The transformed set of vectors are: yi = f ( χ i ) (8) to the augmented system given by equation (13)
2n
2. The transformed set of vectors are given by
2. The mean is given by: y = ∑Wi yi (9) χi ( k + 1 k ) = f χιa ( k k ) , u ( k ) , k (14)
0
3. The covariance is given by : 3. The predicted mean is calculated by
2 na
x̂ ( k + 1 k ) = ∑ Wi χιa ( k + 1 k )
2n
Ρyy = ∑ Wi {yi − y}{yi − y} Τ (10) (15)
i =0
i =0
4. The predicted covariance is calculated by
According to Julier and Uhlmann [5,6], many interesting 2na
properties could be noticed about this algorithm, for instance it Ρ((k + 1 k ) = ∑Wi {χ i (k + 1 k ) − x̂(k + 1 k )}⋅ (16)
leads to greater accuracy and permit rapid implementation, i =0
thanks to the mean and covariance which are calculated using {χ (k + 1 k) − x̂(k + 1 k)} Τ
i
standard vector and matrix operations. 5. Each prediction vector is instantiate through the
B. The Unscented Kalman Filter measurement model
The unscented Kalman filter is obtained by occurring little
modifications on standard one; this, by restructuring the state zi (k +1k) = h (χi (k +1k),u(k),k) (17)
vector and process and measurement models [5, 6]. 6. The predicted observation is calculated by
Consider the following discrete-time nonlinear dynamical 2na
x̂(k +1k +1) = x̂(k +1k) + K(k +1)γ (k +1) (21) - µ i ( k − 1) the probability that the mode Mi is in effect
Τ
Ρ(k +1k +1) = Ρ(k +1k) − K(k +1)Ργγ (k +1k)K (k +1) at time k-1 ,
with γ ( k + 1) the innovation and K(k+1) the weight chosen to r
r
x̂0j(k −1/ k −1) = ∑x̂i (k − 1/ k −1)µi j (k − 1/ k −1) (24) The problem here is that this algorithm is designed with the
i =1 assumption that the target motion models are linear, so, for its
i, j =1,...,r optimality, the use of the Kalman filter is recommended.
the covariance corresponding to the above is However, in the case on which the motion models are
nonlinear we propose to use the UKF detailed in section III.
r
The modification to operate on the IMM algorithm is to use as
Ρ 0j ( k − 1 / k − 1) = ∑µ i j ( k − 1 / k − 1) ⋅
state and covariance estimator the UKF.
i =1 (25)
Thanks to this modification we hope to track accurately targets
i i
[ 0j
Ρ ( k − 1 / k − 1) + x̂ ( k − 1 / k − 1) − x ( k − 1 / k − 1) ⋅ ] whose models are nonlinear, the resulting algorithm is called
the Nonlinear Interacting Multiple Model and noted (IMM-
[x̂ i
( k − 1 / k − 1) − x 0j ( k − 1 / k − 1) ] Τ
i , j = 1 .... r UKF).
3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ 7
&RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH
V. FUZZY-IMM-UKF ALGORITHM
To resolve the problem of data association, we propose to
use a Fuzzy approach. Fuzzy data association (FDA) is used to
evaluate the association degree between the return and the
actual of interest. Inputs are the measurement innovation.
The association degree is β ijF (k ) = FDA(θ , A, Φ ) ,
where FDA(.) is a fuzzy function based on the set of
membership functions A and the set of propositions Φ [10],
using fuzzy logic methodology, the rules in the FDA rule base
are evaluated. The FDA rules to determine the degree of Association weight
association are provided in table 1, like in [10].
The fuzzy variable, FDA, has 49 If-Then rules; in table 1, the Fig.2. FDA output membership functions
fuzzy term set of input variables are given by
{NL,NM,NS,Z,PS,PM,PL}, where NL, NM NS, Z, PS, PM,
and PL denote Negative Large, Negative Medium, Negative VI. SIMULATIONS AND RESULTS
Small, Zero, Positive Small, Positive Medium, and Positive In this section, we perform some simulations to evaluate
Large, respectively. The fuzzy term set of output variables are our algorithm (Fuzzy-IMM-UKF).
{VL, ML, M, MH, VH}, which denote Very Low, Medium The motion models considered are: - constant velocity on
Low, Medium, Medium High, and Very High, respectively. straight line (M1), -constant acceleration on straight line (M2),
- constant velocity on circle (M3), - constant acceleration on
circle (M4).
X coordinate error
Association degree To explore the capability of our Fuzzy-IMM-UKF
algorithm to track maneuvring targets, various scenarios are
NL NM NS Z PS PM PL considered; among of them we select the typical case of three
NL VL VL ML M ML VL VL highly maneuvring targets with crossing trajectories.
Y
coordinate error NM VL ML ML M ML ML VL
We assume that the target is in a 2-D space and its position is
NS ML ML M MH M ML ML sampled every T=1s. For all cases we assume:
Z M M MH VH MH M M - The measurement noise is zero mean, white, independent
PS ML ML M MH M ML ML of the process noise, and with variance σv2=0.01.
PM VL ML ML M ML ML VL - The process noise is zero mean, white, independent of
PL VL VL ML M ML VL VL the measurement noise, and with variance σw2=0.001.
- The initial condition with the state
For the FDA configurations, the sets of membership X =[s , s , s, α , d , R 0, x0 , y0] is
functions which fuzzify these inputs, defined over appropriate
- The probability transition matrix of four models is
universe of discourse, are shown in Fig.1. The output of the
FDA membership functions, which are used to evaluate the
outputs through the application of fuzzy inference (via max- 0.97 0.01 0.01 0.01
0.01 0.97 0.01 0.01
min rule) and center of gravity defuzzification [11], are shown
p =
in fig.2. 0.01 0.01 0.97 0.01
0.01 0.01 0.01 0.97
A. Considered scenario
a) Target 1(black):
The target starts moving according to model M1 until the 1
0.6
th
50 sample when an abrupt trajectory change occur and still 0.3
0
0 20 40 60 80 100 120
c) Target 3 (green): T im e S te p
The target starts moving according to model M1 until the Fig.6. Models Probabilities 3
50th sample when an abrupt acceleration about 0.2 m/s2 occur
and still moving according to this during 50 samples
(switching from model M1 to M2). 25
target 1
40
target 1
target 2 target 2
target 3 target 3
70 35
30
60
25
15
50
20
y axis
10
40 15
10
30 5
x axis
Fig.3. Real and Esteemed Trajectories Fig.7. RMS x and y position error
4.5 0.05
1
target 1 target 1
M1
target 2 target 2
M2
4 target 3 0.045 target 3
0.9 M3
M4
0.04
0.8 3.5
m o de ls p ro ba bilities o f ta rge t 1
0.035
0.7
3
R M S S pee d E rro r
0.03
0.6
2.5
0.025
0.5
2
0.02
0.4
1.5
0.015
0.3
1
0.01
0.2
0.5
0.005
0.1
0 0
0 0 10 20 30 40 50 60 70 80 90 100 0 10 20 30 40 50 60 70 80 90 100
0 20 40 60 80 100 120
Time Step
T im e S te p Time Step
Figure 3 shows that the esteemed and the real trajectory for
0.9 M3
M4
T im e S te p
with the IMM-UKF algorithm (Fuzzy-IMM-UKF) would be
Fig.5. Models Probabilities 2
3URFHHGLQJVRIWKHWK0HGLWHUUDQHDQ&RQIHUHQFHRQ 7
&RQWURO $XWRPDWLRQ-XO\$WKHQV*UHHFH
VII. CONCLUSION
The model-based body motion estimation by using data
coming from visual sensors still an open problem on which we
try to provide a contribution. In this paper we presented a
nonlinear algorithm which attempts to track efficiently a
highly maneuvering target whose trajectory and/or dynamic
could change abruptly, the algorithm proposed is noted IMM-
UKF. To extend this algorithm to multi-target case, we
combined the later with the FDA algorithm to ensure good
data association.
Simulations show that the Fuzzy-IMM-UKF is a good
investment while we are asked to track a highly maneuvrable
targets whose measurement and/or state models present a
strong nonlinearities, and when there different trajectories
cross each other.
REFERENCES
[1] Y.Bar-Shalom and T.E.Fortman, “Tracking and Data Association,”
Mathematics in Science and Engineering, volume 179, Academic Press,
1988.
[2] Y.Bar-Shalom and X. R. Li, “Estimation and Tracking, Principles,
Techniques and Software,” Artech House, Boston, MA (USA), 1993.
[3] B.Espiau, F.Chaumette, and P.Rives, “ A new approach to visual
servoing in robotics,”IEEE Transactions on Robotics and Automation,
8(3):313-326, June 1992.
[4] P.Danes, M.S.Djouadi, D.Bellot, “A 2-D Point-Wise Motion Estimation
Scheme for Visual-Based Robotic Tasks,” 7th International Symposium
on Intelligent Robotic Systems (SIRS’99), Coimbra (Portugal), 20-23
July 1999, pp.119-128
[5] S.J. Julier, and J.K. Uhlmann, “Unscented filtering and nonlinear
estimation,” Proceedings of IEEE, volume: 92, Issue: 3, March 2004,
pages: 401-422.
[6] S.J. Julier, and J.K. Uhlmann, “A New Extension of the Kalman Filter to
Nonlinear Systems,” In International Symposium on Aerospace/Defense
Sensing, Simulation and Control, Orlando, FL, 1997.