Real time implementation of unscented kalman filter for target tracking

INTERNATIONAL JOURNAL OF ELECTRONICS AND
International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN
0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEME
COMMUNICATION ENGINEERING & TECHNOLOGY (IJECET)
ISSN 0976 – 6464(Print)
ISSN 0976 – 6472(Online)
Volume 4, Issue 1, January- February (2013), pp. 208-215
IJECET
© IAEME: www.iaeme.com/ijecet.asp
Journal Impact Factor (2012): 3.5930 (Calculated by GISI) ©IAEME
www.jifactor.com

REAL TIME IMPLEMENTATION OF UNSCENTED KALMAN
FILTER FOR TARGET TRACKING

1 2
Ravi Kumar Jatoth , Dr.T.Kishore Kumar
1
Assistant Professor, Department of ECE, National Institute of Technology-Warangal,
INDIA
2
Assocaite Professor, Department of ECE, National Institute of Technology-Warangal,
INDIA

ABSTRACT

This paper presents the nonlinear state estimation using unscented Kalman filter
simulated in SIMULINK. UKF is an extension of EKF which has been successfully used in
many nonlinear applications. But, the performance is limited due to the truncation of all but
first order terms. As most of the real time problems are nonlinear in nature here we use UKF
which can achieve greater estimation performance than EKF. This is possible as UKF uses
Unscented transform through which first and second order terms of nonlinear system can be
captured. In this paper as we simulated UKF in SIMULINK it is almost equal to the real time
model and can be implemented on the DSP processor.

Keywords : Unscented Kalman Filter, Non linear Estimation, Target tracking, SIMULINK

I. INTRODUCTION

The problem of nonlinear state estimation concerns the task of estimating the state of
a system from noisy sensor information. When system dynamics and observation models are
linear, Kalman filter can be used as a state estimator. However, most applications are
nonlinear and suitable extensions to Kalman filter have been sought. The optimal solution to
nonlinear filtering problem requires a complete description of conditional probability density
[1]. The most widely used estimator for nonlinear systems is the extended Kalman filter
(EKF). In this method the equations are linearized about a trajectory that is continually
updated with the state estimates resulting from the measurements [7]. This linearization
however poses some problems e.g. it can produce highly unstable filters if the assumptions of
local linearity is violated [1]. In this paper we simulate UKF (estimator) which generalises

208


sophisticatedly to nonlinear systems without the linearization steps required by the EKF[1]. The UKF
uses deterministic sampling approach [6]. Approximating a Gaussian distribution is easier than
approximating a nonlinear transformation so state distribution is approximated by a Gaussian random
vector. But, it is represented using a minimal set of carefully chosen sample points. True mean and
covariance of Gaussian random vector are completely captured by these sample points. They can
capture the posterior mean and covariance accurately to the second order when propagated through
the nonlinear easurement equation[6].

II. PROBLEM STATEMENT

Nonlinear state estimation we wish to use Unscented Kalman filter for the estimation of state of
discrete time nonlinear dynamic system
x(k+1)=f(x(k),w(k)) (1)
z(k)=h(x(k),v(k)) (2)
where, x(k) represents the unobserved state of the system
and z(k) is the only observed signal.
The process noise w(k) drives the dynamic system and the observation noise is given by v(k). It is
assumed that the noise vectors w(k) and v(k) are zero-mean Gaussian with covariances Qk and Rk
respectively. The system dynamic model f and h are assumed to be known [2].

Figure1. Tracking of Target motion by means of RADAR

III. IMPLEMENTATION OF TARGET TRACKING IN SIMULINK

The random motion of aircraft and tracking algorithm implemented in SIMULINK is as shown
below

Figure 2. RADAR tracking using embedded MATLAB

209


A. Random aircraft motion
This block is responsible for generation of the random force and is obtained by using a band
limited white Gaussian noise block. This is basically the force applied to the system which is
varying continuously. This allows the user to choose the noise power and the seed for the random
force.
B. Acceleration Model
The acceleration model is a subsystem composed of summer, integrators and gain blocks. Here
we assume a vehicle moving on a horizontal surface with a constant velocity and varying force.
The acceleration of the vehicle is the sum of forces acting on the vehicle divided by the mass of
the vehicle.
dv
=
F − bv (15)
dt M
Where
F is the force created by the vehicle’s engine to propel it forward.
B is the damping coefficient
V is the horizontal velocity of vehicle.
M is the mass of the vehicle.

Figure 3. Acceleration model of aircraft

Another block of this system is the random measurement noise based on band limited white
Gaussian noise block and gain block. The output of noise block is multiplied by the matrix
formed from the square root of the noise covariances.
The x-y to range bearing block is used to convert the measurements from x-y coordinates to polar
coordinates. As we get range and bearing measurements which are similar to the measurements
from RADAR and thus nearer to real time system. The range and bearing of the of the target from
the radar measurement can be obtained by
2 2
r = xk + yk (16)
θ = tan( yk / xk ) (17)

Figure 4. Cartesian to polar coordinates conversion block

210


IV. UNSCENTED KALMAN FILTER.

The UKF is a recursive minimum-mean square-error (MMSE) estimator. It is based
on the unscented transform (UT). The UT is a method for calculating the statistics ofa
random variable, which undergoes a nonlinear transformation [3]. State distribution is
approximated by Gaussian random vector and is represented by a set of deterministically
chosen sample points called sigma points, which completely capture the true mean and
covariance of the distribution. High order information about the distribution can be captured
using only a very small number of points as problems of statistical convergence are not an
issue [1]. Using UT, UKF captures the mean and covariance in the prior and posterior
densities accurately [4]. It is easier to approximate a probability distribution than an arbitrary
nonlinear function, so in UKF the nonlinear functions of system and measurement models are
not approximated as in the EKF. Instead, sigma points are propagated through the nonlinear
functions to yield transformed samples, and the propagated mean and covariance are
calculated from the transformed samples.
Let L-dimension state vector ^x- k-1 with mean ^x- k-1|k-1 and covariance P k-1|k-1 be
approximated by 2L+1 weighted samples or sigma points. Then one cycle of the UKF is as
follows [5].

Sigma point calculation: Compute the (2L+1) sigma points as follows:

λ = α 2 (L + κ ) − L (3)
W0m = λ /(L + λ),
X ki −1|k −1 = xk −1|k −1 + ( (L + λ)Pk −1|k −1 )i ,
ˆ
X k0−1|k −1 = x k −1|k −1 , W m = 1/ 2(L + λ),i = 1,...,L,
ˆ
i

X ki+1|k −1 = xk −1|k −1 − ( (L + λ)Pk −1|k −1 )i,
−
L
ˆ
m
Wi +L = 1/ 2(L + λ),i = 1,...L,
W0c = W0m + (1 − α 2 + β )
Wi m = Wi c , i = 1,....,2L

Where α determines the spread of sigma points around the mean and is usually set to a small
positive value,κ is a secondary scaling parameter which is usually set to 3-L , β is used to
incorporate prior knowledge of distribution of x and ( ( L + λ ) Pk −1|k −1 ) is the ith row or column
i

(depending on the matrix square root form, if P = AT A then the sigma points are formed from
the rows of A. However, if the matrix square root is of the form P = AAT , the columns of A are
used) of the matrix square root of ( L + λ ) Pk −1|k −1 and Wi is the normalized weight associated with
the ith point. Note that Cholesky decomposition is needed for the matrix square root.

Propagation: Propagate the sigma points and obtain the mean and covariance of the state by
i i
X k |k −1 = f ( X k −1|k −1 ), (4)
2L
xk | k −1 = ∑Wi m X ki | k −1 ,
ˆ (5)
i =0
2L
[ ] [
Pk |k −1 = Qk −1 + ∑Wi c X k |k −1 − xk |k −1 × X ki |k −1 − xk |k −1
i
ˆ ˆ ]T
(6)
i =0

211


i
Update: Calculate the measurement sigma points Z k |k −1 using h(.) and update the mean and
Covariance by
Z ki |k −1 = h ( X i
k |k − 1 ), (7)

2L
zk | k −1 = ∑Wi m Z ki | k −1
ˆ (8)
i=0
~
vk = z k − z k |k −1 ,
ˆ (9)
ˆ ˆ ~
xk |k = xk |k −1 + K k vk , (10)
T
Pk |k = Pk |k −1 − K k Pzz K , k (11)
2L
[ ] [
Pzz = Rk + ∑Wi c Z k |k −1 − zk |k −1 × Z ki |k −1 − zk |k −1 ,
i
ˆ ˆ ] T
(12)
i =0
2L
[ ] [
Pxz = ∑Wi c X ki |k −1 − xk |k −1 × Z k |k −1 − zk |k −1
ˆ i
ˆ ]
T
(13)
i =0
−
K k = Pxz Pzz 1 (14)

The Jacobian matrices are not required to implement this algorithm. The other advantage of the UKF
over the EKF is that it can estimate the mean and covariance of the state accurately to second order
for any nonlinearity.

V. SIMULATIONS AND RESULTS

Here we are considering a random vehicle motion with an initial position Px=0, Py=0 and
initial velocity Vx=0, Vy=400ft/sec; the thrust acceleration in Y-direction is 4 ft/sec2 and cross axis
acceleration in X- direction is 5 ft/sec2.The simulation time for problem is 100s. The results are shown
for following inputs
x = [0; 0; 0; 400];P = diag([0.03 0.003 0.01 .001]);Q = diag([0.03 0.003 0.01 .001]);
R = diag([10^2 0.005^2]);
The simulations results are shown below.

Figure 5. Polar plot of actual and estimated trajectory

212


From the above plot it’s observed that estimated trajectory is almost coinciding with the
actual trajectory.

Figure 6. Vehicle motion in the X-Y direction

The above figure shows that the filter is able to track the position of the vehicle almost very
close to the actual path followed by the vehicle. It is also better when compared to the
measured position.

Figure 7. Error in X direction

Figure 8. Error in Y direction

The error is observed in both X and Y directions. It’s observed that error in Y direction
converges with time.
213


Figure 9. Estimation Residual for Range

Figure9 is a plot of the residual for range .From this we can say that range error is decreasing
with number of measurements.

VI. CONCLUSIONS

This paper described the SIMULINK’s implementation of a tracking algorithms,
where the performance evaluation of the UKF algorithm was aimed. The use of SIMULINK
greatly reduced the time spent to achieve this goal, due to its graphical programming
philosophy and flexibility to simulate complex systems. In this paper, we not only
implemented UKF but also the other tracking algorithms like Standard Kalman filter,
Extended Kalman Filter and compared in terms of directional errors and Computational
Complexity as shown in Table below.

Error in X Error in Y Computational
Filter direction direction complexity
(feet) (feet)

Kalman filter 2.782773 379.3095 17.757795 Sec

Extended Kalman 32.8505 142.324 8.357854 Sec.
filter

Unscented
Kalman filter 31.80451 142.252 6.208784 Sec.

Once the UKF is modelled in SIMULINK it is easy to implement on DSP processors

214


REFERENCES

[1] J. Julier, Jeffery K. Uhlmann, “A New Extension of the Kalman Filter to Nonlinear
System,” Signal Processing, sensor fusion, and target recognition VI,1997.
[2] E.A. Wan, R. Van der Merwe, “The Unscented Kalman Filter for nonlinear estimation,”
IEEE 2000.
[3] Simon J. Julier, Jeffery K. Uhlmann, “Unscented filtering and nonlinear estimation,”
Proc. IEEE 92 (3) March 2004.
[4] Sy-Miin Chow, Emilio Ferrer, John R. Nesselroade, “An Unscented Kalman Filter
Approach to the Estimation of Nonlinear Dynamical Systems Models,” unpublished.
[5] Michail N. Petsios, Emmanouil G. Alivizatos, Nikolaos K. Uzunoglu, “Manoeuvring
target tracking using multiple bistatic range and range-rate measurements”, Science
Direct, Signal Processing 87(2007) 665-686.
[6] Zhansheng Duan, X. Rong Li, Chongzhao Han, Hongyan Zhu, “Sequential Unscented
Kalman Filter for Radar Target Tracking with Range Rate Measurements,” 2005 7th
International Conference on Information Fusion (FUSION).
[7] R.G. Brown, P.Y.C. Hwang, “Introduction to Random Signals and applied Kalman
Filtering,” third ed., Prentice Hall1997.

215

Real time implementation of unscented kalman filter for target tracking

Recomendados

Recomendados

Mais conteúdo relacionado

Mais procurados

Mais procurados (20)

Semelhante a Real time implementation of unscented kalman filter for target tracking

Semelhante a Real time implementation of unscented kalman filter for target tracking (20)

Mais de IAEME Publication

Mais de IAEME Publication (20)

Real time implementation of unscented kalman filter for target tracking