Estimation of the speed from the odometer readings using optimized curve-fitting filter

— Odometry is one of most-used techniques used in mobile robotics and autonomous vehicles, especially when the indoor navigation is required or when robot or vehicle moves inside the tunnel. The output of the odometer is usually a count of pulses corresponding to the distance run by given wheel. Due to the quantization noise, estimation of the velocity (first derivative of the distance) is challenging. This paper is focused on curve-fitting filter used for the speed estimation and optimization of its parameters, considering the physical constraints of the robot, sampling frequency of the system and the quantization step. The paper proposes an empirical formula for estimating the optimal parameters of the curve fitting filter. The optimized filter has been evaluated using both simulation and real experiment and compared with several standard differentiation methods.


A. Curve Fitting Methods
These methods assume that the distance travelled by the vehicle or robot can be approximated by some smooth analytical curve. The fitting algorithm considers limited number of recent samples from the odometer, which makes it the finite impulse response (FIR) system. Authors in [1] applied simple backward Euler differentiator to estimate the speed of the train from the odometer readings and improved the precision by the sensor fusion with inertial sensors. Euler differentiator assumes that the distance between two samples develops linearly. Wagner et al. (see [2]) compared the performance of multiple numerical differentiators applied to detection of a fall from the measured depth. They obtained the best results from regularized (adaptive) central derivative method, which is a form of Euler differentiator. When estimating the rate of temperature, the time series obtained by the sensor can be approximated by exponential function as proposed by authors in [3]. Authors in [4] filtered the odometer readings by conventional FIR filter and then used the result for compensation of inertial navigation errors. Taylor polynomial can be easily applied for the signal approximation (see e.g. [5]).

B. State Space Methods
The state space methods rely on the model of the system. Their advantage is that they can detect abnormalities in the measured signal, but the parameters of the model must match the parameters of the real system. Authors in [6] proposed a high-gain observer for estimating the derivative of the noisy signal. The standard state space-based method is Kalman filter [7], applied e.g. in [8] [9]. Jinasena et al. proposed a modelbased estimator to obtain the flow rate of drilling fluids from the measured fluid level [10]. Gang et al. detected the anomalies of the odometer signal caused by wheel slippage by comparing the measured wheel speed with the speed estimated by model of the robot with 4 Meccanum wheels [11].
If we need to determine the speed from the historical data, offline iterative techniques can be used. Total-variation regularization can be applied for differentiating noisy signals [12].

II. SHORT ANALYSIS OF EXISTING METHODS
The simplest method for real-time differentiation is the Euler method: where x(n) is the n-th sample of measured distance, v(n) is the estimated velocity and T is the sampling period. Such approach can be used also for multidimensional signals [20]. Precise estimation of the derivative of a discrete signal is a challenging task since the differentiation amplifies the highfrequency noise. Above mentioned formula has very poor precision. Sometimes a conventional frequency-limiting FIR (Finite Impulse Response) or IIR (Infinite Impulse Response) filters are used to filter-out the resulting noise (see e.g. [13][14] [15]).
The main source of the noise in odometry is the quantization noise. Each odometer has only limited number of steps per one turn of the wheel. The quantization is therefore a down-rounding. The error e of the odometer is in the range [0, Δx) where Δx is the distance between two steps of the odometer. The probability of the error is distributed uniformly, therefore the probability density function (derivative of the probability distribution function) is constant: The RMS of the quantization noise is: The frequency spectrum of the quantization noise is uniformly spread across the whole range of frequencies (0, Fs/2), where Fs is the sampling frequency of the system. Therefore, the conventional frequency-limiting filters are not very suitable for the filtration of the quantization noise.
Better performance is achieved by higher order FIR filters [17]. The simplest higher-order differentiating filter is the extended backward Euler differentiator: x n x n N vn NT where N is the order of the filter. Such filter has better performance than (1), but greater delay. Liu proposed a simple adaptive method which increases the order of the filter while the difference between two samples is not great enough which improves the precision at low speeds [18]. Filters like (4) do not consider the samples between the first and the last sample of the processing window. A maximum flat differentiating FIR filter removes this drawback, and its coefficients are computed recursively [17]: The output of the filter is: The order of the filter N = 2M. Using Parks-McClellan optimization method, it is possible to obtain even better results [17] [24]. Authors in [21] proposed the usage of extended Kalman filter (EKF), which had better performance than Luenberger observer and conventional differentiation. The usage of the EKF requires additional information from the system (accelerometer or gyroscope readings, error models of the sensors) [22]. However, practical experiences shown, that accelerometers are even more sensitive to the vibrations of the robot than the odometers. Backward difference can be expanded into higher order linear filter (Backward Difference Estimator) [23]. This approach can be used in both value and time domain. For the purpose of the velocity estimation from an incremental odometer, it seems better to use fixed position BDE estimator, where we measure the time intervals between two steps of the odometer. Equation (8) shows 3 rd order BDE estimator: where Δt(n) is the time interval between n-th and (n-1)-th occurrence of the odometer pulse. However, such approach is more difficult to implement, because it requires the precise measurement of time difference. Most of the discrete control systems operate at constant sampling frequency [16], therefore it is easier to use fixed time BDE estimator: where: The fixed time BDE estimator in (9) can be rewritten in the form of a FIR filter with coefficients: Authors in [19] applied several types of polynomial curvefitting filters in order to estimate the velocity from encoders. Such filter assumes that the input signal within the processing window can be approximated by polynomial: where ak(n) is k-th coefficient of the polynomial for n-th sample. Considering that the sampling is uniform (t = nT), we obtain: Now we need to estimate the coefficients of the filter in order to minimize the error function: The formula (13) can be rewritten in matrix form: Using pseudoinversion we obtain: Note that the matrix K is constant. The estimated output of the filter is the value of the polynomial (13) at the current sample (i=0): The derivative of the input signal is: . (18) Note that the vector of filter coefficients b is also constant, therefore the filter is a FIR filter.

III. PHYSICAL CONSTRAINTS OF THE WHEELED MOBILE ROBOT
The most important fact is that the speed of the wheel is a state variable, therefore the rate of change is limited. Maximal acceleration of the vehicle is proportional to the friction coefficient µ between wheels and the floor surface: where g is the gravitational acceleration. The acceleration of the vehicle has two components: longitudinal and lateral. Longitudinal acceleration is caused by the propulsion / braking system of the robot. Lateral acceleration is caused by turning. It is not economical nor practical to change the acceleration of the robot too rapidly. Vibrations of the robot may have high 3 rd order derivative, but they are not captured by odometers. Hence we may assume that in real conditions the rate of the acceleration is also limited: Our research shown, that the maximal rate of acceleration has the critical impact on the precision and performance of the velocity estimation.

IV. SIMULATION OF THE ODOMETER
In order to optimize the parameters (M, N) of the fitting filter, we need to calculate the errors of the differentiating filter at full scale of max a , sampling period T and the quantization step Δx. It is reasonably possible to achieve only by simulation, for which we have used MATLAB. For simplicity, we have assumed that the acceleration is a uniformly distributed random number. Then we have limited its rate by the Algorithm 1.
During the simulations, the friction coefficient was µ = 0.4 and the maximal rate of acceleration max a was from the range [0.01, 2]. The quantized odometer signal is computed using floor rounding function.  Our aim is to determine the optimal parameters M, N from given sampling period T, maximal rate of the acceleration max a and the quantization step Δx of the odometer.
We have evaluated all possible combinations of parameters in ranges M = 1 ÷ 5, N = 3 ÷ 200. Note that not all combinations are usable, because the order of the filter N has to be greater or equal to the order M of the curve. The RMS error of the estimated velocity was averaged across 100 simulated experiments, each containing 1000 samples. Then the set of parameters M, N with the smallest error has been chosen. First, we set constant sampling period, constant quantization step and variable maximal rate of acceleration within the range max a = 0.01 ÷ 2 m.s -3 . Based on our previous experiments, the real acceleration rates of vehicles and mobile robots are much higher than 0.01 m.s -3 . The optimal parameters of the curve fitting filter are shown in Fig. 1.
The optimal order of the fitting curve is M = 2 (parabola) in the whole range of the maximal acceleration rate (µ = 0.01 ÷ 1.0). The optimal order of the fitting curve is therefore equal to the order of the last constrained derivative (the acceleration rate is the second derivative of the velocity). If the third derivative of the velocity would be also limited, the optimal order of the fitting curve is 3, but there is no reason why the third derivative of the velocity should be limited based on analysis of the vehicle's dynamics. The optimal order of the filter can be approximated by following empirical function: The match between the simulated values and the empirical function is very tight.
In the second series of simulated experiments, we have used constant limit for the acceleration rate and tested different values of the sampling period in the range T = 0.005 ÷ 0.1 s. The obtained optimal order of the curve fitting filter and its approximated value is in Fig. 2.
The function can be approximated by following equation: In the third dimension, we set constant maximal rate of the acceleration 1m.s -3 , constant sampling period T = 0.1 s and variable size of the quantization step in the range Δx = 0.001 ÷ 0.05 m. The function in Fig. 3 can be approximated by equation (23).
Considering (21) -(23) we obtain the hypothesis: where C is a constant. Using the simulation, the value of the constant was estimated to C = 3.2±0.1 at the whole considered range of parameters T, Δx, ȧmax. The RMS error between the simulated and computed values was RMSE(N) = 1.26.

VI. GENERALIZED METHODOLOGY
Above mentioned approach for finding the relation among the optimal parameters of the filter and the parameters of the signal and noise can be applied in many scenarios. The methodology is following:

Analytical approximation of cuts by a regression function
In this article, we have used the power function qj = K.pi α , where K and α are constants for given cut, pi is the i-th variable parameter of the signal and qj is the optimal value of the j-th parameter of the filter.

Repeating steps 3 and 4 for each parameter of the input
signal. If all cuts can be approximated with power function, then the whole space of the signal parameters can be approximated with the power function:  The methodology cannot be applied automatically since it requires the knowledge of the system, good selection of the experimental cuts and regression functions. If the space of the signal parameters is not too large, we may simulate each set of signal parameters and the optimal parameters of the filter can be approximated by multi-dimensional regression. Such approach may require huge amount of computation power and time.

VII. FILTER PERFORMANCE
In order to determine the performance of the designed optimal curve-fitting differentiator, we have compared its RMS error with other differentiators mentioned in the introduction. For each type of the differentiator, the optimal order of the filter has been chosen (achieving minimal RMS error). The parameters of the test signal are shown in Table 1.
The results of the comparison are shown in Table 2.
For comparison, Fig. 4 shows the frequency response of the optimized differentiators. It shows the gain of the filter with respect to the frequency of the input sine wave. Ideal differentiator has linear frequency response, but it would have very poor performance for noisy signals, since it amplifies higher frequencies. Note that Parks-McClellan method provides best match with the ideal differentiator, but in the application of odometer outputs poor results. The good match between the frequency characteristics of the filter and the ideal differentiator is therefore not relevant criteria for selection of the differentiator for quantized or noisy signal. The optimal differentiator of the noisy signal should suppress higher frequencies.

VIII. EXPERIMENT
The optimal behavior of the curve fitting filter has been proved by simulations. However, it is necessary to show, that the properties of the simulated odometer signal match the properties of the real odometer signal. The odometer signal was obtained from e-puck robot from Cyberbotics [25]. The robot's odometers count pulses of its stepper motors. If we assume that the wheels of the robot do not slip, the count of motor pulses is proportional to the distance travelled by the wheel. The frequency of the motor steps is set by user or by an external controller. Experimental signal from one odometer is shown in Fig. 5. The reference robot speed was set to Comparison of the filters is in Table 3. The optimal orders of the filters were chosen by testing all values for N = 3 ÷ 200 and selecting the one with lowest RMSE.

Count of Samples -
The optimal order of the curve-fitting filter computed by equation (24) is 40, which is a good match between with the experimentally obtained value.
As can be seen in Fig. 6 and Fig. 7, the curve-fitting filter has smaller delay and is slightly more precise than the optimized extended backward Euler differentiation.
The velocity obtained by from the odometers can be applied in the well-known localization system with inertial sensors (gyroscope, accelerometer). In such systems, the accelerometer provides measurement of the gravity acceleration vector, which expresses the vertical direction. However, own acceleration of the object (robot) is negatively affecting the estimation of the vertical direction.
We have applied the proposed differentiator twice in order to obtain the forward acceleration of the robot. The IMU (inertial measurement unit) was mounted at the top of e-puck robot and provided measurement of the acceleration by MEMS accelerometer. Fig. 8 shows the acceleration measured by accelerometer and the acceleration computed from odometer readings. The readings of the accelerometer are affected by vibrations of the robot.
The roll and pitch angle (1 st and 2 nd Euler angles in Z-Y-X convention) can be computed from the accelerometer readings using: where ax,y,z are the axial components of the measured acceleration. Since the robot moved only forward, the lateral acceleration component ay was neglected. The acceleration obtained from odometers can be subtracted from the accelerometer's readings in order to compensate the own acceleration of the robot: The comparison between the results of (27) and (28) is in Fig.  9. The true pitch was zero since the robot moved on horizontal floor. As can be seen, the error of the pitch estimation was slightly decreased by the odometer. The improvement would be greater if the vibrations of the e-puck robot during the movement were smaller.

IX. CONCLUSION
Odometry is an essential part of mobile robotics, especially when the robot operates indoors or inside the tunnel, without available signal from the satellite localization. One of the important tasks is to determine the velocity of the robot or vehicle from the readings of the odometer. Readings of the odometer contain quantization noise, which cannot be filteredout using frequency limiting filters. Standard discrete differentiators have poor performance when applied to the odometer readings. As has been proven by both simulation and experiment, the sensitive adjustment of the differentiator parameters is crucial for improvement of the performance (lowering the error). The curve fitting filter provides good balance between the precision and the complexity of the design.
Main contribution of this article is the formula which computes the optimal order of the well-known curve-fitting filter from the sampling frequency, quantization step and the maximal rate of the acceleration of an odometrical system. The formula is empirical, obtained by multi-parametric simulation. The optimal order of the curve is 2; the fitting curve is quadratic.
Experiments with real odometer signal showed, that the curve-fitting filter has similar precision than the extended backward Euler differentiation, but it has smaller delay. Other types of discrete differentiators are significantly worse, because more the differentiator matches the ideal (continuous) differentiator, more it amplifies the quantization noise of the odometer.
The proposed method for designing of the curve-fitting filter can be used not only in odometry, but also in discrete flow measurement in industrial or intelligent traffic applications.