{'fraction_non_alphanumeric': 0.02790948691260938, 'fraction_numerical': 0.011256312401218148, 'mean_word_length': 4.702791822378544, 'pattern_counts': {'":': 0, '<': 0, '<?xml version=': 0, '>': 1, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 1, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
A Comparison of Robust Kalman Filters for Improving Wheel-Inertial Odometry in Planetary Rovers

This paper compares the performance of adaptive and robust Kalman filter algorithms in improving wheel-inertial odometry on low featured rough terrain. Approaches include classical adaptive and robust methods as well as variational methods, which are evaluated experimentally on a wheeled rover in terrain similar to what would be encountered in planetary exploration. Variational filters show improved solution accuracy compared to the classical adaptive filters and are able to handle erroneous wheel odometry measurements and keep good localization for longer distances without significant drift. We also show how varying the parameters affects localization performance.


I. INTRODUCTION
One of the key problems in robotics is to estimate states like robot poses, locations of landmarks and sensor calibration parameters using the measurements from sensors. Due to inherent uncertainty in the measurements, system and measurement noise has to be taken into account, which are modelled using probability distributions. One of the most common noise distributions used in these problems is the Gaussian distribution [1]. Bayesian filtering [2][3] helps in estimating the posterior of the state given the measurements. For the linear Gaussian systems, this posterior has tractable analytical solution in the form of the well-known Kalman filter [4] which can also be extended to its nonlinear variants. But this assumption is not always valid and might lead wrong inference on the parameters. The Gaussian assumption leads to least-square problem which is highly affected by outliers. This is because of the unbounded nature of the squared error function.
In this paper, we modify the approach presented in [5] with robust filtering algorithms. The formulation in this paper is based on [6] and is an error-state Extended Kalman Filter (EKF) which uses Inertial Measurement Unit (IMU) measurements for error propagation and wheel odometry measurements as corrections. It also utilizes non-holonomic constraints and zero-velocity updates to reduce the impact of the IMU drift and wheel slippage. The advantage of this method is its low computational cost since it does not depend on cameras for visual odometry, which is suited for planetary rovers. It can also help in localization in places where visual odometry might fail, like shadow regions or places lacking visual features [7]. By adding some of these existing methods, we extend upon the current approach to increase the robustness and accuracy of localization solution in presence of erroneous wheel odometry measurements. This paper offers the following contributions 1) it reviews several state of the art robust Kalman filters side-by-side within the same context 2) it offers an experimental evaluation of the methods discussed using a relevant platform and publicly available data-sets 3) the accompanying software implementations are made available in GitHub .
The rest of the paper is divided into six sections. Section II reviews relevant literature on robust filtering. The base algorithm is discussed in the next section followed by a summarised theoretical discussion of the methods which are evaluated in section IV. Sections V and VI contain description of the experimental setup and software implementations. The results are presented and analyzed in section VII and concluded with section VIII.

II. RELATED WORK
There is a rich body of literature that has considered modifying the standard Kalman filter to accommodate outliers. A covariance scaling method is introduced in [8] using the ratio between the Mahalonobis distance between the observed data and the predicted data and the critical value of χ 2 distribution to reduce the effect of observation outliers or heavy-tailed measurement noise distribution. An adaptive factor is defined in [9] that helps the filter to balance the contribution of discrepancies due to dynamic modelling errors and new measurement as well as measurement outliers. [10] uses an adaptive innovation-saturation mechanism to estimate the changing measurement error bound and saturate the innovation process. An adaptive version of the Kalman filter is derived in [11] using a fading factor and the current innovation and residual values to recursively calculate process and  [13]. Similar method has been used with tightly and loosely coupled GNSS/INS EKF with simulated faults in [14], where the robust version has comparable performance to that of fault detection and exclusion techniques. [15] derives a robust filter using the Double-Gaussian Mixture Correntropy Loss between the data and the predicted measurements. Variational inference has also been used to approximate state posteriors with heavy tailed distribution for the measurement models in [16,17,18,19,20]. Faulty measurements are excluded using indicator variables with beta-Bernoulli prior and approximated with the state using mean-field variational inference in [21] in a simulated target tracking scenario. Bayesian weights are assigned to each measurement and estimated along with the filter parameters using Expectation-Maximization (EM) framework in [22].

III. THEORY AND EQUATIONS
In this section, we describe the Kalman filter algorithm and CoreN av [5,23] which we use as our base algorithm. The Kalman filter is the most well-known Bayesian filtering algorithm. It is a maximum likelihood estimator for the linear Gaussian model. The filter equations arex k/k−1 = F kxk−1/k−1 (1) x k/k =x k/k−1 + K kvk (5) wherex k/k is the current estimate of the state using the measurement z k . F k , H k are the state transition and measurement matrices respectively. Matrix P k/k represents the state uncertainty, Q k is the process noise covariance matrix and R k is the measurement noise covariance matrix. The filter gain at time k is denoted by K k .
As detailed in [5], CoreN av is an error state EKF with the state vector represented in the local navigation frame by where δΦ n nb is the error in the attitude of the rover from the body frame (b) to the local navigation frame (n) expressed in the local navigation frame, δv n eb is the velocity error in the navigation frame, δp b is the position error. b a and b g are IMU acceleration and gyro biases respectively. The position error vector is which is expressed in latitude, longitude and height frame. The states are propagated forward with measurements from IMU and corrected with wheel odometry measurements. The measurement z k is also an error vector represented as whereψ nb and cosθ nb are heading rate and pitch angle of the rover body frame with respect to the navigation frame respectively. v lon ,ṽ lat , andṽ ver are predicted longitudinal, lateral and vertical rear wheel speed, respectively. The subscript i is used for values derived from INS solution, and O is used for values derived from the wheel odometry measurements. Motion constraints, such as zero velocity of the vehicle along the rotation axis of the wheels, and zero velocity perpendicular to the traversal surface are used as pseudo measurements to update the state vector [24]. It also utilizes zero-velocity-constraints [25], i.e detects it has stopped using wheel velocity data and uses that information to maintain Inertial Navigation System (INS) alignment and reduce the rate of error growth.

IV. ROBUST AND ADAPTIVE FILTERS
In this section, we provide a brief description of the robust and adaptive methods that have been evaluated in this paper. Acronyms have been defined (inside parenthesis) for ease of presenting the experimental results.

Huber Regression Kalman Filter (HKF)
M-estimators [26,27] are maximum-likelihood estimators for certain specific error distributions that differ from standard Gaussian distribution. They have been implemented successfully in many robotics applications due to lower sensitivity and high breakdown in presence of outliers [28]. An iterative version of M-estimation has been discussed in [13,12] which uses the Huber cost function [29] at every time step instead of the squared error function. This causes the error between the predicted measurement and the actual measurement to grow like the squared function when below a parameter ∆ and linearly when above it.
The following derivation follows [30]. A single filter prediction and update cycle can be written as a least squares problem where

Now the new linear equation can be written as
where Least squares solution of the modified linear equation iŝ S k can be calculated by Cholesky decomposition of the covariance matrix of E k .
However, least squares is highly effected by outliers due to unbounded nature of the squared error function. So, a different cost function can be usedx where m is the dimension of Y k , y ik is the i th element of Y k and b ik is the i th row of B k . If ρ is quadratic, then it reduces to the least squares and the Kalman filter solution. But this function can be set such that the effect of large residuals is reduced. The algorithm is optimal for the Gaussian case. Thus, the function ρ can be squared for smaller values of the argument, but should increase slowly for larger values. Another important requirement is that the influence function ψ be continuous and bounded. It should be bounded so that no single measurement can affect the total cost largely and continuity helps in reducing rounding errors. One such function is the Huber cost function [29] ρ Other robust functions are discussed in [28]. Let p be the dimension of x k . To minimize the cost function in (12), differentiate J and set it to zero , which is a m dimensional diagonal matrix. Then (9) can be written as This equation cannot be solved explicitly since Ψ depends on the unknown state x k . However x k can be solved in an iterative manner using iterative re-weighted least squareŝ This solution converges if ψ is non-increasing. The iteration can be started with the least-square solutionx 0 After the state has converged, the covariance can be estimated by the expression Choosing the tuning parameter ∆ is important, and depends on the perturbing parameter , which represents the proportion of contamination in the assumed residual distribution [30]. If the perturbing parameter is known, then the optimal choice ∆ * is given in [27]. When the perturbing parameter is unknown, ∆ is usually chosen between 1.0 and 2.0.

Covariance Scaling Kalman Filter(CSKF)
The Mahalonobis distance ratio defined in equation (20) can be used for assessing if the measurement is inlier or outlier. Following [8], the measurement noise covariance matrix or the measurement prediction covariance is scaled for every outlier detected using χ 2 criterion.
M k is the Mahalonobis distance of the actual measurement from the predicted measurement. Assuming that the measurement follows a Gaussian distribution, the test statistic that can be used to test if the measurement is an outlier is the critical value of the χ 2 distribution corresponding to significance level α with degree of freedom equal to the measurement dimension, χ mα . This is also used as the Independent Innovation Test in [31].
The scaling factor γ k is used to modify the measurement noise covariance matrix R k . If γ k is less than 1 then measurement Y k is an inlier and R k remains the same. But if scaling factor is greater than 1, denoting an outlier, the inflated R k is given by [8] The scaling of R k results in de-weighting the suspected outlier measurements. The rest of the steps remain same as the standard filter equations,i.e replaceR k in place of R k in (4), (5), (6).

Variational Filters
Variational Bayes filters [32] use a prior for auxiliary variable in the measurement model such that the marginalized measurement distribution has heavier tails. Due to the non-Gaussian nature of this model, state posteriors have to be approximated using variational inference [33]. The state and the auxiliary variables are estimated iteratively until convergence. The most common method in variational inference uses the mean-field assumption to factor the joint distribution of the state and the auxiliary variable as a product of each of the variable distributions. Then, the posterior of both state and auxiliary variable are approximated by minimizing the Kullback-Leibler divergence [34] between the posterior and the product (i.e., maximizing the lower bound).
Here we have implemented the variational inference filters presented in [17], [18] and [20]. All three methods share the same inference method, they only differ in the auxiliary random variable that is considered in the measurement model. [17] assumes R k has a inverse-Wishart [33] prior where R is the nominal measurement noise, s > d − 1 is the degree of freedom, d the dimension of the measurements. The measurement model is Gaussian, Variational inference approximates the posterior of the state q(x k ) with the standard filter correction step i.e Gaussian and q(R k ) as where This posterior from [17] has an elegant interpretation. The posterior noise matrix is a convex combination of the nominal noise matrix and the expected sufficient statistics, s denoting the relative importance between the two.
[18] assumes R k is fixed but scaled by a scalar random variable which follows a Gamma distribution [35] and the measurement distribution is Similar to the previous method, variational inference approximates q(x k ) with the standard filter correction step and q(λ k ) as The expectation inside the trace operator can be approximated using sigma-point operations [36]. The posterior in equation (27) makes sense when examining the mean which can be written as For the case of inliers, the measurements will be closer to their corresponding predictions resulting in smallerγ k , thus larger λ k and thus smaller uncertainty(larger weights). The opposite would happen for outliers resulting in largerγ k , thus smaller λ k and thus larger uncertainty(smaller weights).
The common aspect of both these methods is that after marginalizing out the auxiliary variable, the resulting measurement distribution has heavier tails (in this case, it is the Student's t distribution [33]). Both s in [17] and ν in [18] represent the extent of heavy-tailedness of the distribution, with s, ν = 1 being the Cauchy distribution [35] and s, ν → ∞ denoting the Gaussian distribution. [20] assumes inverse-Wishart priors for both state uncertainty matrix and the measurement noise covariance matrix, which is assumed to be slowly drifting and following the same inference method as above. Readers are referred to [20] for further details. In the results sections, [17] is represented as ORKF 1, [18] as ORKF 2 and [20] as ORKF 3 (ORKF stands for Outlier Robust Kalman Filter). We only implement filtering versions of these algorithms and not the smoothing since we are only concerned with the real-time solution in this work.

Adaptive Loss Functions
Similar to the Huber loss function described in HKF, other robust cost functions have been defined in statistics literature like the Charbonnier, Cauchy, Geman-McClure, and Welsch losses. [37] defines an adaptive version of the cost function which assumes the forms of these different loss functions based on the value of a tuning parameter α. They optimize a modified version of this cost function with an added regularizer so that higher values of α de-weights outliers more but adds penalty for inliers, and lower values of α de-weights outliers less but also adds less penalty for inliers. Joint optimization in this filtering problem can be hard due to the high frequency of the IMU measurements, resulting in missed data. Similar to graduated non-convexity method discussed in [38], we can solve equation (17) iteratively with annealing the parameter α after a fixed number of iterations. We leave this for future work.

V. EXPERIMENTAL SETUP
The field tests for the method evaluation is done with a testbed rover (Pathfinder) (see Fig. 1). The IMU, wheel encoder, and GPS receiver used in the rover, as similar to the work in [5,23], are provided in Table 1. To determine the reference truth solution, differential GPS (DGPS) is used. This solution is obtained by using two dual-frequency GPS receivers and Pinwheel (L1/L2) [39] mounted both on a GPS Base Station and on the testbed rover. A loosely-coupled GPS/IMU fusion algorithm in [5] is used for the state initialization process. The GPS measurements (carrier-phase and pseudorange) are recorded on both rover and base station receivers. The reference truth solution is generated by post-processing on RTKLIB 2.4.2 software [40]. Note that, DGPS accuracy is expected as cm/dm level [41]. To compare the above presented methods, we build upon the implementation in [5] which uses Robot Operating System (ROS) [45] for collecting data from sensors. As discussed in section III, the base algorithm is an error-state EKF which uses IMU measurements (50 Hz) for state propagation and wheel odometry measurements (coming in at 10 Hz) as corrections. The state propagation step remains the same as CoreN av, whereas the correction step is modified to implement the robust methods. The HKF method depends on the parameter ∆. This parameter is determined by the degree of contamination in the assumed Gaussian error distribution, and empirically tuned, in that it is difficult to estimate for the application of four wheel odometery.
In this paper we selected that to be 1.5. The CSKF method does not require any tuning parameters, and the critical value of Chi-square distribution of degree of freedom 4 (size of measurement vector) with significance level of 0.05, which is 9.488 is used for outlier detection. In ORKF 1, we select s to 250. ν is set to 300 in ORKF 2. Following [36], the sigma-point calculations in (28) sets α to 1, β to 2, and κ to 0. Following notations in [9](referred here as ORKF 3), u is initialised to 2000, τ also 2000 with ρ set to 0.9999. All variational methods were run for 5 iterations. This low number is partly due to the fact that increasing number of iterations resulted in skipping IMU measurements which is received at a very high rate within the real-time ROS implementation. Skipped IMU measurements, in turn, resulted unreliable results when repeated with the same data.
For all implemented methods, parameters have been tuned for one of the data sets and re-used in the other ones, due to the similarity in the terrain and their trajectory shapes. The prior measurement and process noise remains the same for all methods. We talk about the data sets in the next section. They are also expected to vary from the ones mentioned above with different kinds of terrain requiring tuning from scratch. These parameters specify certain properties about the underlying process and measurement noise distribution which is discussed in the next section. In future work, we plan to learn these parameters from data acquired as the rover moves.

VI. RESULTS AND DISCUSSION
The algorithms mentioned in section IV including CoreN av, (HKF , CSKF , ORKF 1, ORKF 2, ORKF 3) are tested on the Pathtfinder platform ( Fig. 1) in three separate runs around a field of coal-ash residuals in Point Marion, PA. We use three data sets from [44] referred in the paper as test1 (ashpile_mars_analog1.zip), test2 (ashpile_mars_analog2.zip) and test3 (ashpile_mars_analog3.zip). This terrain was chosen to be reasonably representative of planetary terrains. The solutions of the algorithms and truth (DGPS) are shown in EN U (East − N orth − U p) frame, which is the local navigation frame with origin at the start location for each the data sets. Figure 2 shows the trajectories in the East-North plane. For ease of differentiating the trajectories, the figure shows only the methods that perform better compared to CoreN av for each of the data sets. The 3D norm error and U p error statistics are shown as box plots in Figures 3 and 4 respectively.  In data sets test1 and test3, CSKF and HKF have comparable performance to CoreN av and ORKF 1 and ORKF 2 outperforms all other methods. In test2, however, the variational methods perform much worse with ORKF 1 and ORKF 2 being the worst affected. CSKF and HKF produce good results, better than CoreN av in this case. We see a general improvement in U p (vertical) axis solution with the variational filters ORKF 1 and ORKF 2. It is important to understand what the parameters in each of these methods mean and their assumptions. In HKF, as discussed before the ∆ parameter in (13) depends on a perturbing parameter which represents the amount of contamination in the Gaussian error distribution. For ∆ = 0, the error distribution is assumed to follow the Laplace distribution [35] and ∆ → ∞ follows the Gaussian distribution. Thus the distribution of the innovations i.e (y ik − b ikxk/k ) values in (16) will determine the best value of ∆ at every time step. But with the less number of these innovations in each time step (15 states + 4 measurements = 19), it is hard to do so in this problem. However we do show experimentally that increasing ∆ results in convergence with CoreN av solution which assumes Gaussian error distribution ( Figure 5). This suggests all residuals in HKF with ∆ = 3.0 are bounded by the nominal R covariance ellipse.  CSKF assumes the large innovation is due to outliers in the measurements but could also be caused by wrong estimate of the predicted state. In that case, scaling the covariance matrix does not solve the problem. As discussed in [8], another disadvantage is that an outlier in one dimension of a measurement vector results in the de-weighting of the whole vector.
Variational filters also de-weight the measurements assuming heavy-tailed model which appears to be more effective in this case. Large value of s (250) in ORKF 1 points to more importance given to the nominal measurement noise covariance. This is confirmed by the parameter value of ν (300) in ORKF 2 which is close to the value of s in ORKF 1 since they essentially represent the same measurement model. Also the value of the λ when running the algorithm remains within the range (0.9, 1), which confirms our high belief in the nominal R value. Similarly in ORKF 3, ρ value close to 1.0 results in lesser effect of new residuals on the measurement noise estimate. The advantage of ORKF 3 is the ability to set the degree of belief in the prior state uncertainty for every epoch. The large value of τ represents higher influence of the state uncertainty of the previous epoch on that of the current epoch. This can be favourable if the state uncertainty of previous epoch was estimated correctly, but can get worse it is not the case. Overall, we see ORKF 3 performs more consistently than the other two variational filters over the three datasets. The anomaly in the results is test2, which in spite of being very similar to test1 and test3, causes ORKF 1 and ORKF 2 to perform poorly. One of the probable reasons could be the lack of convergence in the variational update step. One of the main problems with applying the methods discussed above is the lesser number of measurements in this scenario unlike GPS or visual odometry which results in less information about the underlying measurement noise distribution.
As mentioned before, the parameter s in ORKF 1 denotes the relative importance between the prior noise and sufficient statistics. There is another way of interpreting the effect of s. Looking at equation (24), the harmonic mean of the posterior distribution of covariance matrix is which can be rewritten as The first part of the left hand side expression is the prior noise which is not affected by the measurements and also relatively unaffected by s due to its ratio form. S k in the second part is large for outliers and smaller for inliers. Now considering only the case of outleirs, larger values of s will produce a smaller Λ k than smaller values of s, thus de-weighting outliers less. Similar conclusion can be drawn for inliers as well. Thus lesser value of s leads to more de-weighting of both inliers and outliers and vice versa. Also since s is the degrees of freedom of the Student-t distrbuted measurement model in [17], decreasing value of s makes the distribution more heavy-tailed and thus greater weighting. Table (4) shows better localization performance for larger values of s, which suggests the true residual distribution is close to a Gaussian.  Table (5) shows better localization performance for larger values of ν which agrees with results of ORKF 1. This can explain why Huber cost function is not able to improve performance in this scenario. Since the residual distribution has been found to be close to Gaussian, the contamination parameter will be close to zero. [30] shows the variation of optimal parameter in the Huber cost function with respect to , confirming ∆ values greater than 2 for close to zero. Thus greater values of ∆ produced better results for HKF , since it agrees more with the true measurement noise distribution.

VII. CONCLUSION AND FUTURE WORK
We extend upon the EKF in [5] and implement robust filtering algorithms to increase localization accuracy in presence of erroneous wheel odometry measurements due to wheel slip. These methods are evaluated on a rover and seen to generally have improved performance over the standard EKF. The variational filters have better accuracy than others in test1 and test3. The covariance scaling and Huber regression methods show improvements in test2. Results point to a residual distribution that is close to a Gaussian distribution. Future work will include testing with more data sets over longer distances and online parameter learning. Another promising direction of work could be the use of adaptive robust cost functions which adapt to the type of terrain or slope the rover is traversing.