A Gaussian Particle Filter Approach for Sensors to Track Multiple Moving Targets H

1 A Gaussian Particle Filter Approach for Sensors to Track Multiple Moving Targets H. Li

Abstract—In a variety of problems, the number and state assumptions are violated in modern applications, because of multiple moving targets are unknown and are subject the targets’ motion models are unknown, and, possibly, to be inferred from their measurements obtained by a random and nonlinear. Also, due to the use of low-cost sensor with limited sensing ability. This type of problems is raised in a variety of applications, including monitoring passive sensors, measurement errors and noise may be of endangered species, cleaning, and surveillance. Particle non-additive and non-Gaussian. An extended Kalman filters are widely used to estimate target state from its prior filter (EKF) can be used when the system dynamics information and its measurements that recently become are nonlinear, but can be linearized about nominal available, especially for the cases when the measurement operating conditions [7]. An unscented Kalman filter model and the prior distribution of state of interest are non-Gaussian. However, the problem of estimating number (UKF) method, based on the unscented transformation of total targets and their state becomes intractable when (UT) method, can be applied to compute the mean and the number of total targets and the measurement-target covariance of a function up to the second order of the association are unknown. This paper presents a novel Taylor expansion [8]. Gaussian particle filter technique that combines Kalman However, the efficiency of these filters decreases sig- filter and particle filter for estimating the number and state of total targets based on the measurement obtained online. nificantly when the system dynamics are highly nonlin- The estimation is represented by a set of weighted particles, ear or unknown, and when the measurement noise are different from classical particle filter, where each particle non-Gaussian. Recently, a non-parametric method based is a Gaussian distribution instead of a point mass. on condensation and Monte Carlo simulation, known as Index Terms—Kalman Filter, Particle Filter, Gaussian a particle filter, has been proposed for tracking multiple Particle Filter, Multiple Target Tracking targets exhibiting nonlinear dynamics and non-Gaussian random effects [9]. Particle filters are well suited to I.INTRODUCTION modern surveillance systems because they can be applied to Bayesian models in which the hidden variables are The problem of tracking and monitoring targets us- connected by a Markov chain in discrete time. In the ing position-fixed sensors is relevant to a variety of classical particle filter method, a weighted set of particles applications, including monitoring of moving targets or point masses are used to represent the probability using cameras [], tracking anomalies in manufacturing density function (PDF) of the target state by means of plants [1], and tracking of endangered species [2]–[4]. a superposition of weighted Dirac delta functions. At The position-fixed sensor is deployed to measure targets each iteration of the particle filter, particles representing based on limited information that only becomes available possible target state values are sampled from a proposal when the target enters the sensor’s field-of-view (FOV) distribution [10]. The weight associated with each par- or visibility region [5]. The sensor’s FOV is defined as ticle is then obtained from the target-state likelihood arXiv:1501.02411v1 [cs.LG] 11 Jan 2015 a compact subset of the region of interest, in which function, and from the prior estimation of the target the sensor can obtain measurements from the targets. state PDF. When the effective particle size is smaller In many such sensor applications, filter techniques are than a predefined threshold, a re-sampling technique often required to estimate unknown variables of interest, can be implemented [11]. One disadvantage of classical for examples, target number and target state. particle-filtering techniques is that the target-state transi- When the noise in the measurement model is an tion function is used as the importance density function additive Gaussian distribution, the target state can be to sample particles, without taking new observations into estimated from frequent observation sequence using a account [12]. Recently, an particle filter with Mixture Kalman filter [6]. This approach is well suited to long- Gaussian representation was proposed by author for range high-accuracy sensors, such as radars, and to monitoring maneuvering targets [13], where the particles moving targets with a known dynamical model and are sampled based on the supporting intervals of the initial conditions. However, most of these underlying target-state likelihood function and the prior estimation The author H. Li is with Zhejiang University of Technology, function of the target state. In this case, the supporting Hangzhou, 310014, P.R.China . Email: [email protected]. interval of a distribution is defined as the 90% confidence 2 interval [14]. The weight for each particle is obtained time step k. The states for total targets at k is denoted 1 2 N by considering the likelihood function and the transition as Xk = [xk, xk, ··· , xk ] that has N state vectors. function simultaneously. Then, the weighted expectation The estimation of total target state at k is denoted as k 1 2 T k k maximization (EM) algorithm is implemented to use the X = [xk, xk, ··· , xk ]. Let T denote the estimation sampled weighted particles to generate a normal mixture of total target number at tk. The ith target is modeled model of the distribution. as i i Kreucher proposed joint multitarget probability den- xk = Fkxk−1 + νk, (1) sity (JMPD) [15] to estimate the number of total targets in workspace and their state, where targets are moving. where ν ∼ N(0, Q ). By using JMPD, the data association problem is avoided, k k (2) however, the JMPD results in a joint state space, the Furthermore, Fk and Qk are assumed known. dimension of which is the dimension of a target state In standard estimation theory, a sensor that obtains a times number of total targets. Since the number of vector of measurements zk ∈ Rr in order to estimate an total targets is unknown, the joint space size remain unknown state vector set Xk ∈ Rn at time k is modeled unavailable. To overcome this problem, it is assumed the as, number of total targets has a maximum value. Therefore, zk = h(Xk, λk), (3) when the maximum number of targets is large, the joint n+℘ r state space becomes intractable. where h : R → R is a deterministic vector Inspired by [16], this paper presents a novel filter tech- function that is possibly nonlinear, the random vector k ℘ nique which combines Kalman filter and particle filter λ ∈ R represents the sensor characteristics, such as for estimating the number and state of total targets based sensor action, mode [17], environmental conditions, and on the measurement obtained online. The estimation is sensor noise or measurement errors. In this paper, the represented by a set of weighted particles, different from sensor is modeled as N classical particle filter, where each particle is a Gaussian 1 X z = x + ω . (4) instead of a point mass. The weight of each particle k N i k represents the probability of existing a target, while its i=1 gaussian indicates the state distribution for this target. It is further assumed that the whole workspace is visible More importantly, the update of particles is different to a position fixed sensor (not shown). from classical particle filter. For each particle, the gaussian parameters are updated based using Kalman filter III.BACKGROUND given a measurement. To overcome the data association A. Particle Filter Methods problem, in this paper, when one particle is updated, the other particles are considered as the measurement The particle filter is a recursive model estimation condition, which will be explained in Section IV. The method based on sequential Monte Carlo Simulations. novel Gaussian particle filter technique requires less Because of their recursive nature, particle filters are particles than classical particle filters, and can solve easily applicable to online data processing and variable multiple target estimation problem without increasing the inference. More importantly, it is applicable to nonlinear state space dimensions. system dynamics with non-Gaussian noises. The PDF The paper is organized as follows. Section II de- functions are represented with properly weighted and scribes the multiple targets estimation problem formu- relocated point-mass, known as particles. These particles lation and assumptions. The background on the particle are sampled from an importance density that is crucial to the particle filter algorithm and is also referred to filter and Kalman filter is reviewed in Section III. Section κ κ N IV presents the Gaussian Particle filter technique. The as a proposal distribution. Let {xj,p, wj,p}p=1 denote the weighted particles that are used to approximate the method is demonstrated through numerical simulations κ κ posterior PDF f(xj | Zj ) for the jth target at tκ, where and results, presented in Section V. Conclusions and κ 0 κ future work are described in Section VI. Zj = {zj ,..., zj } denotes the set of all measurements obtained by sensor i, from target j, up to tκ. Then, the II.PROBLEM FORMULATION posterior probability density function of the target state, given the measurement at t can be modeled as, There N targets are moving in a two dimensional κ W N N N workspace denoted as where denotes the unknown κ κ X κ κ X κ number of total targets. For simplicity, there is zero f(xj | Zj ) = wj,pδ(xj,p), wj,p = 1 (5) obstacle in the workspace. The goal of the sensor is to p=1 p=1 κ obtain the state estimation for all the targets, denoted where wj,p is non-negative and δ is the Dirac delta as Xk, and target number estimation, denoted as Tk, at function. The techniques always consist of the recursive 3

propagation of the particles and the particle weights. In where Hk is a mapping from system state space to κ each iteration, the particles xj,p are sampled from the measurement space, and white noise Qk is defined as importance density q(x). Then, weight wk is updated j,p ω ≈ N(0,R ) (11) for each particle by k k κ It is assumed that the noise ωk and ν at each time step κ p(xj,p) wj,p ∝ κ (6) are independent. q(xj,p) Let x˜k denote the predicted state estimation given κ κ κ where p(xj,p) ∝ f(xj,p | Zj ). Additionally, the weights xˆk−1, where xˆk−1 is the updated estimation of system are normalized at the end of each iteration. state at k − 1 time step. Furthermore, let Σ˜ k denote the ˆ ˆ One common drawback of particle filters is the de- predicted covariance given Σk−1, where Σk−1 is the generacy phenomenon [12], i.e., the variance of particle updated estimation covariance. Then, in the predicting weights accumulates along iterations. A common way step, to evaluate the degeneracy phenomenon is the effective x˜k = Fkxˆk−1 + Bkuk (12) sample size Ne [11], obtained by, Σ˜ = F Σˆ FT + Q (13) 1 k k k−1 k k Ne = N (7) P κ 2 In the updating step, the measurement zk is used, to- p=1(wj,p) gether with above predicted state and covariance, to up- where wκ , p = 1, 2,...,N are the normalized weights. j,p date the state and covariance. The residual, yk between In general, a re-sampling procedure is taken when Ne < measurement and predicted state is given by Ns, where Ns is a predefined threshold, and is usually N κ κ N yk = zk − Hkx˜k (14) set as 2 . Let {xj,p, wj,p}p=1 denote the particle set that needs to be re-sampled, and let {xκ∗ , wκ∗ }N j,p j,p p=1 The innovation covariance Sk is given by denote the particle set after re-sampling. The main idea ˜ T of this re-sampling procedure is to eliminate the parti- Sk = HkΣkHk + Rk (15) κ∗ κ∗ N cles having low weights by re-sampling {xj,p, wj,p}p=1 κ κ N κ∗ Then, the optimal Kalman gain is calculated as from {xj,p, wj,p}p=1 with the probability of p(xj,p = κ κ ˜ T −1 xj,s) = wj,s. At the end of the resampling procedure, Kk = ΣkHk Sk (16) wκ∗ , p = 1, 2,...,N are set as 1/N. j,p Then, the state and covariance can be updated by

xˆk = x˜k + Kkyk (17) B. Kalman Filter Methods Σˆ k = (I − KkHk)Σ˜ k (18) The well known Kalman filter is also a recursive method to estimate system/target state based on a mea- IV. METHODOLOGY surement sequence, minimizing the estimation uncer- In this paper, a novel Gaussian particle filter technique tainty. The measurement of the system state with an addi- follows the main idea of particle filter for estimating tive Gaussian noise are given by the sensor. Then, in each the number and state of total targets based on the iteration, the Kalman filter consists of two precesses: i) measurement obtained online. Different from classical it predicts the system state and their uncertainties; ii) particle filter, each particle here is a gaussian instead of it updates the system state and uncertainties with the a point mass. The estimation for number of total targets measurement that newly becomes available. The system and their state is presented by a set of weighted particles. dynamics is given as The ith particle at time k is denoted as i i i i i xk = Fkxk−1 + Bkuk + νk (8) Pk = {wk, N (xk|µk, Σk)} (19) where subscript k and k − 1 denote the current and where wi is the probability of existing a target having a previous time index, while Fk is the system discrete state distribution as N(µ, N (xi|µ, Σi)). By this particle transition matrix, and Bk and uk are the control matrix definition, the dimensions of the system state remains the and control input. νk is the white noise, defined as same as the dimensions of each individual target. When these particles are available, the estimated number of νk ≈ N(0, Qk) (9) PNp total targets can be given as T = i=1 wi, where Np is where Σk is the covariance. At kth time step, an mea- the particle number. surement of the system true state xk is made by a sensor, Notice that the particle representation is different from is given by classical particle filter, where each particle represents a zk = Hkxk + ωk (10) possible value of system state. The updating of each 4 particle and total weights are also different from classical Then, by applying Kalman procedure particle filter. Kalman filter is used to update each y = z − H µ (24) particle, the weight and the distribution. Notice that since k k k k T the measurement at each time step is conditioned on all Sk = HkΣkHk + Rk (25) T −1 the targets in the FOV, while in the classical Kalman Kk = ΣkHk Sk (26) filter method one measurement is associated with one µk = µk + Kkyk (27) target, which means data association problem is avoid. Σ = (I − K H )Σ (28) Therefore, the Kalman filter is modified to updated k k k k the particles which are coupled by one measurement, Its proof can be found in the appendix. and some approximations and assumptions are further Once each particle appearing in combination E has needed. been updated, the weight wc is for the particle combination E can be obtained by 1 w = Π(w )ei (1 − w )1−ei × c i i 2 −1 (2π) kΣc k T −1 × exp{−(zk − µc) Σc (zk − µc)} (29)

where c ∈ IE, where IE is the combination index, and −1 Similar to Kalman filters and particle filters, the al- µc) and Σc is given by gorithm proposed in this paper is a recursive method. X µ = H µi (30) Assume that at time step k, the measurement zk is c k k available, and the estimation of the system at time step X i T Σc = Hk ΣkHk + Rk (31) k−1 is represented by a particle set, denoted as Pk−1 = 1 2 Np i {Pk−1,Pk−1,...,Pk−1}, where Np is the number of Then, insert particle {N (µk, Σk)} into a set Gc for all particles. By using the target dynamic function 1, combination c ∈ IE, the set Gc has a weight wc. After all ei 1−ei Pk−1 can be updated to P˜k without using the zk. Due to Gc the combination of E that Π(wi) (1 − wi) > limit of FOV, only a few particles may have contribution is obtained. Weights are updated by to the measurement. Let PS denote set including the wc ¯ ˜ wc = P (32) particles lie in the FOV, while let P = Pk/PS denote wc the compensation set. Only the particles in PS are . Then in each group Gc, the weight of ith particle is updated. Please Note the size of PS is small. Without ˜1 ˜2 ˜s updated as generality, assume that PS = {Pk , Pk ,..., Pk }, where i i wk s the number of particles The update of each particle in wc = i ∗ wc (33) Πwk PS is calculated separately. Without generally, we focus ˜j on updating Pk , Let a boolean set E = [e1, e1, . . . , es], The particles in all set Gc are updated from the same where ei ∈ {0, 1}. For any E with ej = 1 such that particle in the previous set. If two particles in is close ei 1−ei Π(wi) (1 − wi) > , where is a predefined enough, then they are combined as one particle, the threshold, a particle is calculated and denoted as p˜j, weight of which is set as the summation of both weights. Then, the modified Kalman filter is used to give the The distance between µi and µj is defined as Mahal- updated gaussian parameters of all particles with ei = 1. distance T According to sensor model 4, the measurement is given (µi − µj) M(µi − µj) (34) by and its covariance is updated as Ps i s µ ei 1 X X i=1 k Σ = Σi (35) zk = Ps + Ps 2 eiΣi +ωk (20) k ei ( ei) i=1 i=1 i=1,i6=j V. SIMULATION AND RESULTS Compare the above function to (10), we have following As shown in figure (1), N targets, represented by setting blue dots, are moving in the 2 dimensional workspace. 1 The whole workspace is visible to a position fixed Hk = I × Ps (21) ( i=1 ei) sensor (not shown) and the workspace is discretized into Ps i 12 × 12 cells. Each cell represents a 1 × 1 rectangular i=1,i6=j µkei zk = zk − Ps (22) area. The ith cell, denoted as Ci, i ∈ C, is defined by i=1 ei ul ul dr dr s [xi , yi , xi , yi ], where C is the cell index set and 1 X ul ul dr dr (xi , yi ) and (xi , yi ) are up left and down right cor- Rk = Ps 2 eiΣi + ωk (23) ( ei) i=1 i=1,i6=j ner coordinates of the ith rectangular area respectively. 5

Only M cells can be measured at each time step k, same cell, then the detection probability is pd(T ) = (1+SNR)/(1+T ×SNR) and they don’t have to be adjacent. The goal of the pd and the ith sensor measurement at sensor is to estimate the target states and target number time k can be evaluated by at time k. In this paper, information value function ( k based α divergence is used to select the best M cells k k k k k pd(T ) zi = 1 p(zi |X ,T , λa,i, λc ) = k to measure at each step [18]. The estimation of target 1 − pd(T ) zi = 0 states and target number at time k is represented by joint N X k ul k dr multitarget probability density(JMPD) and it is updated T = (xj ≥ xc ) ∩ (xj < xc ) after obtaining new measurements. j=1 k ul k dr k ∩(yj ≥ yc ) ∩ (xj < xc ), c = λa,i (39) k k &! (1+λc )/(1+T λc ) pd(T ) = pd (40) ! ! ! ! ! ! ! ! ! ! ! ! k k k k ! ! ! ! ! ! ! ! ! ! ! ! where xj , yj are two position components of xj ∈ X k ! ! ! ! ! ! ! ! ! ! ! ! and T is the target number. Additionally, operators ”≥”

! ! ! ! ! ! ! ! ! ! ! ! and ”<” return either 1 if true or 0 if false, while ”∩” is the Boolean operator ”and”. For example, as shown in ! ! ! ! ! ! ! ! ! ! ! ! ﬁgure [1], when c = k, T = 2, similarly, when c = j(i), ! ! ! ! ! ! ! ! ! ! ! ! $! T = 1(0). ! ! ! ! ! ! ! ! ! ! ! ! A snapshot of simulations is shown in Fig. 3, where ! ! ! ! ! ! ! ! ! ! ! ! magenta squares represent positive measurement return ! ! ! ! ! ! ! ! ! ! ! ! "! and blue dots represent the true targets’ positions. The ! ! ! ! ! ! ! ! ! ! ! ! simulation results are summarized in Fig. 2, where the ! ! ! ! ! ! #! ! ! ! ! ! ! black curve represents the target state estimation error

! ! ! ! ! ! ! ! ! ! ! ! %! and the red curve represents the target number estimation

! error. As shown in Fig. 2, both errors decreases as more Fig. 1. The workspace contians three point targets. measurements become available.

The target time-discrete state transition function can be written as k+1 k k xi = Fxi + wi (36) where 1 τ 0 0 0 1 0 0 F =   (37) 0 0 1 τ 0 0 0 1

k and wi is 0 mean Gaussian noise with covariance Q =diag(20, 0.2, 20, 0.2), and τ is the time step length, and i ∈ {1, 2, ··· ,T k} [15]. It is further assumed that i) the sensor can measure any cell at time k; ii) the sensor can only measure up to Fig. 2. Simulation Result k M cells at time k. The sensor condition λc represents the signal to noise ratio SNR, currently, it has only one possible value, fixed and known. The measurement zk i VI.CONCLUSIONAND FUTURE WORK is a discrete variable, then joint PMF can be written as A Gaussian particle filter that combines Kalman fil- f(zk, Xk,T k, λk)=f(zk|Xk,T k, λk)f(Xk,T k)f(λk) ter and particle filter is presented in this paper for (38) estimating the number and state of total targets based When measuring a cell, the imager sensor will give on the measurement obtained online. The estimation is a Raleigh return, either a 0 (no detection) or a 1 represented by a set of weighted particles, different from (detection) governed by detecting probability, denoted as classical particle filter, where each particle is a Gaussian pd, and false alarm probability, denoted as pf . According instead of a point mass. The weight of each particle to standard model for threshold detection of Rayleigh represents the probability of existing a target, while its (1+SNR) returns, pf = pd . When T targets are in the Gaussian indicates the state distribution for this target. 6

Fig. 3. Snapshot of simulations

This approach is efﬁcient for the problem of estimating By setting ∂∂Kj = 0, therefore number of total targets and their state. k j j 1 T Kk = Σk(Ps I) i=1 ei s VII.APPENDIX 1 X 1 ×(R + I Σi ( I)T )−1 k Ps e k Ps e ˜1 ˜2 ˜s i=1 i i=1 i=1 i Without losing generality, PS = {Pk , Pk ,..., Pk }, s E = [e1, e2, . . . , es], for any particle such that ej = 1, 1 j 1 X i −1 j j = s Σk(Rk + s Σk) (44). its µk and Σk, given zk and PS. P e (P e )2 i=1 i i=1 i i=1 Ps xi e y = z − i=1 k i ) Then, k k Ps (41) j j j ei i=1 µk = µ˜k + Kkyk (45)

j j j 1 Σk = COV(xk − xˆk) j ˜ j ˜ j Σk = Σk − Ps Σk j j j ei = COV(xk − (x˜k + Kkyk)) i=1 s Ps i 1 1 j j j i=1 xkei X i T −1 = COV x − (x˜ + K ( ) ×(Rk + s I Σ ( s I) ) k k k Ps P e k P e i=1 ei i=1 i i=1 i=1 i Ps i j i=1 x˜kei ×Σ˜ (46) +νk − Ps ) k i=1 ei 1 j j ˜j = COV (I − Ps IKk)(xk − xk) REFERENCES i=1 ei s X −Kj ν − Kj (xi − x˜i ) (42) [1] D. Culler, D. Estrin, and M. Srivastava, “Overview of sensor k k k k k networks,” Computer, vol. 37, no. 8, pp. 41–49, 2004. i=1,i6=j [2] W. Lu, G. Zhang, and S. Ferrari, “A randomized hybrid system 1 1 approach to coordinated robotic sensor planning,” in IEEE Con- = (I − IKj )Σ˜ j (I − IKj )T Ps e k k Ps e k ference on Decision and Control, 2010, pp. 3857–3864. i=1 i i=1 i [3] P. Juang, H. Oki, Y. Wang, M. Martonosi, L. Peh, and D. Ruben- s stein, “Energy efﬁcient computing for wildlife tracking: Design 1 j X 1 j i T Proc. 10th +Ps IKk Σk(Ps IKk) tradeoffs and early experiences with zebranet,” in ei ei i=1 i=1,i6=j i=1 International Conference on Architectural Support for Program- ming Languages and Operating Systems (ASPLOS-X), San Jose, j j +KkRkKk. (43) CA, 2002, pp. 96–107. 7

[4] W. Lu, G. Zhang, S. Ferrari, R. Fierro, and I. Palunko, “An information potential approach for tracking and surveilling multiple moving targets using mobile sensor agents,” in SPIE Defense, Security, and Sensing. International Society for Optics and Photonics, 2011, pp. 80 450T–80 450T. [5] W. Lu, G. Zhang, and S. Ferrari, “An information potential approach to integrated sensor path planning and control,” Robotics, IEEE Transactions on, vol. 30, no. 4, pp. 919–934, Aug 2014. [6] G. B. G. Welch, “An introduction to the kalman filter,” De- partment of Computer Science, University of North Carolina at Chapel Hill, Tech. Rep. [7] S. J. Julier and J. K. Uhlmann, “A new extension of the kalman filter to nonlinear systems,” Proc. AeroSense: 11th Int. Symp. Aerospace/Defense Sensing, Simulation and Controls, pp. 182- 197, 1997. [8] E. Wan and R. Van Der Merwe, “The unscented kalman filter for nonlinear estimation,” in Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium, 2000, pp. 153 –158. [9] Z. Khan, T. Balch, and F. Dellaert, “An mcmc-based particle filter for tracking multiple interacting targets,” in Computer Vision - ECCV 2004, T. Pajdla and J. Matas, Eds., 2004, pp. 279–290. [10] M. Arulampalam, S. Maskell, N. Gordon, and T. Clapp, “A tuto- rial on particle filters for online nonlinear/non-gaussian bayesian tracking,” Signal Processing, IEEE Transactions on, vol. 50, no. 2, pp. 174 –188, 2002. [11] J. Carpenter, P. Clifford, and P. Fearnhead, “Improved particle filter for nonlinear problems,” in Radar, Sonar and Navigation, IEEE Proceedings, vol. 146, no. 1, 1999, pp. 2 –7. [12] Y. Rui and Y. Chen, “Better proposal distributions: Object tracking using unscented particle filter,” vol. 2, 2001. [13] W. Lu, G. Zhang, S. Ferrari, M. Anderson, and R. Fierro, “A particle-filter information potential method for tracking and monitoring maneuvering targets using a mobile sensor agent,” The Journal of Defense Modeling and Simulation: Applications, Methodology, Technology, vol. 11, no. 1, pp. 47–58, 2014. [14] O’Gorman and T. W.”, Applied adaptive statistical method: test of sigficance and condifence intervals. Society for Industrial and Applied Mathematics,Philadelphia, 2004. [15] C. Kreucher, K. Kastella, and O. Hero, “Multitarget tracking using the joint multitarget probability density,” IEEE Transactions on Aerospace and Electronic Systems, vol. 41, no. 4, pp. 1396– 1414, 2005. [16] S.-H. Won, W. Melek, and F. Golnaraghi, “A kalman/particle filter-based position and orientation estimation method using a position sensor/inertial measurement unit hybrid system,” Indus- trial Electronics, IEEE Transactions on, vol. 57, no. 5, pp. 1787– 1798, May 2010. [17] W. Lu and S. Ferrari, “An approximate dynamic programming approach for model-free control of switched systems,” in CDC, 2013, pp. 3837–3844. [18] W. Lu, G. Zhang, and S. Ferrari, “A comparison of information theoretic functions for tracking maneuvering targets,” in Statis- tical Signal Processing Workshop (SSP), 2012 IEEE, Aug 2012, pp. 149–152.