Measuring Tilt with an IMU Using the Taylor Algorithm

Abstract

This article addresses the important problem of tilt measurement and stabilization. This is particularly important in the case of drone stabilization and navigation in underwater environments, multibeam sonar mapping, aerial photogrammetry in densely urbanized areas, etc. The tilt measurement process involves the fusion of information from at least two different sensors. Inertial sensors (IMUs) are unique in this context because they are both autonomous and passive at the same time and are therefore very attractive. Their calibration and systematic errors or bias are known problems, briefly discussed in the article due to their importance, and are relatively simple to solve. However, problems related to the accumulation of these errors over time and their autonomous and dynamic correction remain. This article proposes a solution to the problem of IMU tilt calibration, i.e., the pitch and roll and the accelerometer bias correction in dynamic conditions, and presents the process of calculating these parameters based on combined accelerometer and gyroscope records using a new approach based on measuring increments or differences in tilt measurement. Verification was performed by simulation under typical conditions and for many different inertial units, i.e., IMU devices, which brings the proposed method closer to the real application context. The article also addresses, to some extent, the issue of navigation, especially in the context of dead reckoning.

1. Introduction

Precise measurement of tilt is relatively difficult, but it is also a very important research area due to its potential applications. Examples include geodesy, navigation of autonomous vehicles and drones, underwater multibeam sonar mapping, and aerial photography, which requires not only precise positioning using the Global Navigation Satellite System (GNSS), which is a common element of this type of system, but also platform stabilization [1,2,3,4,5,6]. In other words, navigation, and especially drone navigation, cannot be carried out using only one method. Alternative information sources are required. This article focuses on the fusion algorithm inside inertial measurement units (IMUs) based on micro-electro-mechanical system (MEMS) technology, which measures angular velocities and linear (inertial) accelerations in order to accurately determine the tilt in dynamic conditions, i.e., in motion, without the use of additional calibration devices. This is a sine qua non condition for conducting inertial navigation, which is an alternative to dead reckoning navigation, and is most often used to support navigation in difficult conditions, such as, e.g., underwater. The only thing we need to know when conducting this type of navigation is the initial state, i.e., the starting position and the current accelerometer readings [7]. Inertial navigation works based on Newton’s first law, also known as the law of inertia.

The main achievements of the article include the introduction of a new IMU continuous bias correction algorithm. The article is organized as follows: after the introduction, Part 2 discusses MEMS errors, paying attention to random walk as the main one; Part 3 defines the problem against the background of sensor information fusion; and the most important part, Part 4, presents the algorithm, but also the tests and simulations of the proposed algorithm. Conclusions close the paper.

2. Micro-Electro-Mechanical System Errors

MEMS accelerometers and gyros have many advantages and are commonly used in various applications [4,5]. MEMS accelerometers are very small, typically only a few millimeters in size, and are fabricated using semiconductor manufacturing processes. They are also very low-power, which makes them ideal for battery-powered applications.

However, the basic principle of operation of a micro-electro-mechanical system (MEMS) accelerometer is based on the physical phenomenon of capacitance. The sensor consists of a small mass $m$ suspended on springs, which moves in response to acceleration forces (see Equation (1)). As the mass moves, the distance $x$ between two plates changes, which alters the capacitance [7,8,9]. This change in capacitance is then converted into a voltage signal that can be read by an electronic circuit.

The proof mass motion equation in MEMS is given by Equation (1):

$$m{\displaystyle \frac{{\mathit{\partial }}^{2}x(t)}{{\mathit{\partial }t}^2}}+D_o{\displaystyle \frac{\mathit{\partial }x(t)}{\mathit{\partial }t}}+kx\left(t\right)=F_o(t)$$

(1)

where:

• $\mathrm{m}$—proof mass;
• ${\mathrm{D}}_{\mathrm{o}}$—dumping coefficient;
• k—spring constant;
• ${\mathrm{F}}_{\mathrm{o}}(\mathrm{t})$—force.

This is a regular differential equation, and its solution is presented in Equation (2):

$$x(t)={\mathrm{c}}_1{e}^{1/2t(-\sqrt{{D_o}^2-4km}/m-{\displaystyle \frac{D_o}{m}}))}+c_2{\mathrm{e}}^{1/2t(\sqrt{{D_o}^2-4km}/m-{\displaystyle \frac{D_o}{m}}))}+{\displaystyle \frac{F_o(t)}{k}}$$

(2)

For the initial conditions ${\mathrm{c}}_1$, ${\mathrm{c}}_2$: $x(t_0)=0, {\displaystyle \frac{\mathit{\partial }x(t_0)}{\mathit{\partial }t}}=0,{ t}_0=0$

Inertial navigation (INS) involves double integration of measurements from acceleration sensors and integration of the direction sensor from the gyroscopic sensor. This simple regular differential equation turns into a difficult nonlinear equation if we assume that each of ${\mathrm{D}}_{\mathrm{o}}$, $\mathrm{k}$, $\mathrm{x}$, and ${\mathrm{F}}_{\mathrm{o}}(\mathrm{t})$ is nonlinear and depends on, for example, temperature, pressure and other spatial conditions. Equation (1) then describes the motion of a damped oscillator with more complex harmonic motion, which corresponds to the case where, e.g., a spring stiffness is modeled that does not exactly obey Hooke’s law. The Duffing equation is an example of such a dynamic system that exhibits chaotic behavior. Moreover, the Duffing system is characterized by the phenomenon of resonance in response to step periodic excitation ${\mathrm{F}}_{\mathrm{o}}$(t). In the bifurcation diagram of a two-well, periodically driven Duffing oscillator, up to four levels of structure as a function of the increasing period of the driving term ${\mathrm{F}}_{\mathrm{o}}$(t), can be identified [10,11]. Of course, the bifurcation phenomenon must be controlled using filters, etc., but the above considerations lead to the general conclusion that although MEMS accelerometers have many advantages, they also have some disadvantages that must be taken into account. Namely, the scale factor error multiplies the output of the sensor with respect to the true measurement, constituting a multiplicative error. That type of error is responsible for nonlinearity. Further, the noise of MEMS accelerometers can be sensitive to external factors, such as temperature fluctuations, electromagnetic interference, and vibration rectified error (VRE). This can introduce noise and bias into the output signal, which can affect the accuracy of the measurement. Moreover, MEMS accelerometers typically have a limited dynamic range and are only capable of measuring accelerations within a certain range. If the acceleration exceeds this range, the sensor may saturate or become damaged. Finally, MEMS accelerometers can be sensitive to acceleration in directions other than the intended axis of measurement (cross-axis sensitivity). This can cause errors in the output signal, which can affect the accuracy of the measurement [12,13,14].

Generally, inertial sensor errors can be divided into deterministic or systematic errors and random or stochastic errors [14]. The main parameters of the exemplary accelerometer vs. accelerometer array are presented in

Table 1. Main parameters of the LSM330DLC accelerometer vs. LSM330DLC accelerometer array.

Accelerometer Dynamic Accuracy	LSM330DLC	LSM330DLC Array
Bias stability deviation error [m/s2]	4.4372 × 10−4	1.57224 × 10−4
Variance	4.06215 × 10−4	4.66665 × 10−5
Standard error [m/s2]	0.020154	0.006831
Bias [m/s2]	0.050850	0.211349

for a typical low-cost accelerometer, the LSM330DLC.

Deterministic errors are those errors that can be calculated from the system dynamics using a set of calibration tests. Random errors are errors that do not result directly from the dynamics of the system and must be modeled stochastically. Their estimation depends on knowledge of the sensor noise model and parameters that can be fitted to the variance curve [12]. Simple Allan variance provides a means of identifying and quantifying various noise and error terms that exist in the data [13], as presented in Figure 1.

The in-run bias stability standard deviation error references the minima of the Allan deviation curve, as presented in Figure 1 for the data from an IMU array and for a single IMU in static conditions. Using an IMU array is one way to improve accelerometer and gyroscope measurements [6]. The Allan variance is proportional to the total noise power of the sensor output when passed through a bandpass filter. This filter depends on the sampling frequency, and thus, different types of random processes can be examined by adjusting the correlation time. The results for a MEMS gyroscope are similar [12].

The instability of bias drift is the most important measure of the quality of the accelerometer/gyro and is defined as the lowest point on the Allan variance curve, as presented in Figure 1. This is one of the most important parameters for accuracy and overall performance in the context of unassisted inertial navigation [12,14,15].

2.1. Calibration Techniques

The inertial sensor calibration process is a well-defined set of techniques for determining systematic errors [15]. Due to the difference in the set reference value or reference object, it can be divided into two categories—autonomous calibration and non-autonomous calibration [15]. The incremental method proposed in the article can be classified as an autonomous method.

The non-autonomous calibration method is intended for very precise calibration, i.e., the process of comparing the sensor output signal with known reference information and determining coefficients that force the records from the calibrated devices to match the reference records. Usually, this type of calibration is based on large-scale turntables [8]. In this calibration procedure, the bias and scale factor are determined by comparing known parameters to measured output, which is most often implemented in the navigation integration process. This method can be time-consuming and costly, especially if high accuracy is required [16,17].

Autonomous methods, such as equipment-free multi-position-based calibration, are based on the optimization equation that is solved using an iterative weighted least squares correction scheme. However, the autonomous approach requires the computation of the inverse of the matrix. More equations than the number of unknowns should be generated in this case, covering different positions to avoid singularity when computing the inverse of the matrix and obtaining reliable results. Multiple positions means that each wall, side, and corner held vertically up and down is used to provide appropriately different sets of measurements, generated from up to 26 different positions to avoid singularities [14]. Non-autonomous techniques attempt to cover a wide range of inertial calibration schemes but are still not exhaustive of the range of methodologies used. Regarding the standalone calibration method, the use of this method to calibrate a single device has its drawbacks, namely that it requires a specific reference vector, which is difficult to implement in many practical applications. Furthermore, there is no uniform method for detecting multi-sensor calibration errors, especially when it comes to autonomous calibration. The use of different variants of the Kalman filter technique for calibration has not yet been explored well. Additionally, stochastic error models require standardization [18,19]. Nonlinear optimization techniques have evolved into heuristic optimization schemes, which are said to provide a broader search for achieving global optima; however, they have advantages and disadvantages [20,21,22].

To sum up, current calibration techniques do not simultaneously take into account nonlinear scale factors, magnetic disturbances, and g-dependent bias, and no uniform calibration framework is available. MEMS inertial sensors suffer from significant run-to-run biases and thermal drifts, which need to be estimated using infield calibration schemes as these cannot be calibrated in the laboratory setup and are dynamic in nature. Furthermore, the error coefficients also vary with age and environmental conditions, which can be compensated by an infield calibration scheme, and thus, great impetus should be given to improving these techniques. Thus, generally, the scale factor and bias, but not bias drift, can be eliminated by calibration.

The proposed method is an autonomous method but introduces a new incremental approach to determining the bias/errors that accumulate over time.

2.2. Errors Accumulated over Time

There are some types of errors that are still difficult to overcome, i.e., the instability of bias and bias drift [8,12], which refers to the variation in the bias over time caused, for example, by the self-heating of the accelerometer/gyroscope itself and other components of the entire system (both mechanical and electrical). MEMS accelerometers experience drift over time, which affects the accuracy of the measurement (see Figure 2).

The velocity, or angular random walk (ARW), is a measure of accelerometer and gyro perturbation by some thermo-mechanical noise, which fluctuates at a rate much greater than the sampling rate of the sensor. This is proportional to the square root of the integration time [5], as presented in Figure 2 for stationary measurements, where the bias error changes with time. The uncertainty of the random walk increases with time, making it a non-stationary process. However, it can be considered stationary within small time intervals [13].

The true vector of a MEMS accelerometer and gyro consists of a vector of accelerometer $A$ and a vector of angular rate (AR) $\Omega$:

$$A=\left[\begin{array}{c}{a}_x\\ a_y\\ a_x\end{array}\right] \mathrm{and} \Omega =\left[\begin{array}{c}{\omega }_x\\ {\omega }_y\\ {\omega }_x\end{array}\right]$$

(3)

which, in practice, is:

$$A=\left[\begin{array}{c}{a}_{x_L}\\ a_{y_L}\\ a_{z_L}\end{array}\right]+\left[\begin{array}{c}{a}_{x_g}\\ a_{y_g}\\ a_{z_g}\end{array}\right]+\left[\begin{array}{c}{a}_{x_b}\\ a_{y_b}\\ a_{z_b}\end{array}\right]+\left[\begin{array}{c}{a}_{x_N}\\ a_{y_N}\\ a_{z_N}\end{array}\right]$$

(4)

and relatively:

$$\Omega =\left[\begin{array}{c}{\omega }_{x_L}\\ {\omega }_{y_L}\\ {\omega }_{z_L}\end{array}\right]+\left[\begin{array}{c}{\omega }_{x_b}\\ {\omega }_{y_b}\\ {\omega }_{z_b}\end{array}\right]+\left[\begin{array}{c}{\omega }_{x_N}\\ {\omega }_{y_N}\\ {\omega }_{z_N}\end{array}\right]$$

(5)

where:

$A$—acceleration vector.

The variables $\mathrm{a}$ and $\mathsf{\omega }$ represent acceleration and gyro, respectively. The subscripts $\mathrm{x}$, $\mathrm{y}$, and $\mathrm{z}$ correspond to the respective axes, while the subscripts $\mathrm{L}$, b, and $\mathrm{N}$ indicate linear, bias error, and noise, respectively.

3. Problem Definition

Due to the problems mentioned above, i.e., ARW and VRW, the process cannot be carried out completely autonomously and passively, and continuous measurement correction is required.

Integrating navigation information involves primarily using Kalman optimal filtering due to the nature of the Kalman filter [23,24,25,26,27,28], as presented in Figure 3. The measurement $\mathit{z}$ (innovation) is confronted with the processed measurement from the previous cycle. The cycle or iteration is presented using a feedback loop D (delay); thus, the filtering is essentially the integration of information, as presented in Figure 3 [26].

This approach, shown in Figure 3, is implemented in detail using Algorithm A1 in Appendix A. This algorithm uses the measurement of the acceleration due to gravity to calculate the pitch and roll. The measurement is then corrected with the prediction value using Kalman gain K. This code is the initial step of the proposed algorithm for calculating tilt increments (see Section 4.1. Examples of such systems using the above-mentioned Algorithm A1 or its modifications, e.g., in the form of EKF (extended Kalman filter), is present in IMU sensors as presented Figure 4.

Figure 4 presents MEMS devices from Memsense MX IMU (left top), MicroStrain 3DM-GX3-25 (left middle), Xsens MTi-G-28A53G35 (left bottom), and IMU LSM330 in the form of a Samsung phone (right).

In principle, the error must be removed before taking measurements, but due to its nature and the constantly changing bias error value, calibration must be carried out on an ongoing basis and is most often carried out in the process of fusion or integration of navigation information. Unfortunately, this is difficult due to the principle of equivalence rule.

3.1. Principle of Equivalence as the Main Problem of Inertial Navigation

The principle of equivalence concerns inertial navigation and is a consequence of the principle of relativity resulting from A. Einstein’s general theory of relativity. So far, it has not been possible to demonstrate that the gravitational force due to tilt and the force due to inertia can be distinguished. The equivalence of the force of inertia and the force of gravity shows that the forces of inertia and gravity act on bodies in the same way. This means that if we are under the action of one of these forces, i.e., in the IMU body frame, without an additional source of information, we will not know whether gravity, due to tilt, or inertia is acting on the IMU.

In order to clearly define the problem underlying the operation of inertial sensors, special attention must be paid to the exact distinction between gravitational acceleration due to tilt and linear acceleration. Algorithm A2 in Appendix A illustrates the problem; the algorithm clearly shows that accurate knowledge or measurement of $p$ (pitch) and $r$ (roll) makes it possible to accurately reconstruct the initial values of linear acceleration $a_{x_L}$, $a_{y_L}$, and $a_{z_L}$. If we had accurate tilt measurements, i.e., pitch and roll, we could eliminate the error in measuring the acceleration of gravity G. Unfortunately, the correction is not easy due to the principle of equivalence because the tilt measurement error and the bias error of the accelerometer readings must be eliminated at the same time.

This exact problem affects MEMS chips and IMU devices. As a result, we face a problem that manifests itself in a seemingly minor way, presented in Figure 5: each of the tested IMU devices indicates something different when tilted, which is presented in Figure 6, Figure 7 and Figure 8.

This is due to the fact that each device has a different initial error affecting the accelerometer measurements, i.e., when tilted, as in Figure 5, we obtain different initial readings. Figure 5 shows the devices for which simultaneous tilts were performed, i.e., for the Memsense MX IMU (Memsense, Rapid City, SD, USA) [29], Xsens MTi-G-28A53G35 (Xsens, Enschede, The Netherlands) [30] MicroStrain 3DM-GX3-25 (MicroStrain, Williston, ND, USA) [31], and LSM330 IMU (Samsung phone) (STMicroelectronics, Geneva, Switzerland) [32], but also for the high-precision Seatex MRU IMU-5 Kongsberg (Seatex, Rosenberg, TX, USA) [33]; the differences are obvious, as presented in Figure 6.

In Figure 6, differences in measurements can be observed, e.g., for Xsens MTi-G-28A53G35 (red dashed line) and MicroStrain 3DM-GX3-25 (black line). Tilt measurements were performed simultaneously, and the graphs were placed side by side for clarity. Figure 9 shows the same in detail. Similar recorded measurements are presented in Figure 7 for the Xsens MTi-G-28A53G35.

The errors of acceleration may be small, i.e., of the order of 0.001 G; however, they can unfortunately have a significant impact on dead reckoning measurements using the IMU because small differences in tilt measurement result in large errors. Therefore, the tilt measured is similar for all sensors, but there are differences in the shift from the ordinate level to 0, as shown in Figure 6. The actual measurements, as presented in Figure 6, show differences in readings from different IMUs, and even small errors in the tilt IMU measurement have serious consequences in the context of inertial navigation. Figure 7 presents pitch and roll measurements from Xsens MTi-G-28A53G35 using a Kalman filter, and Figure 8 presents the same measurements enlarged for pitch measurements. Despite the use of the Kalman filter, the measurements of pitch are not stable and differ significantly, even in static conditions.

It is enough that the angle measurement system has an error of 0.1 degrees and a linear acceleration error of 0.017 $[\mathrm{m}/{\mathrm{s}}^2]$. This is a lot because after 10 s, the error in determining the position will be 1.7 m. This is illustrated in Figure 9. The differences that appear during measurements are presented in detail in Figure 9. Unfortunately, the differences are large. Algorithm 1below shows this using pseudocode.

In dynamic conditions, in the presence of inertial acceleration, readings from the IMU are subject to greater errors.

3.2. Cumulative Error Correction

Algorithm 1 shows that a small change in accelerometer bias error ${\mathrm{a}}_{{\mathrm{x}}_{\mathrm{b}}}$ = 0.001 $[\mathrm{m}/{\mathrm{s}}^2], {\mathrm{a}}_{{\mathrm{y}}_{\mathrm{b}}}$ = 0.001 $[\mathrm{m}/{\mathrm{s}}^2], {\mathrm{a}}_{{\mathrm{z}}_{\mathrm{b}}}$ = 0.001 $[\mathrm{m}/{\mathrm{s}}^2]$ impacts the tilt error measurement ${\mathrm{p}}_{\mathrm{m}\mathrm{e}\mathrm{a}\mathrm{s}}$ = 10.043° and ${\mathrm{r}}_{\mathrm{m}\mathrm{e}\mathrm{a}\mathrm{s}}$ = 30.021°.

Relatively small errors in determining the accelerometer load error $a_{x_b}$ = 0.001 $\left[\mathrm{m}/{\mathrm{s}}^2\right], a_{y_b}$ = 0.001 $[\mathrm{m}/{\mathrm{s}}^2], a_{z_b}$ = 0.001 $[\mathrm{m}/{\mathrm{s}}^2]$ causes large errors in tilt calculations. These errors then accumulate in the displacement/navigation calculation process.

Algorithm 1: Representation of the impact of angle error on overall fusion performance

$p$ = 10°; $r$ = 30°; $a_{x_b}$ = 0 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$; $a_{y_b}$ = 0 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$; $a_{z_b}$ = 0 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$; $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left\{\begin{array}{c}{a}_x=\sin \left(deg2rad\times \left(p\right)\right)\times G+a_{x_b}=\mathbf{1.7042} {\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\\ a_y=\cos \left(deg2rad\times \left(p\right)\right)\times \sin \left(deg2rad\times \left(r\right)\right)\times G+a_{y_b}=\mathbf{4.8326} {\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\\ a_z=\cos \left(deg2rad\times \left(p\right)\right)\times \cos \left(deg2rad\times \left(r\right)\right)\times G+a_{z_b}=\mathbf{8.3704} {\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\end{array}\right.$ $p_{meas}=atan{\displaystyle \frac{a_x}{\sqrt{{\left(a_y\right)}^2+{\left(a_z\right)}^2}}}\times rad2deg$ $r_{meas}=atan{\displaystyle \frac{a_y}{a_z}}\times rad2deg$ $p_{meas}$ = 10.000° $r_{meas}$ = 30.000° $p=10$°; $r=30$° $a_{x_b}$ = 0.001 G [${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}]$;$a_{y_b}$=0.001 G $\left[{\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\right]$;$a_{z_b}$ = 0.001 G [${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$]; $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left\{\begin{array}{c}{a}_x=\sin \left(deg2rad\times \left(p\right)\right)\times G+a_{x_b}=\mathbf{1.7140} {\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\\ a_y=\cos \left(deg2rad\times \left(p\right)\right)\times \sin \left(deg2rad\times \left(r\right)\right)\times G+a_{y_b}=\mathbf{4.8424} {\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\\ a_z=\cos \left(deg2rad\times \left(p\right)\right)\times \cos \left(deg2rad\times \left(r\right)\right)\times G+a_{z_b}=\mathbf{8.3801} {\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}\end{array}\right.$ $p_{meas}=atan{\displaystyle \frac{a_x+a_{x_b}}{\sqrt{{\left(a_y+a_{y_b}\right)}^2+{\left(a_z+a_{z_b}\right)}^2}}}\times rad2deg$ $r_{meas}=atan{\displaystyle \frac{a_y+a_{y_b}}{a_z+a_{z_b}}}\times rad2deg$ $p_{meas}$ = 10.043° $r_{meas}$ = 30.021°

The consequences and results of this simple observation can be presented using well-known calculations resulting from the assumed value of acceleration due to gravity G = 9.81437737 $[\mathrm{m}/{\mathrm{s}}^2]$ [34]. Even small errors in the tilt measurement have a large impact on the position measurement.

Table 2. Dead reckoning inertial measurement error due to a lack of tilt compensation.

Pitch/Roll Error [°]	$\mathbf{Acceleration} \mathbf{Error} [\frac{\mathbf{m}}{{\mathbf{s}}^2}$]	INS Error after 10 s [m]
1.0	0.17	8.5
0.1	0.017	0.85
0.01	0.0017	0.085
0.001	0.00017	0.0085

shows examples of errors in position determination caused only by tilt measurement errors; e.g., for errors of 0.01°, the position error after 10 s is 8.5 cm.

The point is that the system is not able to give the exact value of tilt and acceleration from gravity without accurate knowledge of pitch and roll, and vice versa; if we knew the exact values of pitch and roll, that is, we knew the exact direct cosine matrix (DCM) values, inertial navigation would become very precise, apart from built-in errors described by the Allan diagram (e.g., angular random walk).

For example, assuming that the bias error for a typical IMU is 0.001 G, then the pitch and roll error will be approximately 0.02–0.04°, which is a lot. For higher-quality accelerometers or, for example, accelerometers that are an order of magnitude better, with a tenfold smaller systematic error or assuming that $a_{x_b}$ = 0.001 G$, a_{y_b}$ = 0.0001 G$, a_{z_b}$ = 0.0001 G, we observe an improvement of about an order of magnitude $p_{meas}$ = 10.0042827747841°, $r_{meas}$ = 30.0021292279816° (see Algorithm 1). Therefore, taking into account Table 2 above, the error in determining the linear acceleration will be about 0.00034 $[\mathrm{m}/{\mathrm{s}}^2]$–0.00068 $[\mathrm{m}/{\mathrm{s}}^2]$, respectively. This means that even in the case of such high-class accelerometers, the error is not negligible.

4. Materials and Methods

To partially overcome the problem presented in Section 3.2, a new approach is proposed that exploits the nonlinearity existing between the acceleration value and the sensor tilt $p$, $r$ (pitch, roll) and proposes an approach based on the Taylor method, which is presented in Section 4.1 below. This is an incremental method, so the current accelerometer bias error $a_{x_b}$, $a_{y_b}$, $a_{z_b}$ and gyroscope deviation error ${\omega }_{x_b}$, ${\omega }_{y_b}$, ${\omega }_{z_b}$, as shown in Equation (5), can be eliminated (by using incremental $Dp$, $Dr$ measurements).

4.1. New Pitch, Roll, and Accelerometer Bias Estimation Incremental Method

Actual accelerometer measurements also contain bias error, which consists of gravity due to pitch and roll, and random noise, as shown in Equations (4) and (5). Gyroscope errors and accelerometer errors, including random errors, are interdependent and difficult to distinguish.

Assuming linear speed v = 0, $a_{x_N}$, $a_{y_N}$, $a_{z_N}$ and $a_{x_L}$, $a_{y_L}$, $a_{z_L}$ all equal 0, we can temporarily simplify Equation (4) to

$$A\left(p, r,a_{x_b},a_{y_b},a_{z_b}\right)=\left[\begin{array}{c}{a}_{x_g}\\ a_{y_g}\\ a_{z_g}\end{array}\right]+\left[\begin{array}{c}{a}_{x_b}\\ a_{y_b}\\ a_{z_b}\end{array}\right],$$

(6)

Accelerometer measurement $A$ depends on $p, r,a_{x_b},a_{y_b},a_{z_b}$:

$$A\left(p, r,a_{x_b},a_{y_b},a_{z_b}\right)=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left\{\begin{array}{c}{a}_x=\sin \left(deg2rad\times \left(p+Dp\right)\right)\times G+a_{x_b}\\ a_y=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \sin \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{y_b}\\ a_z=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \cos \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{z_b}\end{array}\right.$$

(7)

where variable $A$represents acceleration of the accelerometer in the accelerometer frame, $p$ and $r$ represent pitch and roll, respectively, while $Dp$ and $Dr$ are delta pitch and delta roll.

In the above formulas, we use $Dp$ and $Dr$, which are simply differences (increments) in tilt measurements, i.e., pitch $p$ and roll $r$:

$$\begin{array}{c}Dp=p\left(t_2\right)-p\left(t_1\right),\\ Dr=r\left(t_2\right)-r\left(t_1\right),\end{array}$$

(8)

Figure 10 illustrates the idea. Based on ${\mathrm{D}\mathrm{p}}_0$, ${\mathrm{D}\mathrm{p}}_1$, ${\mathrm{D}\mathrm{p}}_2$, ${\mathrm{D}\mathrm{p}}_3$, etc. the algorithm calculates $\mathrm{p}$.

The direct solution to the above equation for $p$,$r$, $a_{x_b}$,$a_{y_b}$,$a_{z_b}$ is difficult due to the nonlinearity of the above equation (Equation (7)); therefore, Taylor’s differential method was used to solve it, which is briefly presented using the equations below. $Dp$ and $Dr$ can take any values in the range of positive real numbers. In practice, in the range of (0, 90) degrees. We can rewrite Equation (7) in the following form:

$$A\left(p, r,a_{x_b},a_{y_b},a_{z_b}\right)=A\left(\widehat{p}+\mathsf{\Delta }p, \widehat{r}+\mathsf{\Delta }r,\widehat{a_{x_b}}+\mathsf{\Delta }{a}_{x_b},\widehat{a_{y_b}}+\mathsf{\Delta }{a}_{y_b},\widehat{a_{z_b}}+\mathsf{\Delta }{a}_{z_b}\right)$$

(9)

where $\widehat{p},\widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}$ are estimates of real values if $p,r,a_{x_b},a_{y_b},a_{z_b}$. And hence $a_x$ is approximately equal to

$$\begin{array}{cc}\hfill a_x(\widehat{p}+\mathsf{\Delta }p, \widehat{r}+\mathsf{\Delta }r& ,\widehat{a_{x_b}}+\mathsf{\Delta }{a}_{x_b},\widehat{a_{y_b}}+\mathsf{\Delta }{a}_{y_b},\widehat{a_{z_b}}+\mathsf{\Delta }{a}_{z_b})\hfill \\ & \cong a_x\left(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}\right)+{\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}\mathsf{\Delta }p+{\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}\mathsf{\Delta }r\hfill \\ & +{\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{x_b}}}\mathsf{\Delta }{a}_{x_b}+{\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{y_b}}}\mathsf{\Delta }{a}_{y_b}+{\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{z_b}}}\mathsf{\Delta }{a}_{z_b}\hfill \end{array}$$

(10)

Similarly, $a_y$ is approximately equal to

$$\begin{array}{cc}\hfill a_y(\widehat{p}+\mathsf{\Delta }p, \widehat{r}+\mathsf{\Delta }r& ,\widehat{a_{x_b}}+\mathsf{\Delta }{a}_{x_b},\widehat{a_{y_b}}+\mathsf{\Delta }{a}_{y_b},\widehat{a_{z_b}}+\mathsf{\Delta }{a}_{z_b})\hfill \\ & \cong a_y\left(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}\right)+{\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}\mathsf{\Delta }p+{\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}\mathsf{\Delta }r\hfill \\ & +{\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{x_b}}}\mathsf{\Delta }{a}_{x_b}+{\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{y_b}}}\mathsf{\Delta }{a}_{y_b}+{\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{z_b}}}\mathsf{\Delta }{a}_{z_b}\hfill \end{array}$$

(11)

And the same $a_z$:

$$\begin{array}{cc}\hfill a_z(\widehat{p}+\mathsf{\Delta }p, \widehat{r}+\mathsf{\Delta }r& ,\widehat{a_{x_b}}+\mathsf{\Delta }{a}_{x_b},\widehat{a_{y_b}}+\mathsf{\Delta }{a}_{y_b},\widehat{a_{z_b}}+\mathsf{\Delta }{a}_{z_b})\hfill \\ & \cong a_z\left(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}\right)+{\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}\mathsf{\Delta }p+{\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}\mathsf{\Delta }r\hfill \\ & +{\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{x_b}}}\mathsf{\Delta }{a}_{x_b}+{\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{y_b}}}\mathsf{\Delta }{a}_{y_b}+{\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{z_b}}}\mathsf{\Delta }{a}_{z_b}\hfill \end{array}$$

(12)

Taking into account Equation (8), the derivative of ${\mathrm{a}}_{\mathrm{x}}$ with respect to $\mathrm{p}$, $r, a_{x_b}$ is equal to

$${\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}=G\times deg2rad\times (\cos \left(deg2rad\times \left(p+Dp\right)\right)),$$

(13)

$${\displaystyle \frac{\mathit{\partial }{a}_x\left(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}\right)}{\mathit{\partial }r}}=0,$$

(14)

$${\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{x_b}}}=1,$$

(15)

the derivative of $a_y$ with respect to $p$, $r, a_{y_b}$ equals

$${\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}=-G\times deg2rad\times \sin \left(deg2rad\times \left(p+Dp\right)\right)\times \sin (deg2rad\times \left(r+Dr\right))$$

(16)

$${\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}=G\times deg2rad\times \cos \left(deg2rad\times \left(p+Dp\right)\right)\times \cos (deg2rad\times \left(r+Dr\right))$$

(17)

Finally, the derivative of $a_y$ with respect to $a_{y_b}$ equals

$${\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{y_b}}}=1,$$

(18)

while the derivative of $a_z$with respect to $p$, $r,a_{z_b}$ equals

$${\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}=-G\times deg2rad\times \sin \left(deg2rad\times \left(p+Dp\right)\right)\times \cos (deg2rad\times \left(r+Dr\right))$$

(19)

the differential $a_z \mathrm{e}\mathrm{q}\mathrm{u}\mathrm{a}\mathrm{l}\mathrm{s}$

$${\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}=-G\times deg2rad\times \cos \left(deg2rad\times \left(p+Dp\right)\right)\times \sin (deg2rad\times \left(r+Dr\right))$$

(20)

$${\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{z_b}}}=1,$$

(21)

Based on Equation (9), we can write that the estimated $\widehat{A}$ and the actual measurement A exist in the following relationship:

$$\begin{array}{c}A-\widehat{A}=A\left(\widehat{p}+\mathsf{\Delta }p, \widehat{r}+\mathsf{\Delta }r,\widehat{a_{x_b}}+\mathsf{\Delta }{a}_{x_b},\widehat{a_{y_b}}+\mathsf{\Delta }{a}_{y_b},\widehat{a_{z_b}}+\mathsf{\Delta }{a}_{z_b}\right)-A\left(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}\right)=\hfill \\ =\left[\begin{array}{ccccc}{\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}& {\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}& {\displaystyle \frac{\mathit{\partial }{a}_x(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{x_b}}}& 0& 0\\ {\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}& {\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}& 0& {\displaystyle \frac{\mathit{\partial }{a}_y(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{y_b}}}& 0\\ {\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }p}}& {\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }r}}& 0& 0& {\displaystyle \frac{\mathit{\partial }{a}_z(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}})}{\mathit{\partial }{a}_{z_b}}}\\ \dots & \dots & \dots & \dots & \dots \end{array}\right]\hfill \\ \times \left[\begin{array}{c}\begin{array}{c}\Delta p\\ \Delta r\\ \Delta a_{x_b}\end{array}\\ \Delta a_{y_b}\\ \Delta a_{z_b}\end{array}\right]\end{array}$$

(22)

where $\widehat{\mathrm{A}}$ is equal to

$$\widehat{A}=A\left(\widehat{p}, \widehat{r},\widehat{a_{x_b}},\widehat{a_{y_b}},\widehat{a_{z_b}}\right),$$

(23)

while $\mathsf{\Delta }$X is equal to

$$\mathsf{\Delta }X=\left[\begin{array}{c}\Delta p\\ \Delta r\\ \Delta a_{x_b}\\ \Delta a_{y_b}\\ \Delta a_{z_b}\end{array}\right]$$

(24)

and $\mathrm{M}$ is a matrix from Equation (22); therefore, we can use the least squares (LSQ) approach to solve the following equation (see Equation (22)):

$$A-\widehat{A}=\mathsf{\Delta }\mathrm{A}=\mathrm{M}\times \Delta \mathrm{X},$$

(25)

As a result, $\mathsf{\Delta }p, \mathsf{\Delta }r,$ $\mathsf{\Delta }{a}_{x_b}$, $\mathsf{\Delta }{a}_{y_b}$, $\mathsf{\Delta }{a}_{z_b}$ gives:

$$\mathsf{\Delta }\mathrm{X}={(M^T\times \mathrm{M})}^{-1}\times M^T\times \mathsf{\Delta }\mathrm{A},$$

(26)

and the actual $\mathrm{X}$, true $\mathrm{X}$ equals

$$\mathrm{X}=\widehat{X}+\mathsf{\Delta }X,$$

(27)

4.2. Numerical Example

A working example of the algorithm proposed in Section 4.1 is presented using Algorithm A3 in Appendix A. A demonstration of the proposed algorithm is presented in the form of pseudocode. The state estimator $\widehat{X}$ includes the pitch and roll error, and can be initialized using arbitrary scalar values. In subsequent iterations, the actual values are calculated based on measurement $\mathrm{A}$ (see Equation (9)).

4.3. Algorithm Verification Using Simulation

An important step in creating a new algorithm is its simulation and verification. Algorithms 2 and 3 below make it possible to test Algorithm A3 from Appendix A from different perspectives.

Algorithm 2: Algorithm test pseudocode representation

%initialization $p$ = 30;$r$ = 10; $a_{x_b}$ = 0.005; $a_{y_b}$ = 0.002; $a_{z_b}$ = 0.003; $X=\left[\begin{array}{c}30\\ 10\\ 0.005\\ 0.002\\ 0.003\end{array}\right]$ $Dp=\left[\begin{array}{c}5\\ 20\end{array}\right]$ $Dr=\left[\begin{array}{c}7\\ 13\end{array}\right]$ $\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left\{\begin{array}{c}{a}_x=\sin \left(deg2rad\times \left(p+Dp\right)\right)\times G+a_{x_b}=5.6343\\ a_x=\sin \left(deg2rad\times \left(p+Dp\right)\right)\times G+a_{x_b}=6.5721\\ a_y=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \sin \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{y_b}=2.3525\\ a_y=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \sin \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{y_b}=2.8518\\ a_z=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \cos \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{z_b}=7.6912\\ a_z=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \cos \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{z_b}=6.7167\end{array}\right.$ $\widehat{X}=\left[\begin{array}{c}0\\ 0\\ 0\\ 0\\ 0\end{array}\right]$ for i = 1:5 $X=\mathbf{asolve\_bias}(\widehat{X},\mathrm{A}, Dp,Dr)$ $\widehat{X}=X$ end $\widehat{X}=\left[\begin{array}{c}30\\ 10\\ 0.005\\ 0.002\\ 0.003\end{array}\right]$ ;

As presented in Algorithm 2, above, the only inputs are $\mathrm{A}$, i.e., the measurement from the accelerometer, and the difference in the inclination measurement, i.e., $Dp$ and $Dr$. In response, we obtain the vector $\widehat{X}$:

$$\widehat{X}=\left[\begin{array}{c}p\\ r\\ a_{x_b}\\ a_{y_b}\\ a_{z_b}\end{array}\right]$$

4.4. Algorithm Verification Using the Side Effect of New Method—Simulation

Due to the nature of the proposed algorithm, an additional property of the proposed algorithm was obtained. The $Dp$ and $Dr$ matrices can be of any size $N\times 1$, with $N\ge 2$. This property gives rise to an interesting but relatively positive property of this algorithm. As is known, typical waveforms for MEMS acceleration sensors may be subject to random errors, as shown in Figure 2, Figure 7, and Figure 8. We know from the theory of measurement systems [8,16] that the sum of parallel measurements is always better than a single measurement.

The procedure detailed below tests the new algorithm in the presence of such random disturbances. Noise was introduced or added to prove this. Matrix A can have any dimension $M\times 5$, where $M\ge 5$. A slight modification to Algorithm 2 confirms that an additional benefit can be achieved by using a larger number of measurements, i.e., from a larger vector of size $\widehat{A}$.

$$A\left(p, r,a_{x_b},a_{y_b},a_{z_b}\right)=\left[\begin{array}{c}{a}_x\\ a_y\\ a_z\end{array}\right]=\left\{\begin{array}{c}{a}_x=\sin \left(deg2rad\times \left(p+Dp\right)\right)\times G+a_{x_b}\\ a_y=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \sin \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{y_b}\\ a_z=\cos \left(deg2rad\times \left(p+Dp\right)\right)\times \cos \left(deg2rad\times \left(r+Dr\right)\right)\times G+a_{z_b}\end{array}\right.$$

(28)

The algorithm modified in this respect is presented as Algorithm 3. In the presented algorithm, $N\left(\mu ,{\sigma }^2\right)$ represents Gaussian noise.

Algorithm 3: Proposed algorithm’s side effect test pseudocode representation

for j = 1:1000 %add random noise $p$ = 30°;$r$ = 10°; $a_{x_b}$ = $0.005+N\left(\mu ,{\sigma }^2\right);$ $a_{y_b}$ = 0.002 + $N\left(\mu ,{\sigma }^2\right)$; $a_{z_b}$ = 0.003 + $N\left(\mu ,{\sigma }^2\right)$; $\left[\begin{array}{c}\begin{array}{c}{a}_x\\ \dots \\ a_y\end{array}\\ \dots \\ a_z\\ \dots \end{array}\right]=\left\{\begin{array}{c}{a}_x=\sin \left(deg2rad\times \left(p+Dp+N\left(\mu ,{\sigma }^2\right)\right) \right)\times G+a_{x_b}+N\left(\mu ,{\sigma }^2\right)\\ \dots \\ a_y=\cos \left(deg2rad\times \left(p+Dp+N\left(\mu ,{\sigma }^2\right)\right)\right)\times \sin \left(deg2rad\times \left(r+Dr+N\left(\mu ,{\sigma }^2\right)\right)\right)\times G+a_{y_b}+N\left(\mu ,{\sigma }^2\right)\\ ..\\ a_z=\cos \left(deg2rad\times \left(p+Dp+N\left(\mu ,{\sigma }^2\right)\right)\right)\times \cos \left(deg2rad\times \left(r+Dr+N\left(\mu ,{\sigma }^2\right)\right)\right)\times G+a_{z_b}+N\left(\mu ,{\sigma }^2\right)\\ ..\end{array}\right.$ $Dp=\left[\begin{array}{c}14.920\\ 20.362\\ 25.938\\ 36.545\\ \dots \\ \mathrm{N}\end{array}\right]$ $Dr=\left[\begin{array}{c}6.0222\\ 12.1039\\ 18.1310\\ 24.1537\\ \dots \\ \mathrm{N}\end{array}\right]$ for i = 1:5 $X=\mathrm{a}\mathrm{s}\mathrm{o}\mathrm{l}\mathrm{v}\mathrm{e}\_\mathrm{b}\mathrm{i}\mathrm{a}\mathrm{s}(\widehat{X},\mathrm{A}, Dp,Dr)$ $\widehat{X}=X$ end $\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X}=[\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X} \widehat{X}]$ end $\mathrm{S}\mathrm{T}\mathrm{D}p=\mathrm{s}\mathrm{t}\mathrm{d}(\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X}(1,:))$ $\mathrm{S}\mathrm{T}\mathrm{D}r=\mathrm{s}\mathrm{t}\mathrm{d}(\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X}(2,:))$ $\mathrm{S}\mathrm{T}\mathrm{D}{a}_{x_b}=\mathrm{s}\mathrm{t}\mathrm{d}(\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X}(3,:))$ $\mathrm{S}\mathrm{T}\mathrm{D}{a}_{y_b}=\mathrm{s}\mathrm{t}\mathrm{d}(\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X}(4,:))$ $\mathrm{S}\mathrm{T}\mathrm{D}{a}_{z_b}=\mathrm{s}\mathrm{t}\mathrm{d}(\mathrm{t}\mathrm{a}\mathrm{b}\mathrm{l}\mathrm{e}\widehat{X}(5,:))$

Table 3. Standard errors vs. samples N.

Number of Samples N	$\mathbf{STD}\mathit{p}$ [°]	$\mathbf{STD}\mathit{r}$ [°]	$\mathbf{STD}{\mathit{a}}_{{\mathit{x}}_{\mathit{b}}}$$[\frac{\mathbf{m}}{{\mathbf{s}}^2}$]	$\mathbf{STD}{\mathit{a}}_{{\mathit{y}}_{\mathit{b}}}$$[\frac{\mathbf{m}}{{\mathbf{s}}^2}$]	$\mathbf{STD}{\mathit{a}}_{{\mathit{z}}_{\mathit{b}}}$$[\frac{\mathbf{m}}{{\mathbf{s}}^2}$]
2	0.481	0.674	0.022	0.077	0.035
3	0.0917	0.149	0.0058	0.016	0.0086
4	0.0609	0.102	0.005	0.0108	0.0067
5	0.055	0.0817	0.0049	0.01045	0.0053
6	0.0519	0.076	0.005	0.01018	0.005
7	0.057	0.083	0.0045	0.010	0.005
8	0.0536	0.0777	0.00455	0.0098	0.0049
9	0.053	0.077	0.00467	0.0098	0.0048
24	0.0305	0.0435	0.00327	0.0059	0.0028

presents a summary of the values of the standard deviation, i.e., standard deviations calculated for different values of N.

Table 3 shows the results of standard deviations for different numbers N of measurements. The results of the calculations were obtained in conditions close to real ones.

As can be easily seen in Figure 11, as the number of measurements N increases, i.e., the number of equations in Equation (22), the accuracy of pitch, roll, and bias error ($p$,$r$, $a_{x_b}$, $a_{y_b}, a_{z_b}$) measurements improves. This is a positive effect that characterizes the LSQ approach.

The application of the proposed algorithm to real data from MEMS Xsens MTi-G-28A53G35 confirms the simulations performed. Figure 12 shows the effect of using the algorithm described in Section 4.1 for different N = 2 (green), N = 10 (black), and N = 20 (green). The green figures give the best results and confirm the simulations. Figure 12 presents pitch measurements for different N. Quality improves as N increases (real records).

5. Results

The algorithm was tested for the Xsens MTi-G-28A53G35 MEMS sensor because this MEMS sensor has good angle ratio (AR) bias performance, as presented in Figure 2. The algorithm was also tested for the Xsens MTi-G-28A53G35 sensor because it has an implemented EKF filter and, in practice, gives better pitch and roll records [16], as shown in Figure 13.

Record samples from the Xsens MEMS MTi-G-28A53G35 are presented in Figure 13. Columns 2–4 are the accelerometer measurements, columns 5–7 are the gyro angular rate (AR), and columns 10–12 are the roll, pitch, and yaw. The last columns are the result or output of the EKF filter implemented in this device [16].

Integration of Proposed Algorithm with Kalman Sensor Fusion Algorithm

Algorithm A4 from Appendix A shows the process of integrating both algorithms into a combined final algorithm that simultaneously combines/integrates measurements from gyroscopic sensors and accelerometers and corrects the initial pitch, roll, and accelerometer bias error values.

The integration of these algorithms is very simple and perhaps requires further research, at least in terms of better integration. The actual pitch and roll values are corrected by correctly calculated bias errors, i.e., ${\mathrm{a}}_{{\mathrm{x}}_{\mathrm{b}}}$, ${\mathrm{a}}_{{\mathrm{y}}_{\mathrm{b}}}$, ${\mathrm{a}}_{{\mathrm{z}}_{\mathrm{b}}}$, after the “read Xsens MEMS sensor ${\mathrm{a}}_{\mathrm{x}}$, ${\mathrm{a}}_{\mathrm{y}}$, ${\mathrm{a}}_{\mathrm{z}}$” line of the Algorithm A4 from Appendix A.

Figure 14 shows the results of applying this algorithm to the raw records. Measurements from the built-in EKF algorithm in the IMU MTi-G-28A53G35 are presented in blue, i.e., column 11 of the recorded data from the IMU MTi-G-28A53G35 (see Figure 13).

The green line (pitch) presents the results of pitch measurements using the proposed algorithm (see Figure 14); the figure presents the results of the estimated pitch using Kalman (blue) and pitch calculated using the proposed algorithm (green).

Figure 15 presents some details of Figure 14. If we look closer at the measurement record in Figure 15, it can be seen that there are differences in the pitch measurements both recorded and calculated using Algorithm A4 (pitch in green color), and using the algorithm built into the MTi-G-28A53G35 (pitch Xsens in blue color, evaluated using the Kalman algorithm). The standard errors for these pitch measurements are presented in

Table 4. Standard errors of measurements for the proposed algorithm and the Kalman algorithm.

Algorithm	Pitch STD [°]
Kalman algorithm	0.202
Proposed algorithm	0.05625

. This is not surprising if we recall the visualizations in Figure 2, where the gyro accumulated errors are presented and discussed for measurements carried out. An example of a $Dp$ array looks like this:

Dp = [5.0227;11.1731;17.4920;23.5077;29.4961;35.4627;41.4013;47.3563;53.3220;59.3947;65.4093;71.4177;77.3618]°

i.e., 13 increments of $p$. A high dynamics of increments was assumed, from approx. 0 to 80°. It should be emphasized here that the algorithm is suitable for dynamic conditions. Table 4 presents standard errors of measurements for the proposed algorithm and the Kalman algorithm. The proposed algorithm gives much better results.

A test similar to the tests carried out in Algorithm 2 was also performed. Therefore, if we introduce an artificial disturbance into the input data, e.g., to the second column of Figure 13 (the acceleration value $a_x$), by adding any disturbance in the form of, e.g., $a_{x_b}$ = 0.01 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$, to the value of this bias error, it will be recalculated correctly with an accuracy of 0.0001 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$. However, this accuracy depends on the value of the bias error introduced and decreases as it increases, i.e., for bias error of $a_{x_b}$ = 0.06 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$, the accuracy of the estimation is about 0.01 ${\displaystyle \frac{\mathrm{m}}{{\mathrm{s}}^2}}$. This means that the system can be calibrated quite precisely under dynamic conditions.

6. Discussion and Conclusions

The article presents a modified method for determining the initial values of pitch, roll, and acceleration using incremental (differential) pitch and roll measurements. Therefore, a difference in tilt (not a tilt) measurements is required, which can be considered an innovation. The paper presents a detailed theoretical justification of the proposed method, which is based on the derivatives approach to nonlinear equations. It improves the determination of IMU MTi-G-28A53G35 tilt (i.e., pitch and roll) to 0.001° even under dynamic pitch/roll conditions (autonomous method), which is a significant improvement as it makes it possible to determine the position using the dead reckoning method from the IMU system, with an accuracy of approximately 0.2 cm per 10 s (Table 2). The individual parts of the article present detailed implementations of this method, as well as tests on simulated and real data from various IMUs, especially the MTi-G-28A53G35 sensor.

Gyro measurements (AR) do not depend on linear acceleration and gravity, and the proposed method exploits this feature. The most important part of the article and at the same time the most important part of the proposed method is presented in paragraph 4.1, while verification of this method is presented in Section 4.2, Section 4.3 and Section 4.4. The algorithm has a solid mathematical foundation, as presented in Section 4.1, and has been tested on real, recorded and simulated/synthetic data. The integration of the proposed algorithm with the optimization–integration filter, i.e., the Kalman fusion filter, is presented in the form of Algorithm A4 in Appendix A. Although this is an effective integration, it is not a perfect approach, so the next step will be to look into better Kalman-proposed method integration. Further research for better integration and improved implementation of Algorithm A4 may be necessary due to its superior practical applications. Thus, attempts at integration with EKF filters and further research in this direction will be carried out. It is worth recalling another important problem at this point, which is the stability of pitch and roll measurements due to random walk (ARW), i.e., the calculations of tilt and bias error; thus, $p$, $r$, $a_{x_b}$, $a_{y_b}$, $a_{z_b}$, will only be valid for a certain period of time, because the method uses incremental measurements of tilt. This is natural for ARW processes and justifies the usage of the proposed algorithm.

Finally, it is worth emphasizing that the algorithm uses gravitational acceleration G. In the calculations, gravity is assumed to be G = 9.81437737 $[\mathrm{m}/{\mathrm{s}}^2]$. The precision of the results obtained using the proposed algorithm requires an accurate determination of G for a given location on Earth.