Research on motion track error detection and compensation algorithm based on MEMS sensor

. Due to various errors in actual use, MEMS inertial sensors have large errors in the detection of motion trajectories. Therefore, it is necessary to analyse and model the errors to decrease the impact of error sources on the detection system. This errors mainly include systematic random errors and accumulated errors generated during double integral operation, and different filtering methods are used for different types of errors. For random errors, the wavelet fuzzy threshold method is used to filter the sensor output signal. For the accumulated error, the zero-state adaptive compensation algorithm is used to correct the acceleration and integral velocity. Experiments show that the wavelet threshold denoising algorithm combined with the zero-state adaptive compensation algorithm can enhance the preciseness of the MEMS inertial sensor in object trajectory detection.


Introduction
MEMS inertial sensors have the advantages of small size, low cost, high reliability, and short response time, and are widely used in inertial navigation systems.In the application, after integrating the angular velocity and acceleration collected by the inertial sensor, information such as the velocity, position and attitude of the object can be obtained.However, in the acquisition process of inertial equipment signal data, it will be limited by the equipment itself and the external environment.The sensor output signal contains errors such as quantization noise, angular random walk, bias instability, angular rate random walk and rate ramp.The accumulation of these errors during the integration process can cause the output accuracy of the inertial navigation system to degrade over time [1].If it is not processed, the motion data calculated after the second integration will have a second drift.Therefore, it is necessary to analyse and model the error to reduce the impact of the error on the detection system.

Error analysis 2.1 Random error
At present, there are mainly the following algorithms for random noise removal of inertial devices: simple filtering algorithm, Kalman filtering algorithm, wavelet algorithm and mode decomposition (EMD) algorithm [2].For the past few years, wavelet denoising algorithms have been applied more widely in inertial devices.The wavelet threshold denoising has good time-frequency localization analysis ability, small amount of calculation, and good denoising effect.The wavelet coefficients of the wavelet hard threshold denoising algorithm are discontinuous at the threshold, and the reconstruction will cause oscillation [3].The wavelet soft threshold has better continuity, but there is a certain deviation in the wavelet coefficients, which will affect the reconstructed signal accuracy.Some studies put forward the algorithm of wavelet improvement threshold, although it has good effect, but the introduction coefficient is difficult to determine, and the process is complicated [4].On the basis of the previous researches, according to the characteristics of random noise of accelerometers, a new wavelet fuzzy threshold denoising algorithm is proposed by combining fuzzy theory with wavelet threshold denoising.

Cumulative error
The elimination methods of accumulated error include zero speed compensation method, polynomial compensation method and multi-axis dynamic switching method, etc.In actual situations, due to the interference of various errors, the zero-speed compensation method is difficult to calculate the zero-speed situation, so the feasibility of this algorithm is not high.The polynomial compensation method corrects the final displacement calculation result again on the basis of the zero-speed compensation method.However, this method does not solve the problem that the state cannot be accurately identified when the speed is zero in the zerospeed compensation method [5].The multi-axis dynamic switching method can effectively solve the shortcomings of the zero-speed compensation method and the polynomial compensation method, but it does not deal with the accumulated errors in the motion process [6].On the basis of analysing the above three accumulative error processing methods, an adaptive compensation algorithm based on zero state is proposed, which can effectively improve the accuracy of motion trajectory detection results through experiments.

The principle of wavelet threshold denoising
The signal containing noise is decomposed by wavelet, and the effective signal with larger modulus value of wavelet coefficient and the noise signal with smaller modulus value of wavelet coefficient are obtained.Wavelet threshold denoising mainly sets a threshold for the coefficients obtained by wavelet transform, separates the noise from the valid signal, and transforms the processed wavelet coefficients to obtain a reconstructed signal, thereby eliminating the noise.The flow chart of wavelet denoising algorithm is shown in Figure 1.Wavelet threshold filtering includes hard threshold function, soft threshold function, fixed threshold function and max-min threshold function.
The formula for the hard threshold function is as follows: Among them, λ stands for threshold, , is wavelet coefficients before denoising, � , is wavelet coefficients after denoising.
The formula for the soft threshold function: is as follows: The boundary between the wavelet coefficients generated by noise and the wavelet coefficients generated by signals is fuzzy, and the size of the threshold is often difficult to choose.Therefore, the selection of the threshold adopts the method of fuzzy logic, and the 0~1 membership function is executed after wavelet decomposition.The wavelet coefficients are mapped to two membership values of 0 or 1.Among them, 0 means not to perform signal reconstruction, and 1 means to perform signal reconstruction.Combined with classical threshold function and fuzzy membership function, a related algorithm is designed to achieve the purpose of removing random noise.
Using a fixed threshold function, it is easy to set the threshold too large, that is, all wavelet coefficient values will be smaller than the fixed threshold, and there is a phenomenon of "overcorrection".Therefore, a fixed threshold is used as the upper limit of the blur threshold.The minimax criterion can only set some wavelet coefficients to zero, and there is a phenomenon of "over-reservation".Therefore, the minimax criterion thresholds are taken as the lower bound of the blur threshold.Select the appropriate membership function as in equation ( 5) to get the relevant wavelet fuzzy threshold function as in equation (4).
Among them,   ( , ) is the fuzzy membership function at the corresponding scale,  is threshold for the minimax function, which is the lower limit of the fuzzy threshold. is the threshold for a fixed-valued function, which is the upper limit of the fuzzy threshold.

Principle and design of zero state adaptive compensation algorithm
By analysing the acceleration data of the state of rest and the moving state of the object, it is found that although the acceleration data of the object in the static state is not zero, the value of the acceleration will fluctuate up and down within a certain range.Analysing the degree of dispersion of acceleration data distribution, determine a segment of data whose variance is less than a certain threshold as static state acceleration data, and reset it to zero.The selection of the threshold is determined by analysing the standard deviation of the acceleration data [7].First, set a sliding window W with a length of 20 data, and then scan the acceleration data in turn.It is equivalent to dividing the whole acceleration data into a set of several data segments, and each data segment has 20 signal data.
Use the sliding window to analyse the standard deviation of the data segment.If the standard deviation is greater than a certain threshold n, and the mean is a number m that is greater than and close to zero, then it can be considered that the degree of dispersion of the data is large and the object is in motion, and the data from the sensors are recorded.If the standard deviation is less than a certain threshold n, and the mean is a number m less than and close to zero, it can be considered that the degree of dispersion of the data is small, the object is in a state of rest, and the data of the sensor is forced to zero, as in equation (8).
The zero-state adaptive compensation algorithm is used to complete the correction of the zero-state acceleration value, and the acceleration data is integrated to obtain the speed at each moment in the entire movement process.Due to the accumulated error of the acceleration during the movement, the velocity at the end of the movement will be offset relative to the zero point.Therefore, using the sliding window W to scan the velocity data V again, the process of speed judgment is similar to the process of acceleration judgment.After calibrating the speed in the stationary state, it is also necessary to process the accumulated error in the moving state, and use the following equation (9) to eliminate the accumulated error in the moving state [8].
The velocity   2 at the end of the motion is taken as the total amount of accumulated errors in the entire motion process, and the approximate linear relationship between the accumulated error and the time during the motion process is treated, so as to eliminate the accumulated error at each moment in the motion process.In order to test the effect of this method compared with other traditional denoising methods, the MEMS accelerometer signal is compared with the soft threshold function, hard threshold function and the method in this paper.Quantitative analysis via introduced signalto-noise ratio (SNR) and mean squared error (RMSE), the results are shown in Table 1.According to the literature, the larger the signal-to-noise ratio, the smaller the RMS error, the better the denoising effect.

Experiment 2
First, move the tracking system 8m along the X-axis of the accelerometer.After integrating the raw data collected by the accelerometer, the results for velocity and displacement are shown in Figure 5-6.When the accelerometer stops moving, the calculated final velocity is 0.18 m/s, and the calculated terminal displacement is about 6.45 m, which is 1.55 m less than the actual displacement.After the original data is processed by the wavelet fuzzy threshold algorithm, the experimental results are shown in Figure 7.When the accelerometer stops moving, the calculated final velocity is about 0.10m/s, and the calculated final displacement is about 7.25m, which is as small as 0.75m higher than the actual displacement.Finally, the wavelet threshold method combined with the zero-speed adaptive compensation algorithm is used to process the experimental data.The experimental results are shown in Figure 8.When the accelerometer stops moving, the calculated terminal velocity is about 0.005m/s, and the calculated terminal displacement is about 7.86, which is 0.16 m smaller than the actual displacement.In addition, we also measured the distances of 8m, 10m, 20m and 50m respectively, and the results are shown in Table 2.Among them, I represents the unprocessed data, II represents the wavelet fuzzy threshold algorithm for processing, and III represents the wavelet threshold algorithm combined with the zero-speed adaptive compensation algorithm for processing.

Conclusion
In the process of object tracking and detection, due to the high sampling frequency of the accelerometer, the random noise interference is large, and the cumulative deviation of the distance measurement results is large.To solve this problem, we use a wavelet threshold denoising algorithm to filter out the random noise of the sensor.Accumulated errors of acceleration data and velocity data are simultaneously removed using a sliding window.By using a short stationary process, this method can effectively prevent the propagation of errors, thereby reducing the amount of accumulated errors in the calculation process.

Fig. 2 .
Fig. 2. Figure with signal data processed by hard threshold function.

Fig. 3 .
Fig. 3. Figure with signal data processed by soft threshold function.

Fig. 4 .
Fig. 4. Figure with Signal data processed by wavelet fuzzy threshold algorithm.

Fig. 8 .
Fig. 8.The result of wavelet algorithm combined with adaptive compensation algorithm.

Table 1 .
A table with SNR and RMSE of three algorithms.

Table 2 .
A table with SNR and RMSE of three algorithms.