Your SlideShare is downloading. ×
Real time implementation of unscented kalman filter for target tracking
Upcoming SlideShare
Loading in...5
×

Thanks for flagging this SlideShare!

Oops! An error has occurred.

×
Saving this for later? Get the SlideShare app to save on your phone or tablet. Read anywhere, anytime – even offline.
Text the download link to your phone
Standard text messaging rates apply

Real time implementation of unscented kalman filter for target tracking

497
views

Published on


0 Comments
0 Likes
Statistics
Notes
  • Be the first to comment

  • Be the first to like this

No Downloads
Views
Total Views
497
On Slideshare
0
From Embeds
0
Number of Embeds
0
Actions
Shares
0
Downloads
18
Comments
0
Likes
0
Embeds 0
No embeds

Report content
Flagged as inappropriate Flag as inappropriate
Flag as inappropriate

Select your reason for flagging this presentation as inappropriate.

Cancel
No notes for slide

Transcript

  • 1. 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), © IAEMECOMMUNICATION 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.aspJournal Impact Factor (2012): 3.5930 (Calculated by GISI) ©IAEMEwww.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
  • 2. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEMEsophisticatedly to nonlinear systems without the linearization steps required by the EKF[1]. The UKFuses deterministic sampling approach [6]. Approximating a Gaussian distribution is easier thanapproximating a nonlinear transformation so state distribution is approximated by a Gaussian randomvector. But, it is represented using a minimal set of carefully chosen sample points. True mean andcovariance of Gaussian random vector are completely captured by these sample points. They cancapture the posterior mean and covariance accurately to the second order when propagated throughthe nonlinear easurement equation[6].II. PROBLEM STATEMENT Nonlinear state estimation we wish to use Unscented Kalman filter for the estimation of state ofdiscrete 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 systemand 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 isassumed that the noise vectors w(k) and v(k) are zero-mean Gaussian with covariances Qk and Rkrespectively. The system dynamic model f and h are assumed to be known [2]. Figure1. Tracking of Target motion by means of RADARIII. IMPLEMENTATION OF TARGET TRACKING IN SIMULINKThe random motion of aircraft and tracking algorithm implemented in SIMULINK is as shownbelow Figure 2. RADAR tracking using embedded MATLAB 209
  • 3. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEMEA. Random aircraft motionThis block is responsible for generation of the random force and is obtained by using a bandlimited white Gaussian noise block. This is basically the force applied to the system which isvarying continuously. This allows the user to choose the noise power and the seed for the randomforce.B. Acceleration ModelThe acceleration model is a subsystem composed of summer, integrators and gain blocks. Herewe 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 ofthe 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 aircraftAnother block of this system is the random measurement noise based on band limited whiteGaussian noise block and gain block. The output of noise block is multiplied by the matrixformed 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 polarcoordinates. As we get range and bearing measurements which are similar to the measurementsfrom RADAR and thus nearer to real time system. The range and bearing of the of the target fromthe 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
  • 4. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEMEIV. UNSCENTED KALMAN FILTER. The UKF is a recursive minimum-mean square-error (MMSE) estimator. It is basedon the unscented transform (UT). The UT is a method for calculating the statistics ofarandom variable, which undergoes a nonlinear transformation [3]. State distribution isapproximated by Gaussian random vector and is represented by a set of deterministicallychosen sample points called sigma points, which completely capture the true mean andcovariance of the distribution. High order information about the distribution can be capturedusing only a very small number of points as problems of statistical convergence are not anissue [1]. Using UT, UKF captures the mean and covariance in the prior and posteriordensities accurately [4]. It is easier to approximate a probability distribution than an arbitrarynonlinear function, so in UKF the nonlinear functions of system and measurement models arenot approximated as in the EKF. Instead, sigma points are propagated through the nonlinearfunctions to yield transformed samples, and the propagated mean and covariance arecalculated 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 beapproximated by 2L+1 weighted samples or sigma points. Then one cycle of the UKF is asfollows [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,....,2LWhere α determines the spread of sigma points around the mean and is usually set to a smallpositive value,κ is a secondary scaling parameter which is usually set to 3-L , β is used toincorporate 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 fromthe rows of A. However, if the matrix square root is of the form P = AAT , the columns of A areused) of the matrix square root of ( L + λ ) Pk −1|k −1 and Wi is the normalized weight associated withthe 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 iX k |k −1 = f ( X k −1|k −1 ), (4) 2Lxk | 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
  • 5. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEME iUpdate: Calculate the measurement sigma points Z k |k −1 using h(.) and update the mean andCovariance 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 UKFover the EKF is that it can estimate the mean and covariance of the state accurately to second orderfor any nonlinearity.V. SIMULATIONS AND RESULTS Here we are considering a random vehicle motion with an initial position Px=0, Py=0 andinitial velocity Vx=0, Vy=400ft/sec; the thrust acceleration in Y-direction is 4 ft/sec2 and cross axisacceleration in X- direction is 5 ft/sec2.The simulation time for problem is 100s. The results are shownfor following inputsx = [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
  • 6. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEMEFrom the above plot it’s observed that estimated trajectory is almost coinciding with theactual trajectory. Figure 6. Vehicle motion in the X-Y directionThe above figure shows that the filter is able to track the position of the vehicle almost veryclose to the actual path followed by the vehicle. It is also better when compared to themeasured position. Figure 7. Error in X direction Figure 8. Error in Y directionThe error is observed in both X and Y directions. It’s observed that error in Y directionconverges with time. 213
  • 7. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEME Figure 9. Estimation Residual for RangeFigure9 is a plot of the residual for range .From this we can say that range error is decreasingwith 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 SIMULINKgreatly reduced the time spent to achieve this goal, due to its graphical programmingphilosophy and flexibility to simulate complex systems. In this paper, we not onlyimplemented UKF but also the other tracking algorithms like Standard Kalman filter,Extended Kalman Filter and compared in terms of directional errors and ComputationalComplexity 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
  • 8. International Journal of Electronics and Communication Engineering & Technology (IJECET), ISSN0976 – 6464(Print), ISSN 0976 – 6472(Online) Volume 4, Issue 1, January- February (2013), © IAEMEREFERENCES[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