An Increase in Estimation Accuracy Position Determination of Inertial Measurement Units

This paper deals with an increase in measurement accuracy of the Inertial Measurement Units (IMU). In the Inertial Navigation Systems (INS) a fusion of gyroscopes, accelerometers and in some cases magnetometers are typically used. The typical problem of cheap IMU is non-stationary offset and high level of noise. The next problem of IMU is a problem with a bumpy floor. For this case it is necessary to a have high quality chassis to eliminate additional noise. Also, it is possible to eliminate this noise by using some algorithm, but results are still poor. These properties lead to the inaccurate position estimation in the integration process. Even a small offset error leads to a big mistake in position determination and grows quickly with a time. This research is focused on the elimination of these poor properties and increase of accuracy of position estimation using Kalman Filtration.


Introduction
Precise navigation of mobile robots in unknown areas is common issue in much research.For the robot navigation in exterior the Global Navigation Satellite System (GNSS)is commonly used.For better results in outdoors is also possible to use the Satellite Based Augmentation Systems (SBAS).In Europe the European Geostationary Navigation Overlay Service (EGNOS)is usually used, while in US Wide Area Augmentation System (WAAS)is used.The benefit of using GNSS navigation systems is the achieved accuracy, typically better than 15 meters.Our research is focused on the autonomous navigation of a robotics platform in indoor spaces like ruins, mines, etc.The GNSS navigation system is not suitable for indoor areas because in the buildings navigation does not have a signal received from satellites or base stations.For these cases, it is necessary to develop an alternative navigation system.
The navigation of a robotics platform in indoor spaces, a modern navigation system called visual odometry can be used.This system is known from the eighties, but massive penetration is connected with fast Digital Signal Processors (DSP).This system is based on capturing a scene by the camera.The algorithm calculates interesting points from an actual and previous captured scene.The algorithm searches pairs of interesting points and afterwards calculate a distances between these pairs, called pixel distance.Also it is necessary to use some type of transformation to transform pixel distances to real coordinates.The distance estimation is performed using the common elevation angle of a sensing camera, the size of a sensing chip and focal length.This problem is described in detail in papers [1], [2], [3] and [4].An alternative navigation system is called Inertial Navigation System (INS).This system uses accelerometers and gyroscopes.High level of noise and non-stationary offset is typical for cheap measurement units.These properties lead to the inaccurate acceleration estimation in the integration process.This is typical especially for low-cost accelerometers.Even a small offset error leads to a big mistake in position determination and grows quickly with time.Normally, the high level of noise is possible to eliminate using advanced digital filters [5], but in this case we need a special filter like Kalman filter.In our research we are focused on the eliminating a wrong position estimation using a Direct Cosine Matrix, Odometrymeasurement and Kalman filter [6].The final device will be used as fusion of a navigation of robotics platform with an optical measurement device [7], [8], [9].

Direct cosine matrix (DCM)
The Direct Cosine Matrix provides transformation vectors in Euclidean space around an origin by an angle θ.In practical use it is necessary to have some rotation formalism to provide a rotation around three independent axes called Yaw, Pitch, Roll (in some cases called α, β and γ).These axes are shown in Figure 1.
Basic clockwise rotation around an x-axis by an angle γ in three dimension space can be written in a matrix form (1). Similar equations (2), (3) exists for an y-axis and an z-axis. (2) where R n is the rotation matrix defined on an axis and α, β and γ are angles of clockwise rotation.A final three-dimension rotation matrix is created by a simple matrix multiplication (4).

Position estimation correction using DCM
The correction of position estimation using DCM by simple matrix multiplication is provided using (4).The corrected results using DCM transformation was presented in [1].Precision of results was poor and it is necessary to increase precision to use this system for fusion with a higher precision navigation system based on visual odometry.In this time, we are testing a suitability of increasing anestimation accuracy position determination of IMU only on direct path.The DCM correction will be implemented to the final version of robotics platform navigationnavigation of robotics platform using a fusion of visual odometry, Inertial Measurement Unit and optical odometers at every wheel.

Position estimation correction using moving average filters
The moving average filters are usually used to eliminate the extreme deviations in a data series.The diagram of fifth order Moving Average Filter (MVA) is shown inFigure 2.
In our research we tried to use this type of filtration to eliminate extreme deviations in our measured data using IMU.The results of using this type of filtration are shown inFigure 6.
where M is an order of filter, Ai are coefficients of filter, x k+i are input data, y k are filtered output data

Kalman filter and it´s Implementation to our system
The Kalman filter is a modern, effective and robust method for solving problems of dynamic systems with random signals.The implementation and using of Kalman Filter is described in papers [10], [11].The Kalman filter is established using equation ( 6) and (7).
where x(t) is state vector in time t, y(t) is a vector with measurements in time t, u(t)is thedrive state vector, v(t) is process noise vector, e(t) is the measurement noise vector, A,B,C,D are matrixes,which providesbehaviour of system.
In our case, a model of traveled distance we can establish using equation (8).
where x(t) is a distance in time, x 0 is an initial distance, v is a speed, a is an acceleration, t is a time.
Using equation (9) we can establish a time-dependent system of equations.
where x m is a vector of measured results, x 0 is a vector containing the initial distance (before measurement),v is a vectorcontaining theinitialspeed (before measurement) The vector x m is a vector which contains measured distances according to time and it is reference to our system.
where y e (t)isan actual estimated path using acceleration from accelerometers, y e (t-1)is a previous value of estimated path, a(t) is time-depend acceleration acquired from accelerometers.
In our case we can rewrite an equation ( 6) and ( 7) to easier form, because theinformation about the influence of the control signalshave not been assumed.
The behaviour of a system is defined by the simple state vector.This vector is created using measured distance and calculated speed.
where s(t) is calculated speed -derivation a distance by time.
x(t + 1) = x(t + 1) The last variables are v(t) and e(t).The noise process vector v(t) have two elements because the state vector x(t) also have two elements (measured distance x(t) and it´s derivation-speed s(t)).
The Kalman filter usingVarianceMatrix of noise of process signed Q and Variance Matrix of noise of measurement signed R. The both matrixes are possible to establish using equations ( 16), (17), ( 18) and ( 19) The Kalman filter minimalizes the mean value of trace of the correlation matrix.
where E[…] is mean value, ‫ݔ‬ ො(‫)ݐ‬ is an optimal result (the best estimation value of x(t)) In the Kalman filter is recalculation step and predictive step.In the recalculation part, the estimations of the state vector and a correlation matrix of error are increasing to the time t-1.
Equations ( 21) to (25) provide a recursive solution of a model which is described by the equations ( 11) and (12).
The last step of The Kalman filter design is setting of initial values.The best estimated value x ො(t) is equal to zero, because the initial speed and distance is also equal to zero.

Propossed approach
In our research we are using a wireless-controlled robotics platform.On this platform is fitted a cheap Inertial Measurement CHR-6Dand user-made optical odometer.The parameters of the optical odometer are shown at Table 1.In Table 2 the chassis parameters are shown.The IMU is based on accelerometer ADXL335 and gyroscopes LP510AL for roll and pitch axes and LY510ALH for yaw axis.All parameters of IMU are written in Table 3.The odometer is based on an infrared light barrier and a 64 step encoder wheel.The parameters of optical wheel are written inTable 1.The wheel rotation is captured by a STM32F4Discovery development kit.The measured data from the odometer is transferred to the Raspberry PI 2 via a serial link every 0.25 seconds.In this time, data from odometer and IMU are transferred via a wired connection, but in near future this connection will be upgraded to the Bluetooth connection.In this time, Then, the captured data is processed in Matlab software.In the future, all algorithms will be implemented at this platform.In Figure 3 our robotics platform is shown.

Position estimation
The position estimation of the path using IMU is calculated by equations ( 27) and (28).
where S a (t) is the calculated distance using data from accelerometers, v(t) is the calculated speed, a(t) is measured acceleration, Ts is sampling frequency.The position estimation of path using optical odometer is calculated by equation ( 28).
where so(t) is calculated distance using data from the odometer, c(t) is count of impulses measured by odometer in defined time, Wc is a wheel circumference (in our case 182,21 mm), N is a number of impulses(steps) in one rotation of a wheel (in our case 64 steps).
The maximal error caused by resolution of odometer wheel can be calculated using equation ( 30) and (31).where e o is maximal error caused by the resolution of the odometer wheel; ߙ ݈ is an angle covered in one step; Wc is wheel circumference, N is a number of steps on the odometer wheel (in our case 64 steps).

Results and discussion
At present, in our solution we are using post-processing calculations in Matlab software, but in future all calculations will be implemented into a powerful microcontroller, like Raspberry Pi 2 or equal.The captured data is calculated in pair in Matlab software.The first data array contains a number of counted impulses from the odometer which depends on time; the second data array contains acceleration in depends on time; the third data array contains angular velocity also in depends on time.In this time we are using only a first and second data array for position estimation.The third data array (measured angular velocities) will be used in the following research.
At the Figure 4, the original measured data is shown.This measured data contains a lot of noise caused by the bumpy floor.For this case it is necessary to use some type of filtration.The result using a moving average filter of the tenth order is shown in Figure 6.Using of digital filters is described in detail in [7].In Table 4 and Table 5, estimated path length using an odometer and an IMU is shown.In the table results calculated from filtered data using a moving average filter is shown.For better readability, the first five measurements from fifteen are shown in the tables.Totally, each path had fifteen repetitions.The statistical values are calculated from all measurements.At the Figure 6, the filtered data using Kalman filter is shown.
In Table 6 the filtered data using MVA is shown.InTable 7, the filtered data using the Kalman Filter is shown.From the tables it is possible to find that, the Kalman filter rapidly increase a precision compared with unfiltered data.The results using MVA has better precision than unfiltered data but worse than parameters in case of using the Kalman Filter.
From Table 4 and Table 5 it is possible to recognize that, the odometer has more precise measurement than IMU.

Conclusion
In this paper an increasing estimation accuracy position determination of Inertial Measurement Units was described.This paper is focused on the implementation of the Kalman Filter to our robotics platform.Firstly this research used an alternative to Kalman filtration -Average Moving Filter (AMF).Results were better compared with unfiltered data, but precision was still poor.For this case it is necessary to implement predictive filtration -Kalman filter.The results recorded using the Kalman filter has very well precision.The main problem of the Kalman filter is that, it is necessary to have a precision reference signalfor this case we are using an Odometer.
The odometer has the best precision.The disadvantage of the odometer is that, measuring only path length (impulses) and does not care about curves etc.For this case, it is necessary to have gyroscopes for recording a rotation of robotics platform.Also a big disadvantage of the odometer is wheel slip.In case of a slippery floor, the odometer counts impulses, but the position of robotics platform is the same.The disadvantage of IMU is a big noise in measured signal and lower precision than the odometer.The IMU have problems with additional noise and have a lower precision than the odometer.
Also big disadvantageof IMU is a problem with bumpy floor.In case of bumpy floor, it is impossible to measure acceleration and rotation using IMU without high quality chassis.To eliminating this problem we are using a fusion of Odometer and IMU.
In this time, we are testing suitability of our theory only on a direct path.The DCM correction will be implemented to final version of a robotics platformnavigation of robotics platform using a fusion of visual odometry, Inertial Measurement Unit and mechanical odometers at every wheel.A very big advantage of our visual odometry is precision of position estimation [12].The final device will be used as a fusion of navigation of the robotics platformwith an optical measurement device to create a 3D space map of dangerous spaces like a ruins, abandoned mines etc. [7], [8], [9]

5 the
Raspberry Pi 2 is used for recording data stream.

Figure 4 .
Figure 4. Original acceleration measured using inertial measurement unit vs calculated from odometer.

Figure 5 .
Figure 5. Filtered acceleration measured by IMU using Moving average filter.

Figure 6 .
Figure 6.Filtered acceleration measured by IMU using Kalman filter.

Table 2 .
Chassis parameters of our robotics platform.

Table 4 .
Estimated path length using odometer.

Table 5 .
Estimated path length using IMU.

Table 7 .
An estimated path using IMU -Kalman filter.