Nonlinear Measurement Update for Recursive ... - Science Direct

Available online at www.sciencedirect.com

ScienceDirect Procedia Computer Science 92 (2016) 543 – 548

2nd International Conference on Intelligent Computing, Communication & Convergence (ICCC-2016) Srikanta Patnaik, Editor in Chief Conference Organized by Interscience Institute of Management and Technology Bhubaneswar, Odisha, India

Nonlinear Measurement Update for Recursive Filtering Based on the Gauss von Mises Distribution Mu-yiChen*, Hong-yuanWang School of Information Science and Engineering, Shenyang Ligong University, Shenyang, China

Abstract In conventional Kalman-based state estimation algorithms, there is an assumption that the uncertainties in the system state and measurements are Gaussian distributed. However, this Gaussian assumption ignores the periodic nature of angular or orientation quantities. In this paper, the Gauss von Mises (GVM) distribution model defined on a cylindrical manifold is employed, the Dirac mixture approximation method is extended to deal with sampling with GVM, in order to perform recursive filtering, the GVM approximation to joint distribution is proposed, the formula to compute posterior distribution is derived.Finally, the measurement update algorithm is developed. Simulation results show that when the system state contains a circular variable, the proposed GVM filter can achieve more accurate estimates than the traditional extended Kalmanfilter(EKF), thereby providing a novel method to estimate system state specialized to GVM distribution. © 2016 2014The TheAuthors. Authors.Published Published Elsevier byby Elsevier B.V.B.V. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/). Selection and peer-review under responsibility of scientific committee of Missouri University of Science and Technology. Peer-review under responsibility of the Organizing Committee of ICCC 2016

* Corresponding author. Tel.: +86-138-4012-0624; fax: +86-24-2468-2012. E-mail address:[email protected]

1877-0509 © 2016 The Authors. Published by Elsevier B.V. This is an open access article under the CC BY-NC-ND license (http://creativecommons.org/licenses/by-nc-nd/4.0/). Peer-review under responsibility of the Organizing Committee of ICCC 2016 doi:10.1016/j.procs.2016.07.380

544

Mu-yiChen and Hong-yuanWang / Procedia Computer Science 92 (2016) 543 – 548 Keywords:Gauss von Mises(GVM) distribution; Dirac mixture approximation; Recursive Bayesian filtering; Nonlinear measurement update;

1. Introduction Accurate estimation of system state is crucial in many applications such as navigation, motion estimation. Since most of the filters including standard Kalman-based filters adopt Gaussian assumptions, the results are not satisfactory when the state vector contains periodic angular or orientation variables, especially when the measurements from low-cost sensorsinvolves significant uncertainties. Therefore, we need more accurate models and filtering methods to take into account the underlying structure of the state space. Various probability distribution models defined on different manifolds are available in the literature. The generalization of von Mises distribution to cylindrical manifold1, multivariate von Mises distribution2, partially wrapped normal(PWN) distribution for rigid motion group SE(2) 3are proposed, but corresponding state estimation algorithms are not given in these work. Circular fusion filter4, quaternion Bingham filter5 are also proposed for the circular filtering problem.However, these filters only consider the case when the state isa single circular variable or 3D rotation represented by a quaternion.Gauss von Mises(GVM) distribution is proposed by J.T.Horwood 6,7to deal with the space surveillance tracking environment that is typically nonlinear and non-Gaussian. Itcan characterize the uncertainty in the orbital state of a space object. But in their work, only uncertainty propagation problem has been considered, i.e., the time update step in Bayesian filtering, without presenting algorithms for the measurement update step. The rest of the paper is organized as follows. The GVM distribution is introduced in Section 2.The Dirac mixture approximation method is extended to enable sampling with GVM distribution in Section 3. In Section 4, the GVM approximation to joint distribution is proposed, the formula for calculating GVM parameters for posterior distribution is derived, and the measurement update algorithm is developed. In Section 5, simulation results are presented. Finally, conclusions are drawn in Section 6. 2. Definition of the GVM Distribution Model Definition 1. (Gauss von Mises (GVM) Distribution) 6. The random variables (T , x ) S u R n are said to be jointly distributed as a Gauss von Mises (GVM) distribution if and only if their joint PDF has the form:

p( x,T ) GVM ( x,T ; P, P,D , E , * , N ) { N ( x; P, P)VM (T ; 4( x), N )

(1)

where:

VM (T ; 4( x), N ) exp(N cos(T 4( x))) / 2S I0 (N ) 4( x ) D E T y (1/ 2) y T * y , y

L1 ( x P ) , P

(2) LLT .

(3)

The parameter set (P, P, D , E , * , N ) should meet the following constraints: P R n , P is an n u n symmetric positive-definite matrix, D R , E R n , * is an n u n symmetric matrix, and N ! 0 . The matrix L is the lowertriangular Cholesky factor of P . The notation ( x,T ) : GVM (P, P, D , E , * , N ) is used to represent that ( x,T ) are jointly distributed as a GVM distribution with parameter set (P, P, D , E , * , N ) . We can apply the transformation: y L1 ( x P ) , I T D E T y (1/ 2) y T * y to transform GVM (P, P, D , E , * , N ) to canonical GVM distribution with:

p( y,I ) GVM ( y, I; 0, I,0, 0, 0, N )

N ( y; 0, I )VM (I;0, N )

(4)

Mu-yiChen and Hong-yuanWang / Procedia Computer Science 92 (2016) 543 – 548

545

3. Dirac Mixture Approximation to GVM distribution To estimate the state of a nonlinear dynamic system from noisy measurements, sample-based methodsare widely used. Among deterministic sampling methods, the unscented transformation methodis typically used for Gaussian distribution.Recently, the Dirac mixture approximation method8,9is proposed, which try to minimize an appropriate distance measure between the given probability density and its approximation, while maintaining some certain moments. It is capable of covering the support of the original densityhomogeneously, besides, the number of sampling points are controllable. Our GVM approximation method is based on Dirac mixture approximation, that includes the following four steps: x Dirac mixture approximation of multivariate canonical Gaussian Densities8 on R n is used first to generate q samples { yJ i }iq 1 ; x These samples are transformed via n u n orthogonal rotation matrix to generate more samples, d orthogonal rotation matrix including the identity matrix will lead to dq samples { yJ i }idq1 . When orthogonal rotation matrices are chosen carefully, the samples could homogeneously cover the support of the original density while maintaining moments invariant for canonical Gaussian distribution. (In R2, let T

2S / d , the orthogonal rotation

matrices can be chosen to represent the rotations around the origin by 0,T , 2T ,...,(d 1)T . In R3, polyhedral group or its subgroup, for example, the tetrahedral group of order 12 or the octahedral group of order 24, can be used to construct the orthogonal rotation matrices, etc.) x Splitting each point to generate l angular samples with VM (0, N ) distribution, if the splitting coefficientis c , then {IJ ij }cj

1

represents the c samples corresponding to yJ i , therefore, cdq samples ( yJ i , IJ i ) for canonical

GVM distribution on R n u S are generated. x The samples ( xV i ,TV i ) for GVM distribution with parameter set (P, P, D , E , * , N ) can be generated via the following transformation: xJ i

P LyJ , and TJ i

i

IJ D E T yJ (1/ 2) yJT * yJ . i

i

i

i

4. Measurement update for GVM distribution In this paper, we will consider discrete-time stochastic dynamic systems of the form: ( xk ,Tk ) f ( xk 1 ,Tk 1 ) , zk g ( xk ,Tk ) vk , where ( xk ,T k ) R n u S is the system state vector, zk R m is the measurement vector, and vk ~ N (0, R) is the Gaussian measurement noise.Bayesian recursive filtering includes time update step and measurement update step. For the GVM distribution model, the time update step is given by J.T.Horwood6 and we will focus on the measurement update step. 4.1. GVM Approximation to the Joint Distribution To compute the posterior state distribution, we compute the GVM approximation to the joint distribution first, which can be seen as an intermediate step10 toward the desired GVM approximation to the posterior p( xk ,Tk | z1:k ) . % % % % Our objective is to compute the parameters (P%k , P% k , D k , E k , * k , N k ) of the GVM approximation to the joint, i.e., % % % p( xk , zk ,Tk | z1:k 1 ) ~ GVM (P%k , Pk , D%k , E k , * k , N%k ) , given the predicted approximation p( xk ,T k | z1:k 1 ) ~ GVM ( Pk , Pk , D k , E k , * k , N k ) . For the linear part, since p( xk , zk ,Tk | z1:k 1 ) p( zk | xk ,Tk ) p( xk ,Tk | z1:k 1 ) , and the marginal distribution p( xk ,Tk | z1:k 1 ) is known from the time update step, it remains to compute the marginal p( zk | z1:k 1 ) and the crosscovariance Pkxz .To get the Gaussian approximation for the linear part, we follow the three-order GVM quadrature method6to compute P kz , Pkxz and Pkzz , then we obtain:

546

Mu-yiChen and Hong-yuanWang / Procedia Computer Science 92 (2016) 543 – 548 ª Pk º % ª« Pk P ᧨ » k xz T z ¬( Pk ) ¬ Pk ¼

P%k «

Pkxz º » Pkzz ¼

(5)

% % In order to estimate D%k , E% k , * k , N k , suppose the Dirac points are ( xJ i , TJ i ) , then the predicted measurements are % given by: zJ i g ( xJ i ,TJ i ) . Using these points, the optimal estimate of the parameters D%k , E% k , * k can be achieved by the least square method. % Let N%k N k , the estimation of D%k , E% k , * k leads to the optimization problem: % (D%k , E% k ,* k )

1 q arg min ¦ i 1 exp(i(Dˆ Eˆ T ( xJTi , zJTi )T ( xJTi , zJTi )*ˆ ( xJTi , zJTi )T )) exp(iTJ i ) 2 Dˆ , Eˆ , *%

2

(6)

The starting point can be chosen as the following:

ª * 0 nu m º ªE º D%k D k , E%k « k » , *%k « k » ¬0¼ ¬0mun H I mum ¼

(7)

where H is small.Methods such as Gauss-Newtoncombined with trust region methodscan be used to solve this nonlinear least squares problem. To avoid local minima, the homotopy continuation method11 can be employed. 4.2. Posterior distribution Computation When the GVM approximation to joint distribution is achieved, the posterior p( xk ,Tk | z1:k ) can be computed as: p( xk ,Tk | z1:k ) ~ GVM (Pk , Pk , Dk , E k , * k , N k ) .

% % % % p( xk , zk , T k | z1:k 1 ) GVM ( P%k , P% k ,Dk , E k , * k , Nk ) | z p( zk | z1:k 1 ) N ( Pk , Pkzz ) VM (4( xk , zk ), N%k ) N ( P%k , P% k) VM (4( xk , zk ), N%k ) N ( Pk , Pk ) z zz N ( Pk , Pk )

p( xk ,T k | z1:k )

(8)

where:

Pk

Pk Pkxz ( Pkzz )1 ( zk Pkz ) , Pk

Pk Pkxz ( Pkzz )1 ( Pkxz )T

1 4( xk , zk ) D%k E%kT y%k y%kT *%k y%k , y%k 2 Let yk

(10)

Lk1 ( xk Pk ) , 4( xk ) is given by:

4( xk ) D k E kT yk Let

1 ª xk Pk º L% k « z » ¬ zk Pk ¼

(9)

1 T yk * k yk 2

(11)

547


Nk

N%k , Pk

ª E% º 1 1 Lk LTk , E% « %» , L%k k ¬ E2 ¼

ª L% 11 «% L ¬ 21

º %T % %1 % L% 12 » , Lk * k Lk V % L22 ¼

ªV% º V% 11 12 « % %» ¬V21 V22 ¼

(12)

comparing (10) and (11) yields:

Dk

T % z %T % %T % %T % xz zz 1 D% [ E% 1 L12 E 2 L22 ( E1 L11 E 2 L21 ) Pk ( Pk ) ]( z k P k )

1 1 zz T xz T % xz zz 1 % xz zz 1 ( zk Pkz )T [ V% ( Pk ) ( Pk ) V11 Pk ( Pk ) V21 Pk ( Pk ) ]( zk Pkz ) 22 2 2

(13)

T % z T % zz T xz T % T %T % E k [[(E% 1 L11 E 2 L21 ) ( zk Pk ) (V21 ( Pk ) ( Pk ) V11 )]Lk ]

(14)

*k

(15)

LTkV11 Lk

4.3. Measurement update algorithm description For measurement update, there are three inputs: x The measurement equation zk g ( xk ,Tk ) vk ; x The GVM approximation to the predicted distribution p( xk ,T k | z1:k 1 ) ~ GVM ( Pk , Pk , D k , E k , * k , N k ) ; x The measurement zk . The outputis theGVM approximation of the posterior distribution, with the parameter set given by (Pk , Pk , Dk , E k , * k , Nk ) . Ourproposed algorithm consists of four main steps: 6 x Recover P%k , P% k of the joint distribution using the quadrature method . x Generate q Dirac sampling points {( xJ i ,TJ i )}iq 1 from the predicted GVM distribution using the method proposed

in Section 3, and compute zJ i g ( xJ i ,TJ i ) . % x Set N%k N k , and solve the optimization problem (6) to recover D%k , E% k , * k of the joint distribution. x Compute the parameter set (Pk , Pk , Dk , E k , * k , Nk ) of the GVM approximation of the posterior distribution using (9), (13)-(15). 5. Simulation and results

In our simulation, we set state variables as [ x1 , x2 , x3 ,T ]T .The nonlinear state equations are given by: x&1 x2 , x&2 x1 x32 K / x12 , x&3 2x2 x3 / x1 ,T& x3 . The measurement equations are given by: z1 sin 1 ( Re / x1 ) , z2 D0 T . In this simulation, we combine the time update step given by J.T.Horwood6 and our proposed measurement update step to perform recursive GVM filtering(GVMF). The GVMF and the traditional extended K / ( R03 ) Kalmanfilter(EKF) are compared. The parameters used are: Re 30 , K 1000 , R0 50 , v0 7 , Z0 T T ˆ . Let xini [ R0 , v0 , Z0 ] , it is supposed that the initial state estimate is given by X 0 [ R0 , v0 , Z0 , S ] for EKF and Xˆ 0 ~ GVM ( xini , I, S, 0, 0, 2) for GVMF while the true initial state is X 0 [ R0 , v0 , Z0 , 0]T , which indicates that the initial state estimate is poor. The results are shown in Fig.1(a) and Fig.1(b), it can be observed that our proposed GVMF converges to the ground-truth at a fast speed, while the absolute angle estimate error by EKF increases with time. Apparently the proposed GVMF achieves better estimates for angle estimation compared to EKF.

548


Fig. 1. (a) Absolute angle velocity estimates error; (b) Absolute angle estimates error.

6. Conclusions In this paper, the GVM distribution model is employed, Dirac mixture approximation method is extended to GVM distribution, the GVM approximation to the joint distribution is given, the formula for GVM parameter computation for the posterior distribution is derived, and the measurement update algorithm is developed. Using our proposed measurement update combined with the time update given by J.T.Horwood6, recursive filtering can be implemented. Results show that the GVM filter is capable of giving more accurate angle estimates than the traditional EKF. Acknowledgements The authors would like to thankLiaoning Provincial Education Department in China for supporting the research under grant entitled ‘Scientific Research Fund of Liaoning Provincial Education Department’ (Grant No.L2013083).

References 1.MardiaKV, Sutton TW. A model for cylindrical variables with applications.Roy. Statist.Soc. Ser. B. 1978; 40:229-233. 2.MardiaKV, Hughes G, Taylor CC, et al. A Multivariate von Mises Distribution with Applications to Bioinformatics.Canadian Journal of Statistics. 2008;36(1): 99-109. 3. KurzG, GilitschenskiI, HanebeckUD. The partially wrapped normal distribution for SE(2) estimation. 2014 International Conference on Multisensor Fusion and Information Integration for Intelligent Systems (MFI), Beijing, China.Sep.2014. 4. StienneG, ReboulS, AzmaniM, et al. A Multi-temporal Multi-sensor Circular Fusion Filter.Information Fusion. 2013;18(7): 86-100. 5. Tracking 3-D Rotations with the Quaternion Bingham Filter. MIT CSAIL, Tech. Rep. Mar. 2013. 6.HorwoodJT, PooreAB. Gauss von Mises Distribution for Improved Uncertainty Realism in Space Situational Awareness. SIAM/ASA Journal on Uncertainty Quantification. 2014;2(1): 276-304. 7. Aristoff J, Horwood J, Poore A. Implicit-Runge–Kutta-based methods for fast, precise, and scalable uncertainty propagation. Celest. Mech. Dyn. Astron. 2015;122(2): 169–182. 8.HanebeckUD. Kernel-based Deterministic Blue-noise Sampling of Arbitrary Probability Density Functions. Proceedings of the 48th Annual Conference on Information Sciences and Systems (CISS 2014), Princeton, New Jersey, USA, Mar. 2014. 9. HanebeckUD.Sample Set Design for Nonlinear Kalman Filters viewed as a Moment Problem.Proceedings of the 17th International Conference on Information Fusion (Fusion 2014), Salamanca, Spain, Jul. 2014. 10.DeisenrothMP,Ohlsson H. A General Perspective on Gaussian Filtering and Smoothing: Explaining Current and Deriving New Algorithms. Proceedings of the American Control Conference. 2011. 11. AllgowerEL, GeorgK. Introduction to Numerical Continuation Methods. SIAM. 2003.