Microsoft Word - 1422 Max 101.docx HUNGARIAN JOURNAL OF INDUSTRY AND CHEMISTRY VESZPRÉM Vol. 42(2) pp. 57–64 (2014) A COMPARISON OF ADVANCED NON-LINEAR STATE ESTIMATION TECHNIQUES FOR GROUND VEHICLES GYÖRGY MAX! AND BÉLA LANTOS 1 Department of Control Engineering and Informatics, Budapest University of Technology and Economics, Magyar tudósok körútja 2, Budapest, 1117, HUNGARY !Email: max@iit.bme.hu This paper presents a comparative examination of state-of-the-art non-linear state estimation techniques. First, the Sigma- Point (SPKF) stochastic estimators are investigated on the basis of the Unscented Transformation. This is followed by the technique of non-linear symmetry-preserving observers for non-linear dynamics on the basis of symmetrical LIE groups. The variety of the deterministic and stochastic state estimation algorithms is studied the kinematics of ground vehicles. Two cases are described that deal with the reliability of path tracking, the convergence and speed of off-line calibration. Keywords: state estimation, invariant observers, unscented KALMAN filter, vehicle kinematics Introduction Vehicles are indispensable in modern society, and vehicle safety is consequently of importance in everyday life. A lack of information about the state of vehicle and parameters presents a major obstacle for the development of vehicle control systems. The effectiveness of vehicle stability control largely depends on the accuracy of the vehicle state parameters that are measured by appropriate sensors. Unfortunately, measurements from these sensors contain bias, as well as electrical noise, and can drift with temperature changes. However, the state estimation method is also able to dramatically reduce errors introduced by measurement and process noise contained in the signal. The focus of this work is the non-linear state estimation techniques. Firstly, the ‘Sigma-Point KALMAN filter family’ (SPKF) is discussed on the basis of the ‘Unscented Transformation’ (UT) and STERLING’s interpolation [3, 5, 6]. Secondly, the symmetry-preserving observer is presented [15, 16, 18], which is based on Lie-transformation of invariant frames. The various state estimation procedures are then examined and compared through an illustrative non- linear example of vehicle kinematics. Sigma-Point KALMAN Filters Sigma-Point KALMAN filter [2, 4, 7] is a collective name for derivativeless KALMAN filters that employs the deterministic sampling based Sigma-point approach to calculate the optimal terms of the state estimation. Common methods use the Scaled Unscented Transfor- mation [4] and STERLING’s polynomial interpolation. The former replaces the original set of sigma-points with a transformed set in order to minimize the errors of higher order terms, the latter approximates the derivation of the divided difference filter [10, 11] and central difference filter in [12]. The Scaled Unscented Transformation The Scaled Unscented Transformation (SUT) has two steps. First, a fixed number of sigma points is chosen to capture the desired moments (at least mean and covariance) of the original distribution. Then the sigma points are propagated through the non-linear function and the moments of the transformed variables are estimated. The advantage of SUT over the Taylor series based approximation is that SUT is better at capturing the higher order moments caused by the non-linear transformation, as discussed in [1, 8-10]. Consider a non-linear function y = g(x) and assume x has mean x and covariance xP . The first two moments of y are calculated by the following two steps: 1) Weighted Sigma-point selection: X0 = x Xi = x + (L +λ)Px( )i=1…L Xi = x − (L +λ)Px( )i=L+1…2L (1) with weights w0 m = λ L+λ w0 c = λ L+λ + (1−α2 +β ) wi m = wi c = λ 2(L+λ ) (2) 58 where wi m, and wi c are associated with the ith sigma- point such that 1 2 0 =∑ = L i iw . Symbols λ, α, and β denote scaling parameters and ( ) ixPL )( λ+ is the ith column of the matrix square root of the weighted xPL )( λ+ covariance matrix. 2) Propagation: )( ii XgY = y ≈ wi mYi i=0 2L ∑ Py ≈ wi c(Yi − y )(Yi − y )T i=0 2L ∑ Pxy ≈ wi c(Xi − x )(Yi − y )T i=0 2L ∑ (3) where y , yP , and xyP are the approximated mean, covariance and cross-covariance respectively. The Unscented KALMAN Filter (UKF) The algorithm of the UKF according to Refs. [3, 7, 8] consists of the following steps: 1) Initialization: x̂0 = E x0[ ] , Px0 = E (x0 − x0 )(x0 − x0 ) T" # $ % (4) x̂0 a = E x̂0 a! " # $= x̂0 T v0 T n0 T! "% # $& T (5) P0 a = Px0 0 0 0 Rv 0 0 0 Rn ! " # # # # $ % & & & & (6) 2) State estimation for ∞= …1k : a) Calculate Sigma-points: Xk−1 a = x̂k−1 a x̂k−1 a +γ Pk−1 a x̂k−1 a −γ Pk−1 a" # $ % & ' (7) b) Time-update: Xi,k|k−1 x = f (Xi,k−1 x ,Xk−1 v ,uk−1), i = 0,…, 2L x̂k − = wi mXi,k|k−1 x i=0 2L ∑ Pxk − = wi c(Xi,k|k−1 x − x̂k −)(Xi,k|k−1 x − x̂k −)T i=0 2L ∑ (8) c) Measurement-update: Yi,k|k−1 = h(Xi,k|k−1 x ,Xk−1 n ), i = 0,…, 2L ŷk − = wi mYi,k|k−1 i=0 2L ∑ Pŷk = wi c(Yi,k|k−1 − ŷk −)(Yi,k|k−1 − ŷk −)T i=0 2L ∑ Pxkyk = wi c(Xi,k|k−1 x − x̂k −)(Yi,k|k−1 − ŷk −) i=0 2L ∑ (9) Kk = Pxkyk Pŷk −1 x̂k = x̂k − +Kk (yk − ŷk −) Pxk = Pxk − −KkPŷkKk T (10) where xk a = xk T vk T nk T! "# $ %& T and Xk a = (Xk x )T (Xk v )T (Xk n )T! "# $ %& T are the L dimension vectors of the augmented states and Sigma-points respectively, vR and nR are the covariances of the process and measurement noise respectively, λγ += L , λ = α2(L+κ)+L, α = 10-3, κ = 0, β = 2, and wi is the weighting parameter defined in Eq.(2). The Central-Difference KALMAN Filter This approach is based on STERLING’s polynomial interpolation, which was used in the derivations of divided difference filter [10, 11] and central difference filter in Ref. [12]. The result of this approximation can be used for sigma-point approach as in the case of SUT. The algorithm consists of the following steps: 1) Weighted Sigma-point selection: X0 = x Xi = x + h Px( )i=1…L Xi = x − h Px( )i=L+1…2L (11) with weights w0 m = h2−L h2 , wi m = 1 2h2 , wi c1 = 1 4h2 , wi c2 = h2−1 4h2 , (12) where wi m, and wi c are associated with i = 1, …, 2L sigma-point. 2) Initialization: the same as defined in Eqs.(4-6) 3) State estimation for k = 1, …, ∞: a) Sigma-points for time-update: x̂k−1 av = x̂k−1 v" # $ % , Pk−1 av = Pxk−1 0 0 Rv " # $ $ % & ' ' (13) Xk−1 av = x̂k−1 av x̂k−1 av + h Pk−1 av x̂k−1 av − h Pk−1 av " # $ % & ' (14) b) Time-update: Xi,k|k−1 x = f (Xi,k−1 x ,Xk−1 v ,uk−1), i = 0,…, 2L x̂k − = wi mXi,k|k−1 x i=0 2L ∑ Pxk − = wi c1 (Xi,k|k−1 x − XL+i,k|k−1 x )2 + i=1 L ∑ wi c2 (Xi,k|k−1 x + XL+i,k|k−1 x − 2X0,k|k−1 x )2 i=1 L ∑ (15) 59 c) Sigma-points for measurement-update: x̂k−1 an = x̂k − n" #$ % &' , Pk|k−1 an = Pxk − 0 0 Rn " # $ $ % & ' ' (16) Xk|k−1 an = x̂k|k−1 an x̂k|k−1 an + h Pk|k−1 an x̂k|k−1 an − h Pk|k−1 an " # $ % & ' (17) d) Measurement-update: Yi,k|k−1 = h(Xi,k|k−1 x ,Xk|k−1 n ), i = 0,…, 2L ŷk − = wi mYi,k|k−1 i=0 2L ∑ Pŷk = wi c1 (Yi,k|k−1 −YL+i,k|k−1)2 i=0 2L ∑ + wi c2 (Yi,k|k−1 +YL+i,k|k−1 − 2Y0,k|k−1)2 i=0 2L ∑ Pxkyk = w1 c1Pxk − (Y1:L,k|k−1 −YL+1:2L,k|k−1)T Kk = Pxkyk Pŷk −1 x̂k = x̂k − +Kk (yk − ŷk −) Pxk = Pxk − −KkPŷkKk T (18) Symmetry-Preserving Observers Among many problems in control theory, the symmetric nature of the system can be utilized, such as in the case of optimal control for feedback or regulations. The symmetry properties can also be used in the design of observers. Symmetry-preserving observers are also known as invariant observers that are based on the differential geometric background of abstract LIE- groups [15, 16, 18]. The constructive algorithm can be defined for designing invariant observers, which is based on the group transformation of symmetry. The aim is to produce invariant frames and invariant output errors to transform a locally asymptotically convergent observer around an equilibrium point into an invariant one, retaining its first-order approximation. The method in Refs. [16, 18] benefits from the fact that the error equations and the first-order approximation can be calculated explicitly and the relationships can be defined globally. In addition, the invariant error equations make stability issues easier to deal with [19- 21]. Assumptions Suppose G is a LIE-group with identity e on manifold Σ. The group transformation (ϕg )g∈G on Σ is a smooth mapping (g,ξ )∈G ×Σ→ϕg(ξ )∈ Σ (19) such that ϕe(ξ ) = ξ and ϕg2 (ϕg1 (ξ )) =ϕg2g1 (ξ ) for ζ, g1, and g2. The transformation group is local if ϕg(ξ ) is defined only when g lies sufficiently near to e. We consider only local transformations. Invariant observers are designed for the smooth non- linear system d dt x = f (x,u) , y = h(x,u) (20) where x, u, and y belong to the open subsets X ⊂ ℝn, ⊂U ℝm, and ⊂Y ℝp respectively and p ≤ n . Let r ≤ n be the dimension of the LIE-group. We assume that for each x the mapping x→φg(x) is full rank. Time functions u(t) and y(t) are known. Invariant Pre-Observer For the differential geometric description of the symmetric observers [14], the following definitions and theorems should be introduced: Definition 1. The group of local transformation on manifold UX× is expressed by (X,U) = (φg(x),ψg(u)) (21) where φg(x),ψg(u) is local diffeomorphism and g ∈G Definition 2. System d dt x = f (x,u) is G-invariant, if f (φg(x),ψg(u)) = Dφg(x) ⋅ f (x,u) for g, x, u. Thus, the system remains unchanged for the local transformation, !X = f (X,U) . Definition 3. The output y = h(x,u) is G-equivariant, if there exists ρg transformation on Y, such that g ∈G and h(φg(x),ψg(u)) = ρg(h(x,u)) for g, x, u. Thus, the output remains unchanged for the local transformation, Y = h(X,U) . Definition 4. The vector field w is G-invariant on manifold X, if d dt x = w(x) is invariant to the transformation, i.e. w(φg(x)) = Dφg(x) ⋅w(x) for g, x. Definition 5. An invariant frame (w1, w2, …, w3) consists of n linearly point-wise independent G- invariant vector fields on X thus (w1(x),w2(x),…,wn (x)) is a basis of the tangent space TX(x) . Lemma 1. The invariant frame can be constructed from the canonical basis of X. The vector field defined by wi(x) = (Dφγ (x) ) −1 ⋅ ∂∂xi (22) for i = 1, …, n forms an invariant frame. Definition 6. The system d dt x̂ = F(x̂,u, y) is a pre- observer, if F(x,u,h(x,u)) = f (x,u) is satisfied for x and u. Furthermore, if lim x̂(t) = x(t) as t→∞ , then the pre-observer is asymptotic. 60 Definition 7. The pre-observer is G-invariant, if ),,ˆ()ˆ())(),(),ˆ(( yuxFxDyuxF gggg ⋅= ϕρψϕ (23) is satisfied for g, x̂ , u, and y i.e. the observer remains unchanged for the group transformation on YU,,X . Thus, X = φg(x) , U =ψg(u) , Y = ρg(y) , and d dt X̂ = F(X̂,U,Y ) . An invariant observer is an asymptotic G-invariant pre-observer. Moreover, if the pre-observer is G- invariant and rank (F) = dim y , then the output map y is G-equivariant. Definition 8. Smooth map (x̂,u, y)→ E(x̂,u, y)∈ ℝp is an invariant output error if a) y→ E(x̂,u, y) is invertible for x̂ , u, and y b) E(x̂,u,h(x̂,u)) = 0 for x̂ and u c) E(φg(x̂),ψg(u),ρg(y)) = E(x̂,u, y) for x̂ , u, and y. The first two properties mean E is an output error, the third expresses the invariance. The introduction of the invariant error is necessary, because the output error ŷ− y does not usually preserve the geometry of the system [21]. The following three theorems [15, 16] support the construction of the symmetry-preserving observers. Theorem 1. The observer d dt x̂ = F(x̂,u, y) is a G- invariant pre-observer for the G-invariant non-linear Eq.(20) with a G-equivariant output if and only if F(x̂,u, y) = f (x̂,u)+ Li(I(x̂,u),E(x,u, y)) i=1 n ∑ wi(x̂) (24) where E is an invariant output error, (x̂,u)→ I(x̂,u)∈ ℝn+m&r is a full rank function, Li is a smooth function, such that Li(I(x̂,u), 0) = 0 and (w1,w2,…,wn ) is an invariant frame. Since Li(I,E) = Li(I,E)E Li(I,E)wi i=1 n ∑ = wi(Li(I,E) i=1 n ∑ E) = w1 … wn( ) L1(I,E) ! Ln (I,E) " # $ $ $ $ % & ' ' ' ' E (25) The observer can be written as follows F(x̂,u, y) = f (x̂,u)+W (x̂)L(I(x̂,u),E(x̂,u, y))E(x̂,u, y). (26) Theorem 2. The following statements are valid: • (x̂,u, y)→ E(x̂,u, y) invariant output error exists, • there is a (x̂,u)→ I(x̂,u)∈ℝn+m&r invariant function, • every other invariant output error has the form of !E(x̂,u, y) = L(I(x̂,u),E(x̂,u, y)) (27) where L is a smooth function such that L(I, 0) = 0 and E →L(I,E) is invertible. Theorem 3. Consider the equilibrium of a non-linear system f (x,u ) = 0 and y = h(x,u ) Assume that the linearized system A,B,C,D around this equilibrium is observable, where A = ∂f ∂x (x,u ) , B = ∂f ∂u (x,u ) ,C = ∂h ∂x (x,u ) , A = ∂h ∂u (x,u ) and let L be such that A+ LC is a stable matrix. From the locally asymptotically convergent observer d dt x̂ = f (x̂,u)+ L(ŷ− y) , ŷ = h(x̂,u) (28) an invariant observer can be constructed by the same linear approximation d dt x̂ = f (x̂,u)+W (x̂)L(I(x̂,u),E(x̂,u, y))E(x̂,u, y) (29) where L = −W (x)−1LV−1 and V = ∂E ∂y(x,u,y ) is an invertible matrix of pp× dimensions. There is no general algorithm for designing the Li gain functions of Theorem 1. However, if we consider the following invariant state error η(x, x̂) = φγ (x)(x̂)−φγ (x)(x) (30) instead of the xx −ˆ state error, where )(xγ is the solution of φg a (x) = c (31) with respect to g, then the invariant error will only depend on the I(x,u) trajectories. Constructive Algorithm Consider an invariant system, i.e. unchanged by transformation Eq.(21) with an equivariant output (Definition 3). The non-linear observer design for non- linear dynamic systems [10, 11] can be divided into the following steps: a) Choose a G LIE-group and a group transformation taking into account the symmetrical properties of the system. b) Solve the normalization Eq.(31). Build an invariant error E and the complete set of scalar invariants I. c) Construct the invariant frame Eq.(22). d) Determine the pre-observer Eq.(25). e) Linearize the system f (x,u ) = 0 around the equilibrium and obtain A,B,C,D matrices. Check the observability and design an invariant observer from the chosen linear observer by using Theorem 3. Choose an appropriate L . f) Choose the parameters of the linearized error equations, based on the invariant state-errors η, such that the invariant system will be asymptotically stable. 61 Then the non-linear symmetry-preserving observer is asymptotically stable, and converges locally and exponentially along all system trajectories. Case Study: A Non-Holonomic Vehicle In this section, the previously introduced state estimation procedures are employed for an example of a non-holonomic vehicle tracking control. The well- known model in the robotics of the simplified vehicle kinematics is considered. The tracking is maintained by satisfying the kinematic constraints with the appropriate choice of reference signals. Kinematic Model The simplified kinematic model of the vehicle can be written in the following form !x !y !θ ! " # # # $ % & & & = ucosθ usinθ uv ! " # # # $ % & & & , h(x, y,θ ) = (x, y) (32) where u is the velocity, v is the curvature, i.e. a function of the steering angle and h is the measured output. All signals contain additive Gaussian noise. The model assumes that the contact point of the wheel and the ground does not slip. Notice, that the linearized system is not observable. Tracking For path tracking the control signals are set to satisfy the kinematic constraints, thus u and v are calculated as follows: u(t) = !xr 2(t)+ !yr 2(t) and v(t) = !!yr (t) !xr (t)− !xr (t)!!yr (t) u3(t) (33) where xr(t) and yr(t) are time functions of the reference signal. The initial conditions for tracking control is θr (0) = tan −1 !yr (0) !xr (0) " # $ % & ' . (34) Hence, the reference signals for the vehicle velocity and steering function can be calculated. In the simulations, sinusoidal path tracking is implemented using the reference signals xr (t) = t, and yr (t) = Asin(ωt). Invariant Observer Design Group Transformation Eq.(32) is independent of the origin and of the orientation of the frame chosen, i.e. it is invariant with regards to the group of rotations and translations. We chose the Lie-group G = SE(2) and defined the group transformation as follows: φ(xg,yg,θg )(x, y,θ ) = xg yg θg ! " # # # # $ % & & & & ⋅ x y θ ! " # # # $ % & & & = = x cosθg − ysinθg + xg xsinθg + ycosθg + yg θ +θg ! " # # # # $ % & & & & (35) ψ(xg,yg,θg )(u,v) = u v ! " # $ % & ρ(xg,yg,θg )(x, y) = x cosθg − ysinθg + xg xsinθg + ycosθg + yg ! " # # $ % & & One can check that Eq.(32) is invariant to the transformations ϕ, and ψ in terms of Definition 1 and the output is equivariant to transformation ρ in terms of Definition 3. Invariant Output Errors The normalization Eq.(31) for :0=c 0 = x cosθγ − ysinθγ + xγ 0 = xsinθγ + ycosθγ + yγ 0 =θ +θγ (36) The solution of Eq.(36), the scalar invariants and output errors can be written as follows respectively xγ yγ θγ ! " # # # # $ % & & & & = x y θ ! " # # # $ % & & & −1 = −x cosθ − ysinθ xsinθ − ycosθ −θ ! " # # ## $ % & & && = γ x y θ ! " # # # $ % & & & (37) I(x, y,θ,u,v) =ψγ (x,y,θ )(u,v) = u v ! " # $ % & (38) E = ρ(xγ ,yγ ,θγ )(x̂, ŷ)− ρ(xγ ,yγ ,θγ )(x, y) = cosθ̂ sinθ̂ −sinθ̂ cosθ̂ " # $ $ % & ' ' x̂ − x ŷ− y " # $$ % & '' (39) Invariant Frame W = (Dφγ (x,y,θ )(x, y,θ )) −1 ∂ ∂(x,y,θ ) = = Dφγ (x,y,θ )(x, y,θ ) = cosθ −sinθ 0 sinθ cosθ 0 0 0 1 # $ % % % & ' ( ( ( . (40) 62 Invariant pre-observer d dt x̂ ŷ θ̂ ! " # # # $ % & & & = ucosθ̂ usinθ̂ uv ! " # # # # $ % & & & & + cosθ̂ −sinθ̂ 0 sinθ̂ cosθ̂ 0 0 0 1 ! " # # # # $ % & & & & L × cosθ̂ sinθ̂ −sinθ̂ cosθ̂ ! " # # $ % & & x̂ − x ŷ− y ! " ## $ % && (41) where L is a smooth 23× gain matrix, whose elements depend on the invariant error E and the invariants I. Error Equations and Error Dynamics η = ηx ηy ηθ ! " # # # # $ % & & & & = γ (x̂, ŷ,θ̂ ) x̂ ŷ θ̂ ! " # # # $ % & & & −γ (x̂, ŷ,θ̂ ) x y θ ! " # # # $ % & & & = = (x̂ − x)cosθ̂ + (ŷ− y)sinθ̂ −(x̂ − x)sinθ̂ + (ŷ− y)cosθ̂ (θ̂ −θ ) ! " # # # # $ % & & & & (42) !ηx !ηy !ηθ ! " # # # # $ % & & & & = u(1− cosηθ )+ (uv+ L31ηx + L32ηy )ηy usinηθ − (uv+ L31ηx + L32ηy )ηx 0 ! " # # # # $ % & & & & + +L ηx ηy ! " # # $ % & & (43) It can be seen that the invariant error equations are independent of the trajectory and only depend on the relative quantities ηx, ηy, and ηθ and the I invariants. Stability and Convergence The weighting matrix is defined as follows L = − |u | a ubEy −uv uv−ubEy − |u | c 0 −ub " # $ $ $ $ % & ' ' ' ' , (44) where a and b are care positive scalars and Ey = ηy. The invariant error dynamics can be made locally asymptotically stable. Furthermore, the resulting symmetry-preserving observer is almost globally asymptotically convergent, i.e. it converges any initial condition except for one as described in Refs. [15-16]. Simulation Results The state estimation techniques are illustrated in two tracking examples. For comparative purposes, the following algorithms have been implemented: Extended KALMAN Filter (EKF), Central Difference and Unscented KALMAN Filter (CKF, UKF) and the symmetry preserving observer (SYM) discretized by the sample time of the sigma-point estimators. The reference paths are xr (t) = t, and yr (t) = Asin(ωt) , where A = 3 m and ω = π/5 rad s-1. Zero mean additive Gaussian noise is assumed with standard deviations of 0.01 and 0.005 for the process noise and 0.02 for the measurement noise. The sampling time is Ts = 0.1 s. The initial values are x0 est = y0 est =θ0 est =1 , and x0 = y0 = 0 . Example 1 The vehicle starts from the origin of the xy-plane with large initial estimation error and tracks a sinusoidal path. Fig.1 shows the estimated output positions of the vehicle using sigma-point KALMAN filters and symmetry-preserving observers. The estimation errors of different algorithms can be seen in Figs.2 and 3. The errors are smaller and show smoother signals in the case of the SPKF estimators, which was expected, since the SYM observer is deterministic. Large errors appear at the peak values of the reference signals; however, both estimation techniques achieved the same satisfactory results in spite of the relatively small sampling frequency. Figure 2: Estimation errors of the sigma-point and extended KALMAN filters from Example 1 63 Root-mean-square errors can be seen in Table 1. We can conclude that among the processes of state estimation techniques the SYM observer is competitive with SPKF estimators. Example 2 In this example the vehicle performs an off-line estimation before movement starts, followed by an on- line estimation along a sinusoidal path. It is notable that in the stationary state the velocity is zero and at the beginning of movement the velocity changes to a non- zero value abruptly, see Eq.(33). Figs.4-7 illustrate the estimated output positions and the measurement errors for the sigma-point estimators and the invariant observers respectively The error signals show a quick convergence in the case of the symmetry-preserving observer. The speed of convergence depends on the parameters set in the L matrix; however, high gains can cause the observer to be divergent and make larger errors. Since there are no u and δ signals in the stationary position and the orientation is not directly measured, the orientation can only be estimated after the vehicle starts moving. The error signal oscillates around zero in a similar way to Example 1. Due to calibration, smaller errors appear. The invariant observer and sigma-point estimators have the same magnitude of error components. The root-mean-square errors of the simulation can be seen in Table 2. Figure 3: Estimation errors of the symmetry-preserving observer (SYM) from Example 1 Figure 6: Estimation errors of the sigma-point and the extended KALMAN filters from Example 2 Figure 4: Estimated output position of the vehicle using sigma-point state estimation (UKF, CKF) and extended KALMAN Filter (EKF) from Example 2 Figure 5: Estimated output position of the vehicle using symmetry-preserving observer (SYM) from Example 2 Figure 7: Estimation errors of the symmetry- preserving observer (SYM) from Example 2 Table 1: Root-mean-squared errors of the estimated signals from Example 1 RMSE x, m y, m θ, rad UKF 0.1368 0.1587 0.0338 CKF 0.1366 0.1577 0.0336 EKF 0.1371 0.1551 0.0352 SYM 0.1371 0.1628 0.0455 Table 2: Root-mean-square errors of the estimated signals from Example 2 RMSE x, m y, m θ, rad UKF 0.0998 0.1180 0.0627 CKF 0.0993 0.1175 0.0628 EKF 0.1013 0.1165 0.0648 SYM 0.1264 0.1453 0.0760 64 Conclusion In this paper we presented a comparison of two different recently developed state estimation techniques. The Sigma-Point (SPKF) stochastic estimator is based on the Unscented Transformation and the non-linear symmetry-preserving observers are based on the group transformation of symmetrical LIE groups and invariant frames. Two examples are considered for the estimation of vehicle kinematics: the first example deals with the path tracking reliability of the vehicle; the second example examines the convergence and the speed of the off-line calibration followed by path tracking. The examples illustrate that for non-linear systems in noisy environments the use of stochastic state- estimators is needed. Moreover the Sigma-point approach provides more accurate estimation results than the commonly used non-linear EKF state estimators. The symmetry preserving observers for non-linear systems benefit from their constructive design method, which provides local asymptotic convergence. This method can be utilized by non-linear applications in which the physical and mechanical symmetry properties of the system can be exploited. REFERENCES [1] BAR-SHALOM Y., LI X.-R., KIRUBARAJAN T.: Estimation with Applications to Tracking and Navigationm, Wiley Interscience, New York, NY, U.S.A., 2001 [2] SÄRKKÄ S.: Recursive Bayesian inference on stochastic differential equations, PhD Thesis, Helsinki University of Technology, Helsinki, Finland, 2006 [3] JULIER S.J., UHLMANN J.K.: Unscented filtering and non- linear estimation, Proc. IEEE, 2004, 92(3), 401–422 [4] VAN DER MERWE R., WAN E.A., JULIER S.: Sigma-Point KALMAN Filters for Nonlinear Estimation and Sensor- Fusion: Applications to Integrated Navigation, Proc. AIAA Guidance, Navigation & Control Conference (GNC), Providence, RI, U.S.A. 2004 [5] VAN DER MERWE R.: Sigma-Point KALMAN Filters for Probabilistic Inference in Dynamic State-Space Models, PhD Thesis, Oregon Health & Science University, Eugene, OR, U.S.A., 2004 [6] JULIER S.J., UHLMANN J.K.: Corrections to unscented filtering and non-linear estimation, Proc. IEEE, 2004, 92(12), 1958–1958 [7] JULIER S.J., UHLMANN J.K.: Unscented filtering and non- linear estimation, Proc. IEEE, 2004, 92(3), 401–422 [8] WAN E.A., VAN DER MERWE R.: The unscented KALMAN filter, in KALMAN Filtering and Neural Networks, Ed.: HAYKIN S., John Wiley and Sons, New York, NY, U.S.A., 2001, Chapter 7 [9] NORGAARD M., POULSEN N., RAVN O.: Advances in Derivative-Free State Estimation for Non-linear Systems Tech. Rep. IMM-REP-1998-15, Dept. Math. Modelling, Technical University of Denmark, Lyngby, Denmark, 2000 [10] NORGAARD M., POULSEN N., RAVN O.: New Developments in State Estimation for Nonlinear Systems, Automatica 2000, 36(11), 1627–1638 [11] ITO K., XIONG K.: Gaussian Filters for Nonlinear Filtering Problems, IEEE Trans. Automatic Control 2000, 45(5), 910–927 [12] VAN DER MERWE R., WAN E.: Efficient Derivative-Free KALMAN Filters for Online Learning, Proc. 9th Eur. Symp. Artificial Neural Networks (ESANN) Bruges, Belgium, 2001, 205–210 [13] VAN DER MERWE R., WAN E.: The Square-Root Unscented KALMAN Filter for State- and Parameter- Estimation, Proc. IEEE Int. Conf. Acoustics, Speech, and Signal Processing (ICASSP), Salt Lake City, UT, 2001, 6, 3461–3464 [14] SALAM ERAN: Filtering Algorithms and Avionics Systems for Unmanned Aerial Vehicles, PhD Thesis, MINES ParisTech, Paris, France, 2010 [15] BONNABEL S., MARTIN P., ROUCHON P.: Non-linear Symmetry Preserving Observers on LIE-Groups, IEEE Transactions on Automatic Control, 2009, 54(7), 1709- 1713 [16] BONNABEL S., MARTIN P., ROUCHON P.: Symmetry- preserving Observers, IEEE Transactions on Automatic Control, 2008, 53(11), 2514–2526 [17] BONNABEL S., MARTIN P., ROUCHON P.: A non-linear symmetry-preserving observer for velocity aided inertial navigation, Proc. 2006 American Control Conference, Minneapolis, MN, U.S.A., 2006 [18] AYHANMAN N., ROUCHON P.: On invariant asymptotic observers, CDC, 2002 [19] ROUCHON P., GUILLAUME D.: Observation and Control of Simplified Car, IFAC, 1998 [20] BONNABEL S.: Symmetries in observer design: review of some recent results and application to EKF-based SLAM, Robot Motion and Control 2011, Lecture Notes in Control and Information Sciences 2012, 422, 3–15 [21] COLLON C., RUDOLPH J., WOITTENNEK F.: Invariant Feedback Design for Control Systems with Lie- symmetries – a kinematic car example, Discrete Cont. Dyn. Sys. 2011, 2(11), 312–321