Optik124 (2013) 4468–4471
Contents lists available at ScienceDirect
Optik
j o u r n a l h o m e p a g e:w w w.e l s e v i e r.d e/i j l e
o
Unscented Kalman?lter and its nonlinear application for tracking a moving target Haitao Zhang?,Gang Dai,Junxin Sun,Yujiao Zhao
Guangzhou Haige Communications Group Incorporated Company,Guangzhou510663,PR China
a r t i c l e i n f o
Article history:
Received25August2012 Accepted11January2013
Keywords:
The extended Kalman?lter Unscented transform Unscented Kalman?lter a b s t r a c t
The extended Kalman?lter(EKF)is probably the most widely used estimation algorithm for nonlinear systems.However,more than40years of experience in the estimation community has shown that is dif?cult to implement,dif?cult to tune,and only reliable for systems that are almost linear on the time scale of the updates.To overcome these limitations,this paper proposes the unscented Kalman?lter (UKF).And the algorithms of the FEKF,SEKF and UKF are given.Furthermore,the state models and measurement models of a target are set up.For comparison purpose,the three algorithms is simulated for the target tracking,and the algorithm performance is analyzed and compared by the simulation results of FEKF,SEKF and UKF.Numerical results demonstrate that FEKF and UKF give almost identical results while the estimates of SEKF are clearly worse.The UKF is easier to implement,avoiding Jacobian and Hessian matrices computation.
? 2013 Elsevier GmbH. All rights reserved.
1.Introduction
In control and communication systems,noisy signals are fre-quently processed to achieve estimates of signals,states,or https://www.doczj.com/doc/c618452123.html,ually an effort is made to construct estimators which achieve an optimal estimate for some convenient criterion of opti-mality such as the minimum mean square error criterion.To solve it,one begins by modeling the evolution of the system and the noise in the measurement.The resulting models typically exhibit complex nonlinearities,thus precluding analytical solution.So,the unscented Kalman?lter(UKF)is presented,and has been proposed by Julier and Uhlmann[1,2].It is easier to approximate a Gaussian distribution than it is to approximate arbitrary nonlinear functions. Unlike the Extended Kalman Filter(EKF),which is based on the lin-earizing the nonlinear function by using Taylor series expansions [2,3].And the series approximations in the EKF algorithm can lead to poor representations of the nonlinear functions and probability distribution of interest.As a result,this?lter can diverge.
UKF uses the true nonlinear models and approximates a Gauss-ian distribution of the state random variable.Furthermore,it only needs a minimal set of carefully chosen sample points,by which the posterior mean and covariance can be accurate to the second order for any nonlinearity,avoiding Jacobian and Hessian matri-ces computation[4].If the priori random variable is Gaussian,the posterior mean and covariance could be accurate to the third order [5],whatever the nonlinearity is.As it gives a tradeoff between accurate state estimation,limited computational cost and ease of ?Corresponding author.
E-mail address:tianmen822@https://www.doczj.com/doc/c618452123.html,(H.Zhang).implementation[6],it has been shown that UKF is a superior alter-native to EKF in the state estimation and parameter estimation.
In this paper,the contribution is the performance of UKF and EKF.The considered EKF includes the?rst EKF(FEKF)and the second EKF(SEKF).The simulation results indicate that the computational complexity of UKF is smallest,and FEKF and UKF give almost iden-tical performances.
The paper is organized as follows.Section2gives UT and UKF algorithm.Section3describes the system dynamics and measure-ment models.In Section4,FEKF,SEKF and UKF algorithms are Simulate,and a comparative analysis of the three algorithms is presented.Section5summarizes the results.
2.Nonlinear?lter
This section would?rstly summarize the?ltering algorithms of UKF,FEKF and SEKF.
2.1.Unscented transform
The unscented transform can be used to provide a Gaussian approximation for the joint distribution of variables x and y of the form[7–9].
x
y
～N
m
U
,
P C U
C T
U
S U
(1)
The transformation is done as follows:
0030-4026/$–see front matter? 2013 Elsevier GmbH. All rights reserved. https://www.doczj.com/doc/c618452123.html,/10.1016/j.ijleo.2013.03.013
H.Zhang et al./Optik 124 (2013) 4468–4471
4469
https://www.doczj.com/doc/c618452123.html,pute the set of 2n +1sigma points from the columns of the matrix (n + )P :
x (0)=m
x (i )=m +
(n + )P
i ,
i =1,...,n
x (i )=m ?
(n + )P
i ,
i =n +1, (2)
(2)
And the associated weights:W (0)m = /(n + )
W (0)
c = /(n + )+(1??2+ˇ)
W (i )
m =1/ 2(n + ) ,
i =1, (2)
W (i )c
=1/
2(n + )
,
i =1, (2)
(3)
Parameter is a scaling parameter,which is de?ned as =?2(n +?)?n
(4)
The positive constants ?,ˇand ?are used as parameters of the method.
2.Propagate each of the sigma points through non-linearity as
y (i )=g (x (i )),
i =0, (2)
(5)
3.Calculate the mean and covariance estimates of y as U ≈
2n i =0W (i )
m y (i )
S U ≈
2n
i =0
W (i )
c (y (i )
? U )(y
(i )
? U )
T
(6)
4.Estimate the cross-covariance between x and y as
C U ≈
2n
i =0
W (i )
c (x (i )?m )(y (i )? U )
T
(7)
2.2.The unscented Kalman ?lter
The UKF makes use of the unscented transform described above to give a Gaussian approximation to the ?ltering solution of non-linear optimal ?ltering problems of form,but restated here for convenience,
x k =f (x k ?1,k ?1)+q k ?1
y k =h (x k ,k )+r k
(8)
where x k ∈R n is the state,y k ∈R m is the measurement,q k ?1～N (0,Q k ?1)is the Gaussian process noise,r k ～N (0,R k )is the Gaussian measurement noise.f is the dynamic model function and h is the measurement model function.
Using the matrix form of UT described above,the prediction and update steps of the UKF can computed as follows.
The prediction step:
Compute the predicted state mean m ?k
and the predicted covari-ance P ?
k as
X k ?1=[m k ?1...m k ?1]+
?2(n +?)[0
P k ?1?
P k ?1]
?X
k =f (X k ?1,k ?1)m ?k
=?X k w m P ?k
=?X
k W [?X k ]T
+Q k ?1(9)
The update step:
Compute the predicted mean k and covariance of the measure-ment S k ,and the cross-covariance of the state and measurement C k :
X ?
k
=
m ?k
m ?k m ?k
+
?2(n +?) 0
P ?k
?
P ?k
Y ?k =h (X ?k ,K ) k =Y ?k W m
S k =Y ?k W [Y ?k ]T
+R k
C k =X ?k W [Y ?k ]
T
(10)
Then compute the ?lter gain K k and the updated state mean m k and covariance P k :
K k =C k S ?1
k
m k =m ?k
+K k [y k ? k ]P k =p ?k
?K k S k K T k
(11)
2.3.The extend Kalman ?lter
First and second order extended Kalman ?lters are presented,which are based on linear and quadratic approximations to the transformation.The ?rst order EKF algorithm step can be found in [10],and the second EKF algorithm step can be also gained in [3].
3.Mathematical model of a moving target
Next we review three nonlinear ?lters application in which we track a moving object with sensors,which measure only the angles of the object with respect to positions of the sensors.There is a one moving target in the scene and two angular sensors for tracking it.Solving this problem is important,because often more general multiple target tracking problems can be partitioned into sub-problems,in which single targets are tracked separately at a time.
3.1.The state model
The state of the target at time step k consists of the position in two dimensional Cartesian coordinates x k and y k and the velocity
toward those coordinate axes,˙x
k and ˙y k .Thus,the state vector can be expressed as
?X k =(x k
y k ˙x
k ˙y
k )T
(12)
The dynamics of the target is modeled as a linear,discretized Wiener velocity model
?X k =?
?
??10
t
010 t 001
00
1
?
????
?
????x k ?1
y k ?1˙x
k ?1˙y
k ?1?
?????
+?q k ?1(13)
Where ?q
k ?1is Gaussian process noise with zero mean and covariance
E [?q k ?1?q T k ?1]=??????????
1
3
t 3
12
t 20
013
t 301
2 t 21
2 t 20 t 00
12
t 20
t
?
?????????
q
4470H.Zhang et al./Optik 124 (2013) 4468–4471
Where q is the spectral density of the noise,which is set to q =0.1in the simulations.
3.2.The measurement model
The measurement model for sensor i is de?ned as
?i k =arc tan
y k ?s i y
x k ?s i x
+r i
k
(14)
where (s i x ,s i y )is the position of sensor i and r i k
～N (0, 2).The derivatives of the measurement model,which are needed by FEKF,can be computed as
?h i (x k )?y k =(x k ?s i x
)(x k ?s i x )2+(y k ?s i y
)2?h i (x k )?x k =?(y k ?s i y
)(x k ?s i x )2+(y k ?s i y
)2?h i (x k )
?˙x k =0,?h i (x k )?˙y
k =0,i =1,2With these the Jacobian can written as H x (x k ,k )=?
??
(x k ?s 1
k )
(x k ?s 1x )2+(y k ?s 1y )
2?(y k ?s 1
y )
(x k ?s 1x )2
+(y k ?s 1
y )
2
0(x k ?s 2
x )
(x k ?s 2x )2+(y k ?s 2
y )
2
?(y k ?s 2
y )
(x k ?s 2x )2+(y k ?s 2
y )
2
?
?
?
The non-zero second order derivatives of the measurement function are also relatively easy to compute in this model:
?2h i (x k )
?x k ?x k =
?2(x k ?s i x
)((x k ?s i x )2+(y k ?s i y )2)2
?2h i (x k )
?x k ?y k =
(y k ?s i y )2?(x k ?s i x
)2((x k ?s i x )2+(y k ?s i y
)2)2
?2h i (x k )
?y k ?y k
=
?2(y k ?s i y
)((x k ?s i x )2+(y k ?s i y
)2)2
Thus,the Hessian matrices of SEKF can be written as,
H i xx (x k ,
k )=?
????
??
?2(x k ?s i x )
((x k ?s i x )2+(y k ?s i y )2
)
2
(y k ?
s i y )2
?(x k ?
s i x )2
((x k ?s i x )2
+(y k ?s i
y )2)
2
00
(y k ?s i y )2?(x k ?s i x )2((x k ?s i x )2+(y k ?s i y )2)2
?2(y k ?s i
y )
((x k ?
s i x )
2+(y k ?
s i y )2
)
2
000
00
?
?
?????
where,i =1,2.
For comparison purposes,the following root mean squared error (RMSE)performance measures are adopted.The RMSE model [11]:
x RMSE =
1n n k =1
(x k ??x
k )2
(15)˙x
RMSE =
1n n
k =1
(˙x
k ??˙x k )2(16)
where,x k is the actual position,?x
k is the estimated position;˙x k is the actual velocity,and ?˙x
k is the estimated velocity of the target at instant of time k ,n is the number of samples.
4.Simulation result and analysis
In this section an example with position and velocity of a mov-ing target is outlined to demonstrate the accuracy of these three ?lters.Several simulations have been carried out and results are presented.The experiments are developed under the MATLAB 7.0.1simulation environment.
x (m)
y (m )
Fig.1.Filtering results of trajecttory.
Setting the same parameters and conditions,FEKF,SEKF and UKF are employed to estimate the states of the moving target.
The simulation initial conditions are as follows:Simulation time is 500s;Sampling interval is T =0.01s.
The starts with state X 0=(0010),and in the estimation we set the prior distribution for the state to x 0～N (0,P 0).Where,P 0=Diag 0.10.11010
The sensors are placed to (s 1x ,s 1y )=(?1,?2)and (s 2x ,s 2y
)=(1,1).The measurement noise variance is 0.05.The transforma-tion parameter ?=0.5,and the transformation parameter ˇ=2and ?=?1.
In the simulations we also give the target a slightly random-ized acceleration,so that it achieves curved trajectory,which is approximately the same in different simulations.The trajectory and estimates of it can be produced by Matlab codes,follow as:
a =zeros(1,500);dt=0.01;
a(1,50:100)=pi/2/51/dt +0.01*randn(1,51);a(1,200:250)=pi/2/51/dt +0.01*randn(1,51);a(1,350:400)=pi/2/51/dt +0.01*randn(1,51);x =[0;0;1;0];%Starting state t =0;X =[];for i=1:500
%Dynamics in continous case F =[0010;0001;000a(i);00-a(i)0];
%Discretization for the data generation x =expm(F*dt)*x;%Save the data
t =t +dt;X =[X x];end
plot(X(1,:),X(2,:))
4.1.Simulation results
The simulation results are shown in Figs.1–3.
The trajectory and estimates of it can be seen in Fig.1
Figs.2and 3show the errors of position and velocity with FEKF,SEKF and UKF.As can be seen from the ?gures,FEKF and UKF give almost identical results while the estimates of SEKF are clearly worse.Especially in the beginning of the trajectory SEKF has great dif?culties in getting on the right track,which is due to the relatively big uncertainty in the starting location.After that the estimates are fairly similar.
4.2.RMSE values
In Table 1,the root mean square error (RMSE)is given for these three ?lters.
H.Zhang et al./Optik 124 (2013) 4468–4471
4471
t (s)
x p o s i t i o n e r r o r (m )
Fig.2.Errors of position with FEKF,SEKF and UKF.
t (s)
X v e l o c i t y e r r o r (m /s )
Fig.3.Errors of velocity with EKF1,EKF2and UKF.
Table 1
RMSE values of estimating the position and the velocity.
Method
RMSE (x )
RMSE (˙x
)FEKF
0.1070.637SEKF 0.1260.641UKF
0.105
0.623
In Table 1we have listed the root mean square errors of all tested methods.The numbers prove the previous observations,the
performance of UKF superior to FEKF’s.And the estimates of SEKF
are clearly worse.
5.Conclusions
The UKF is introduced to be applied in tracking a moving target in this paper,and are developed for the object moving parame-ter estimation.The kinematic states are estimated with an FEKF ?lter,with an SEKF and with an UKF,respectively.Figs.1–3and Table 1show that the FEKF and UKF give almost identical perform-ances while the estimates of SEKF are clearly https://www.doczj.com/doc/c618452123.html,pared to the EKF,the advantage of the UKF is that it uses the true nonlin-ear models and approximates a Gaussian distribution of the state random variable.And the UKF avoided troublesomes that are the calculation of complex Jacobian and Hessian matrices.The UKF is a feasible algorithm for a moving target tracking.In the practice,UKF is recommended as a better alternative especially for low-cost applications that have limited computation ability.
References
[1]Yanling Hao,Zhilan Xiong,Feng Sun,et al.,Comparison of unscented Kalman
?lters,in:The 2007IEEE International Conference on Mechatronics and Automation,2007,pp.895–899.
[2]Rudolph van der Merwe,Arnaud Doucet,Naudo de Freitas,et al.,The Unscented
Particle Filter.Technical Report CUED/F-INFENG/TR 380,Cambridge University Engineering Department,2000.
[3]Haitao Zhang,Jian Rong,Xiaochun Zhong,The application and design of sec-ond order EKF based on GPS/DR integration for land vehicle navigation,in:2008International Conference on MultiMedia and Information Technology (MMIT’08),2008,pp.837–841.
[4]Bilal Akm,Umut Orguner,Aydm Ersak,et al.,Simple derivative-free nonlinear
state observer for sensorless AC drives,IEEE/ASME Trans.Mechatron.11(5)(2006)634–643.
[5]Rudolph van der Merwe,Eric A.Wan.Sigma-point Kalman ?lters for integrated
navigation.https://www.doczj.com/doc/c618452123.html,/～tracker/#Anchor-Publications-14401[6]M.B.Luca,S.Azou,G.Burel,A.Serbanescu,On exact Kalman ?ltering of poly-nomial systems,IEEE Trans.Circuits Syst.53(6)(2006)1329–1340.
[7]Simo Sarkka,On unscented Kalman ?ltering for state estimation of continuous-time nonlinear systems,IEEE Trans.Automatic Control 52(9)(2007)1631–1641.
[8]Yuanxin Wu,Dewen Hu,Meiping Wu,et al.,Unscented Kalman ?ltering for
additive noise case:augmented versus nonaugmented,IEEE Signal Process.Lett.12(5)(2005)357–360.
[9]Leopoldo Angrisani,Aldo Baccigalupi,Rosario Schiano Lo Moriello,Ultrasonic
time-of-?ight estimation through unscented Kalman ?lter,IEEE Trans.Instrum.Meas.55(4)(2006)1077–1084.
[10]Haitao Zhang,Jian Rong,Xiaochun Zhong,The application and design of
EKF smoother based on GPS/DR integration for land vehicle navigation,in:2008IEEE Paci?c-Asia Workshop on Computational Intelligence and Industrial Applications (PACIIA’08),vol.1,2008,pp.704–707.
[11]P.Jorge Escamilla-Ambrosio,Neil Mort,Hybrid Kalman ?lter-fuzzy logic adap-tive multisensor data fusion architectures,in:The 42th IEEE Conference on Decision and Control,USA,2003,pp.5215–5220.