NONLINEAR ESTIMATION METHODS FOR AUTONOMOUS TRACKED VEHICLE WITH SLIP.doc

nonlinear estimation methods for autonomous tracked vehicle with slip*abstract: in order to achieve precise, robust autonomous guidance and control of a tracked vehicle, a kinematic model with longitudinal and lateral slip is established. four different nonlinear filters are used to estimate both state vector and time-varying parameter vector of the created model jointly. the first filter is the well-known extended kalman filter. the second filter is an unscented version of the kalman filter. the third one is a particle filter using the unscented kalman filter to generate the importance proposal distribution. the last one is a novel and guaranteed filter that uses a linear set-membership estimator and can give an ellipsoid set in which the true state lies. the four different approaches have different complexities, behavior and advantages that are surveyed and compared. key words: tracked vehicle nonlinear estimation kalman filter partis filter set-membership filter0 introductiontracked vehicles are currently ucsu :n many urban reconnaissance and rescue mission scenarios11, autonomous planetary explorations, snd other applications where terrain conditions are difficult to predict. tviey re better suited for such tasks than wheeled vehicles due to the larger contact area of tracks that provides them with better stability and traction at various ground conditions3.the general kinematic model of tracked vehicles without slip is similar to common wheeled robots, in which the slip is considered as disturbance or noise to the model4. however, the slip in both longitudinal and lateral directions appears very often in practice, and has a critical influence on the performance of vehicles. in order to improve the accuracy of the localization and control the robot more efficiently, it is important to establish the slip model in which the slip is regarded as time-varying parameters. currently some works have covered this question56, but few consider the slip in both directions. as the slip parameters can also be incorporated into the controller, they may be estimated with the pose of the vehicle simultaneously. to solve this problem of nonlinear joint estimation, some nonlinear filters can be employed.the classical method is the extended kalman filter (ekf)71. the ekf is an approximate extension of kalman filtering techniques to nonlinear systems by linearizing the state dynamics at each step of the time. however, it does require that the state dynamics is sufficiently differentiable (i.e. the jacobian exists), and the linear approximation can lead to poor representations of the nonlinear functions and probability distributions of interest. as a result, this filter may diverge.recently, a new variant of the kalman filter, unscented kalman filter (ukf) 9!, which approximate the nonlinearity to a higher order, has been proposed. the filter is founded based on the intuition that it is easier to approximate a gaussian distribution than arbitrary nonlinear functions. it may result in a more accurate filter than ekf. ukf can generate much better estimates of the covariance of the states. because the above two filters are based on the kalman filter, they cant get rid of the assumption of gaussian distributions.the popular solution strategy for the general filtering problem is to use sequential monte carlo methods, also known as particle filters10. these methods allow for a complete representation of the posterior distribution of the states, so they can deal with any nonlinear dynamic. however, in order to get a good estimation, many particles are needed. a new particle filter that uses ukf to generate the importance proposal distribution has been proposed1. this novel filter, which can be called asupf, tai greatly decicase the number of particles but still guarantee the estimation accuracy.the stochastic algorithms mentioned above often need some prior knowledge of noise distributions, and are not proper for those applications that require guaranteed uncertainty bounds such as many robust and optimized control customization approaches. the set-membership filters based on unknown-butbounded noise assumption have more advantages under these conditions1213. for nonlinear systems, an extended version of linear set-membership filter was proposed to guarantee nonlinear estimation14. this extended set-membership filter (esmf) linearizes the nonlinear dynamics about the current estimate in a way similar to the ekf. the remaining terms are bounded and incorporated into the algorithm as additions to the process or sensor noise bounds. this allows the solution to be guaranteed for nonlinear systems so long as the bound on the nonlinear term is guaranteed. esmf is compatible with many robust control methods that require hard bounds, and planning algorithms that require guaranteed uncertainty information. it is also computationally efficient for on-line recursive implementation.the rest of the paper is organized as follows: in section 1, the kinematic model with slip is established and the problem of nonlinear joint estimation is described; in section 2, algorithms of four filters are described; the simulation results are showed and compared in section 3; in section 4, conclusions are drawn and future work is proposed.1 kinematic model1.1 kinematic modelingin this section, a general kinematic model with slip of tracked vehicles will be developed. the slip is described by three time-varying parameters. fig. 1 shows the platform of the vehicle undergoing general plane motion.in order to describe the motion of the tracked vehicle, a fixed reference frame f(xw, yw) and a moving frame f2(xm ym) attached to the vehicle body with origin at the geometric center om of the vehicle are defined. the motion of the vehicle is composed of the translation velocity vc=(vx vy)t and the rotational velocity fflfc=d yr/fa, where vx and vy stand for the projection of the vc to axes of the frame f2, and y is the yaw angle. vy is termed with the side-slipping velocity. / is the instantaneous centre of rotation. because of the sideslip, it often shifts ahead 0mby a mount d. an angle a exists between the line connecting / to om and the perpendicular of/to x axis of frame f2.in the frame f2, a suitable model with slip can be written as in the simulation and experiments, x, y and if are the measurements.1.2 statement of joint estimation problemin joint estimation151, states and parameters are estimated simultaneously using a same filter. it is often used to solve the state feedback control with uncertain parameters, or the modeling of the parameters with noise and states that cant be measured directly. because of the incorporation of the states and the parameters, more accurate results may be made using this approach. in the localization of the tracked vehicle with slip, the pose and the slip parameters should be estimated at the same time. a new state vector is defined as a combination of the old state and parameter vector, x, = x y yi ir ctt. in this augmented state, the dynamic of the slip parameters are often unknown. they can be rewritten as e r is the additive process noise which drives the model.2 nonlinear filters for estimationthis section will introduce four different nonlinear filters: extended kalman filter, unscented kalman filter, particle filtewith ukf proposal, and extended set-membership filter. the algorithms will be described with respect to the following general nonlinear systemwhere wk and v*+i are process and measurement noise with covariance qt and rt+u respectively. the initial state vector is defined as *0-2.1extended kalman alter (ekf)the ekf is a minimum mean-square-error (mmse) estimator based on the taylor series expansion of the nonlinear functions around the estimated states. using only the linear expansion terms, it is easy to derive the following update equations for the gaussian approximation to the poster distribution of the states.given the estimation x, and the covariance pt at time k,a=0,1, , then at time k+, the ekf algorithm can be summarized as follows.note that the nonlinear system is linearized only when the covariance and filter gain coefficients are computed. for the state estimation propagation, the original nonlinear model is used.2.2unscented kalman alter (ukf)unlike ekf, the ukf does not approximate the nonlinear process and observation models. instead, it uses the true nonlinear models and approximates the distribution of the state random variable. the ukf, which does not need to compute the jacobian, uses the so-called unscented transform (ut) and sigma points to propagate all of them through models. as a result, the ukf often leads to more accurate estimations than ekf. the ukf algorithm is given below.(1) initialize withwhere l is the dimension of the augmented states; a controls the size of the sigma point distribution and should be ideally a small number to avoid sampling non-local effects when the nonlinearities are strong; and ft is a non-negative weighting term, which can be used to acknowledge the information of the higher order moments of the distribution. for a gaussian prior the optimal choice is ft = 2 , and to guarantee positive semi-definiteness of the covariance matrix, k 0 is chosen. the rest parameters are defined as note that the above equations are for the most general form of the ukf. for some special cases such as the model is linear, the algorithm can be simplified significantly.23 unscented particle filter (upf)the advantage of particle filter compared to kalman-based filter is that there is no gaussian approximation assumption. however, there are still some other problems. too many particles will increase the time cost greatly, while fewer particles will result in a bad estimation. moreover, the particles generated on each step dont take into account the measurements. to solve these problems, a new particle filter with ukf proposal has been developed recently nl. this filter uses ukf to generate the important proposal distribution. in comparison with general particle filter, upf tends to generate more accurate estimation with fewer particles. it is more suitable for online estimation. the algorithm is described as follows.(1) initialization2) select step: multiply/suppress panicles with high/low importance weights to obtain random psrticlos3) take some optional steps to improve the performance, such as mcmc sveb.(3) output ths founc estimationsfor the degeneration problem, re-sampling or other optional step- s.iould be required in the above algorithm.2.4 extended set-membership filter (esmf)all above stochastic nonlinear filters are based on bayesian theory, and some prior information on the distribution of the noises must be known ahead. unfortunately, nothing but the hard bounds of the noises often can be assumed. this kind of noise is called unknown-but-bounded noise. in addition, current robust control and coordination approaches, including on-line optimizing algorithms, require bounded uncertainty information in their formulations in order to guarantee closed loop stability and performances. this leads to the use of a kind of method known asset-membership or set-valued estimation. for a general class of nonlinear systems, an extended version of the linear set-membership filter has been developed li4 this approach is to cast the nonlinear dynamics in a way that allows the advantage of the linear smf framework can be taken. specially, the nonlinear dynamics is linearized about the current estimation; the remaining terms are then bounded using interval analysis and incorporated into the process noise or measurement noises. then a linear smf algorithm is used and an ellipsoid estimation set is made. the true value is guaranteed to lie in the ellipsoid.an ellipsoid set can be defined as and the two ellipsoid bounds of the two noisesthen at time k+, the esmf algorithm can be summarized in the following steps.(1) calculate the state interval based on the ellipsoid extrema where the superscript i,j stand for the (i,j) elements of a matrix.(2)find the maximum interval for the lagrange remainderestimate both states and slip parameters simultaneously. the using interval analysispurpose here is to compare filter performances and find out weakness of the different approaches. the platform used in studys a tracked vehicle with two arms (fig. 2).(3)calculate the ellipsoid bounding the linearization errorthe observation model should be dealt in the same way as attaining the incorporated measurement noise(5) calculate the predicted situ; ellipsoid using the linear smf filter step(6) calculate the updated state ellipsoid using the linear smf filter stepspecial attention should be given to the two following problems: the values of three parameters pq, p pk can be optimized at each time step in an effort to find the smallest bounding ellipsoid. some criterions such as maximizing the volume of ellipsoid or minimizing the trace of the bounding matrix can be used. when (5*0, the state uncertainty ellipsoid is not defined. this will occur when the basic algorithm assumption is not satisfied. thus, it is a good check for the health of the algorithm.3 simulationin this section, the four filters mentioned above will be applied to the kinematic model of the tracked vehicle with slip to the sensors equipped on the vehicle include 2 accelerometeis. 2-dof electrolytic tilt sensor, a pair of cameras, a gyroscope and a gps. the vehicle has the ability of striding over some obstacles, and can localize itself in both 2d and 3d environments, although only planar motion is considered in the study.in the model, the angular velocities of the two wheels that drive the two main tracks are considered as input variables. the constants in the model are chosen as follows: the width of the two main tracks is 6=0.65 m; and the radius of two wheels is r=0.35 m. the three states about the vehicles pose are measurements. the total time of the simulation is chosen as f = 20 s, and the time-step is 0.1 s. the noises used in the model, including the process noise and the measurement noise, are assumed to have two forms: one is the gaussian noise with zero mean value and 2% variance of the nominal values; the other is the ellipsoid bounded noise whose bound is the same as the variance of above gaussian noise. in fact, the points selected in the ellipsoid of the latter one are uniformly distributed. in order to verify the efficiency of filters, some sudden changes are assumed to occur in three slip parameters in the course of moverment.in order to achieve satisfactory result, proper tuning of the filters is essential. the values in q, and /fj+i for kalman based filters should be determined carefully. for ukf, the three constant filter parameters are chosen as a = 1, /? = 2, y = 0. in the upf, the number of particles is 20, and the other parameters are selected as the same as ukf. in the esmf, 3 a bound is used for the gaussian noise.figs. 3a3d show the results of the yaw angle estimation for gaussian noise. results for other states are not potted because they are similar to this state. in each figure, the real line is the true values (for simulation purpose) of the state or parameter variable. the dashed is the estimated value of the filter. the two dash-dotted lines are the estimated upper and lower bounds. the four sub-figures are results related to the four different filters. note that the bound for upf is not drawn because it is similar to ukf.fig. 4 shows the estimations of the sideslip ratio a for gaussian noise. other estimations results of states and parameters are similar and omitted for simplification. in each subfigure, the result for the four different filters are shown and compared.fig. 5a to fig. 5d show the results of the yaw angle estimation for gaussian noise. fig. 6 shows the estimations of the sideslip ratio a for ellipsoid bounded noise. results for other states are not potted because they are similar to this state as fig. 3. in each figure, the meanings of all lines are the same as fig. 3.in order to compare the efficiency of the four filters, the prediction accuracies ex and e2 are defined by the following root-mean-square performance criteria.note that the two equations are both used for the purpose ofsimulation, because the true states and parameters are unknown inpractice.another importance performance of the filters is the cpu time consumed. this performance should be considered carefully for on-line estimation of filters.the above three criteria are listed in tables 1. ?. tble 1 is the performances of filters for gaussian raise. table 2 is the performances of filters for ellipsoid bounded noise.the simulation results show that the four filters can all achieve tracking purpose. but from the view of accuracy, ukf and upf have best result, while the difference between them is very small. the estimation given by ekf is not as good as the above two filter, but better than esmf.compared to ukf, more errors are introduced in ekf because of linearization resulting in worse estimation. sometimes the ekf even can diverge for strong nonlinear condition. ukf uses the unscented transform to approximate the true nonlinear system directly and often generate a significant improvement in performance. for example, in the estimation of the sideslip parameter, the ukf can track the sudden step changes immediately while ekf takes more time.the upf is the combination of the ukf and the classic particle filter. with the same filter parameters, the upf can achieve similar results to the ukf, sometimes even better. the most important improvement is that fewer particles are used in upf than the normal pfs. this means less time elapsed and more practical to online usage. moreov