Improvement of Initial Azimuth Estimation Time for a North Finding System Using Low–Cost MEMS Sensors and a Compact 3–Axis Turntable Under Non–Horizontal, Magnetically Disturbed, and GNSS–Unavailable Environments

1. Introduction

The most elementary approach to estimate the azimuth of a moving object is to employ a magnetic compass or a magnetometer. In the context of aircraft and ships, where the presence of magnetic interference is a concern, it might not be employed with devices; the utilization of a gyrocompass becomes necessary [1]. Gyrocompasses have a high–accuracy estimation. But their challenges are substantial in size and difficult to maintain, therefore prompting recent interest in methodologies employing Micro Electro Mechanical Systems (MEMS) gyro sensors to estimate azimuth. To estimate azimuth employing a MEMS gyro sensor, it is first necessary to estimate as initial process the angle between the sensor’s azimuth and due north (initial azimuth). There are two kinds of due north detection employing MEMS gyro sensors. One involves estimation while sensor rotating continuously, and the other involves holding the sensor stationary in multiple orientations to estimate angle between the azimuth and due north. Zhang et al. [2] proposed a method for determining north employing a MEMS gyro sensor and a MEMS accelerometer based on the Rotation Modulation Technique. Luo et al. [3] measured angular velocity in 72 directions to estimate angle between the azimuth and due north. In recent years, as exemplified by the “North Finder” [4], there are portable devices capable of estimating angle between the azimuth and due north. On the other hand, compact mobile vehicles, such as Unmanned Underwater Vehicle (UUV)s, possess constrained loading capacity and are anticipated to be utilized in substantial groups. Therefore, it is imperative that these devices are constructed with low–cost components. From these background, we focused on a method for detecting due north employing low–cost MEMS gyro sensors. Therefore, in a previous study [5], the authors proposed a north finding system to address the following issues.

• The employing of these devices poses a significant challenge due to their substantial physical dimensions, which complicates their transportation.
• The existence of a horizontal platform and a mechanism capable of altering the azimuth angle is imperative.
• Precise determination of the northward direction is not feasible under an inclined condition due to the utilization of an accelerometer in the calculation of the angle.
• The verification in instances where the precise direction of due north remains unknown has not been undertaken.
• The time required to detect due north is not discussed in any detail.
• In areas where GNSS signals cannot reach, such as underwater or in mountainous regions, a GNSS-independent method for detecting due north is necessary.

Our previous study [5] developed a portable north finding system for use in non–horizontal and magnetically disturbed environments, designed to be mountable on UUVs. The system consists of a low-cost MEMS sensor and a compact 3–axis turntable. The compact 3–axis turntable has the capacity to rotate ${\mathrm{360 }}^{\circ }$ in each axis, and has a function that automatically searches for and maintains the horizontal position. The signal measurement and signal processing for the low–cost MEMS sensor is performed by a single–board computer. Based on methodology of Luo et al. [3], the compact 3–axis turntable was rotated in 16 directions, and due north detection was performed based on the results of fitting a cosine curve to the obtained signals.

However, our previous study [5] has challenges, specifically in three areas. First, the measurement time required for due north detection is approximately 3.3 h, which is relatively long. Second, because of the offset error and the scale error of the accelerometer are estimated by performing a measurement before every lap, the overall procedure becomes time consuming. And finally, it is necessary to conduct a detailed analysis of the number of las required to reduce the error in due north detection , as determined by sampling with replacement, to less than ${\mathrm{1 }}^{\circ }$(here in after referred to as the "multiple lap simulation"). Therefore, to address these challenges, this paper proposes the following improvements.

• The measurement duration in each direction was increased from 7.5 s to 25 s; this extended duration contributed to reducing the number of laps required to achieve an RMS error less than $1^{\circ }$ for due north detection.
• The offset error and the scale error of acceleration were estimated as linear functions with temperature.
• Quaternion is estimated from the dot product and the cross product by the normalized acceleration vector and the reference attitude vector. The attitude is then estimated through this quaternion.
• Previous study utilized signals for angular velocity about the x-axis and acceleration about the y-axis, but in this paper utilized angular velocity and acceleration in x-axis and y-axis to estimate parameter.
• To evaluate statistically, 5,000 bootstrap samples were generated utilizing resampling with replacement. The due north estimation was then performed employing these datasets, and the relationship between laps and estimation error was statistically analyzed. Employing this dataset, multiple lap trials were then simulated through 10,000 sampling with replacement, and the number of laps required to achieve an RMS error of less than $1^{\circ }$ was investigated.

This paper is structured as follows. In Section 1 the background and objectives are described. In Section 2 denotes the coordinate systems. In Section 3 the signal processing methods are explained in detail In Section 4 the verification of compact 3–axis turntable results and discussions are presented. In Section 5 the experimental results and discussions are presented. Finally, in Section 6 the obtained findings are summarized. In the appendix the method of signal processing of our previous study is described [5].

2. Coordinate System

The coordinate system utilized in this paper is shown in Figure 1 and is right–handed with the z- axis pointing downward. The blue, red, and green arrows shown in the figure correspond to the x-, y-, and z- axes, respectively. Solid lines and dashed lines represent the rotation relative coordinate system (C–frame) and the sensor–fixed coordinate system (S–frame), respectively. The blue, red and green rounded arrows in the figure indicate the rotation arrows, respectively: rotation around the x- axis: roll $\varphi$, rotation around the y- axis: pitch $\theta$, and rotation around the z- axis: yaw $\psi$. The black and orange rounded arrows in the figure indicate the rotation direction of Earth’s rotation and heading gap between the initial azimuth of the observer and the due north direction, respectively.

3. Signal Processing

3.1. Overview

The flow of signal processing in the proposed system is shown in Figure 2. In the figure, the boxes represent signal processing. The black color boxes show the process is the same as our previous study [5], and the red color boxes show the process changes in this study. The signal processing of our previous study [5] is written in the appendix in detail. Signals ${\mathbf{\alpha }}_n$ measured from the accelerometer are processed utilizing the robust Kalman filter (RKF) [6] to remove outliers. The processed signals ${\mathbf{\alpha }}_n^r$ are converted to rotation angles utilizing Equations 7–8. These angles are then converted to angular velocity, ${\mathbf{\omega }}_n^{\alpha }$, through numerical differentiation expressed by Equation A2. The angular velocity ${\mathbf{\omega }}_n^{\alpha r}$, measured from the accelerometer, is the consequence of the processing of ${\mathbf{\omega }}_n^{\alpha }$ with the RKF. The angular velocity ${\mathbf{\omega }}_n^s$ measured from the gyro sensor are processed utilizing the RKF to remove outliers. The processed angular velocity ${\mathbf{\omega }}_n^r$, is converted to C–frame utilizing Equation 9. For due north estimation, the weighted average value, ${\mathbf{\omega }}_n^f$, of the frequency distribution of ${\mathbf{\omega }}_n^c$ and ${\mathbf{\omega }}_n^{\alpha r}$ are utilized. This calculation is performed utilizing Equations 10–12. Finally, the phase a was estimated by utilizing Eq. 2, where a denotes the angle between the initial azimuth of the observer and the due north direction.

3.2. Detection of Due North

The magnetic declination between magnetic north and due north at the measurement location in Yokosuka, Kanagawa Prefecture, is approximately $7^{\circ }$ [7]. Since the gyro sensor operates in a right–handed coordinate system with the z- axis pointing downward, it detects an angular velocity of approximately $3.40\times {10}^{-3\circ }/\mathrm{s}$ when oriented toward due north. Consequently, ${\omega }_{x_n}$ in azimuth $\psi$ and angular velocity when toward due north $3.40\times {10}^{-3\circ }/\mathrm{s}$ as amplitude A can be calculated utilizing the following equation.

$$\begin{array}{cc}\hfill {\omega }_{x_{\psi }}& =Acos\left(\psi \right)\hfill \end{array}$$

(1)

Therefore, when angular velocity ${\mathbf{\omega }}_x(={[{\omega }_{x_0}\dots {\omega }_{x_N}]}^{\top })$ and azimuth $\mathit{\psi }(=[{\psi }_0\dots {\psi }_N])$ are measured from observation points spaced at regular intervals at a position rotated by an arbitrary angle $a^{\circ }$ from the due north, by estimating phase a from Equation 2 based on the following nonlinear least squares method [8], the due north can be estimated.

$$\begin{array}{c}\hfill \underset{a}{min}{\displaystyle \sum _{i=0}^N}({\omega }_{x_i}-Acos({\psi }_i+a))\end{array}$$

(2)

Note that, following our previous study [5], this study also divides the measured signals into two. Specifically, two minimized phases are estimated for the first half (N=8 , ${\omega }_{x_0}$ to ${\omega }_{x_7}$ ) and the letter half (N=9 include ${\omega }_{x_8}$ to ${\omega }_{x_{16}}$ ). The average value of the estimated the first half and letter half phases is defined as the initial azimuth. To calculate Equation 2, the "curve_fit" function in Python’s SciPy library was utilized.

3.3. Remove the Offset Error and the Scale Error of Acceleration

Let signal from the accelerometer ${\mathbf{\alpha }}_n^s(={[{\alpha }_{x_n}^s,{\alpha }_{y_n}^s,{\alpha }_{z_n}^s]}^{\top })$, the offset error of the acceleration ${\mathbf{\alpha }}_n^c(={[{\alpha }_{x_n}^c,{\alpha }_{y_n}^c,{\alpha }_{z_n}^c]}^{\top })$, the scale error of the acceleration ${\mathbf{\alpha }}_n^r(={[{\alpha }_{x_n}^r,{\alpha }_{y_n}^r,{\alpha }_{z_n}^r]}^{\top })$, and the processed signal ${\mathbf{\alpha }}_n(={[{\alpha }_{x_n},{\alpha }_{y_n},{\alpha }_{z_n}]}^{\top })$. The offset error and the scale error of the acceleration can be removed utilizing the following Equation 3 [9].

$$\begin{array}{c}\hfill {\mathbf{\alpha }}_n=\left[\begin{array}{c}{\alpha }_{x_n}\\ {\alpha }_{y_n}\\ {\alpha }_{z_n}\end{array}\right]=\left[\begin{array}{c}({\alpha }_{x_n}^s-{\alpha }_x^c)/{\alpha }_x^r\\ ({\alpha }_{y_n}^s-{\alpha }_y^c)/{\alpha }_y^r\\ ({\alpha }_{z_n}^s-{\alpha }_z^c)/{\alpha }_z^r\end{array}\right]\end{array}$$

(3)

The offset error ${\mathbf{\alpha }}_n^c$ and the scale error ${\mathbf{\alpha }}_n^r$ are estimated utilizing the following steps. First, acceleration is measured at a total of 169 points: the 13 points obtained by stopping the compact 3–axis turntable at 28.${\mathrm{1 }}^{\circ }$ intervals around the x- axis, and the 13 points obtained by then rotating it 28.${\mathrm{1 }}^{\circ }$ around the y- axis and stopping. This generates a point cloud in three–dimensional (3D) space for each rotation angle. Next, since this point cloud contains errors, the triaxial ellipsoid is estimated utilizing these data via the least squares method. The estimated center and radius of the triaxial ellipsoid become the offset error ${\mathbf{\alpha }}_n^c$ and the scale error ${\mathbf{\alpha }}_n^r$, respectively. Incidentally, it is known that the offset error ${\mathbf{\alpha }}_n^c$ and the scale error ${\mathbf{\alpha }}_n^r$ of the accelerometer correlate with temperature. Therefore, estimate the triaxial ellipsoid in 2072 trials in environments with different temperatures. To remove outliers from the estimated values, the RKF was utilized. To estimate the linear function relating to temperature, the least squares method was utilized. The estimation results for the offset error ${\mathbf{\alpha }}_n^c$ and the scale error ${\mathbf{\alpha }}_n^r$ under different temperature environments are shown in Figure 3. The figures plot the offset error or the scale error on the vertical axis and temperature on the horizontal axis. The blue circles, orange circles, and red lines represent the estimated values, the values after outlier removal utilizing the RKF, and the regression line, respectively. The estimated parameters of the linear function are shown in Table 1.

3.4. Quaternion Estimation Utilizing Acceleration Vector and Reference Attitude Vector

The quaternion ${}_A{}^B{\mathbf{q}}_n$ representing the rotation from vector $\mathbf{A}$ to vector $\mathbf{B}$ is expressed by the following Equations 4–6 [10].

$$\begin{array}{cc}\hfill & {}_A{}^B{\mathbf{q}}_n=\left[\begin{array}{c}{}_A{}^B{\mathbf{q}0}_n\\ {}_A{}^B{\mathbf{q}1}_n\\ {}_A{}^B{\mathbf{q}2}_n\\ {}_A{}^B{\mathbf{q}3}_n\end{array}\right]=\left[\begin{array}{c}cos(m_n/2)\\ r_{x_n}sin(m_n/2)\\ r_{y_n}sin(m_n/2)\\ r_{z_n}sin(m_n/2)\end{array}\right]\hfill \end{array}$$

(4)

$$\begin{array}{cc}\hfill & {\mathbf{r}}_n={\mathbf{\alpha }}_n\times {\mathbf{\alpha }}_0\hfill \end{array}$$

(5)

$$\begin{array}{cc}\hfill & m_n={\mathbf{\alpha }}_n·{\mathbf{\alpha }}_0\hfill \end{array}$$

(6)

Suppose that the vector $\mathbf{A}$ is normalized acceleration ${\mathbf{\alpha }}_n$ with the offset error and the scale error removed, and vector $\mathbf{B}$ is the normalized acceleration ${\mathbf{\alpha }}_0(={[0,0,-1]}^{\top })$ at the attitude ${[\varphi ,\theta ,\psi ]}^{\top }={[0,0,0]}^{\top }$, then these relationships are always relative and therefore equivalent to the attitude of the translational coordinate system. The quaternion corresponding to the acceleration signal ${\mathbf{\alpha }}_n$ is expressed by the following Equation 7.

$$\begin{array}{c}\hfill {\mathbf{q}}_n^{\alpha }=\left[\begin{array}{c}{q}_{0_n}\\ q_{1_n}\\ q_{2_n}\\ q_{3_n}\end{array}\right]=\left[\begin{array}{c}cos(m_n/2)\\ r_{x_n}sin(m_n/2)\\ r_{y_n}sin(m_n/2)\\ r_{z_n}sin(m_n/2)\end{array}\right]\end{array}$$

(7)

The conversion from quaternions to Euler angles is performed by the following equation.

$$\begin{array}{c}\hfill \left[\begin{array}{c}{\varphi }_n^c\\ {\theta }_n^c\end{array}\right]=\left[\begin{array}{c}{tan}^{-1}{\displaystyle \frac{q_{3_n}^2-q_{2_n}^2-q_{1_n}^2+q_{0_n}^2}{2(q_{0_n}{q}_{1_n}+q_{2_n}{q}_{3_n})}}\\ {sin}^{-1}\left\{2(q_{1_n}{q}_{3_n}-q_{0_n}{q}_{2_n})\right\}\end{array}\right]\end{array}$$

(8)

The angular velocity ${\mathbf{\omega }}_n^c$ in the C–frame can be calculated as follows:

$$\begin{array}{c}\hfill {\mathbf{\omega }}_n^c=\left[\begin{array}{c}{\omega }_{x_n}^c\\ {\omega }_{y_n}^c\end{array}\right]=\left[\begin{array}{c}{\omega }_{x_n}^{r}cos{\theta }_n^c\\ {\omega }_{y_n}^{r}cos{\varphi }_n^c\end{array}\right].\end{array}$$

(9)

3.5. Frequency–Weighted Average

The angular velocity signals, processed by the RKF is divided into 500 bins sorted in ascending order of $\omega$. Let $m_i$$(1\le i\le 500)$ the number of samples in the i-th bin, ${\omega }_i^m$ the corresponding angular velocity value, $m_{max}$ the maximum of $m_i$, then, a conditionally weighted average is computed, utilizing only those bins for which the number of samples exceeds a threshold defined by $\gamma ·m_{\max }$, where $\gamma$ is a time constant in the range $0<\gamma <1$. In other words, only bins with over $\gamma$ are included in the weighted average calculation. Due to the presence of significant outliers in ${\omega }_x^{\alpha r}$ and ${\omega }_y^{\alpha r}$, an additional condition is represented as described in Equations 10–12.

$$\begin{array}{cc}\hfill & {\overline{\omega }}_{k_{\psi }}^c={\displaystyle \frac{{\sum }_{\begin{array}{c}i=1\\ m_i>m_{max}\gamma \end{array}}^{500}{\omega }_i^m{m}_i}{{\sum }_{\begin{array}{c}i=1\\ m_i>m_{max}\gamma \end{array}}^{500}{m}_i}}(k=\{x,y\})\hfill \end{array}$$

(10)

$$\begin{array}{cc}\hfill & {\overline{\omega }}_{k_{\psi }}^{\alpha r}={\displaystyle \frac{{\sum }_{\begin{array}{c}i=1\\ m_i>m_{max}\gamma \\ {\omega }_i^m<0.{02}^{\circ }\end{array}}^{500}{\omega }_i^m{m}_i}{{\sum }_{\begin{array}{c}i=1\\ m_i>m_{max}\gamma \\ {\omega }_i^m<0.{02}^{\circ }\end{array}}^{500}{m}_i}}(k=\{x,y\})\hfill \end{array}$$

(11)

where $\gamma$ is a coefficient 0.5.

${\overline{\omega }}_{x_{\psi }}^c,{\overline{\omega }}_{y_{\psi }}^c,{\overline{\omega }}_{x_{\psi }}^{\alpha r}$, and ${\overline{\omega }}_{y_{\psi }}^{\alpha r}$ are utilized to derive ${\overline{\omega }}_{x_{\psi }}^f$ according to the following equation.

$$\begin{array}{c}\hfill {\overline{\omega }}_{x_{\psi }}^f=\left\{\begin{array}{cc}{a}_0{\overline{\omega }}_{x_{\psi }}^c+a_1{\overline{\omega }}_{x_{\psi }}^{\alpha r}+a_2{\overline{\omega }}_{y_{\psi }}^c+a_3{\overline{\omega }}_{y_{\psi }}^{\alpha r}\hfill & \mathrm{if}\begin{array}{c}\hfill -0.{005}^{\circ }\le {\overline{\omega }}_{x_{\psi }}^c\le 0.{005}^{\circ }\\ \hfill \wedge -0.{005}^{\circ }\le {\overline{\omega }}_{x_{\psi }}^{\alpha r}\le 0.{005}^{\circ }\\ \hfill \wedge -0.{005}^{\circ }\le {\overline{\omega }}_{y_{\psi }}^c\le 0.{005}^{\circ }\\ \hfill \wedge -0.{005}^{\circ }\le {\overline{\omega }}_{y_{\psi }}^{\alpha r}\le 0.{005}^{\circ }\end{array}\hfill \\ \mathrm{Missing}\hfill & \mathrm{otherwise}\hfill \end{array}\right.\end{array}$$

(12)

where $\mathbf{a}$ is a parameter vector given by $\mathbf{a}=[0.6,0.4,0,0]$ in this paper. The parameter vector was estimated by the lowest RMS error utilizing 77 laps data which measured in the experiment. The estimation results for the parameter vector $\mathbf{a}$ are shown in Figure 4. The figure plots the parameter values on the horizontal axis and the RMS error on the vertical axis. The blue, orange, green, and red lines in the figure represent parameters $a_0$ through $a_3$, respectively, and the minimum value is indicated by a line. From the figure, the RMS error for the parameter $a_0$ of ${\overline{\omega }}_{x_{\psi }}^c$ is low in the range from 0.1 to 0.7. The RMS error for the parameter $a_1$ of ${\overline{\omega }}_{x_{\psi }}^{\alpha r}$ is low in the range from 0.3 to 0.6. For ${\overline{\omega }}_{y_{\psi }}^c$ and ${\overline{\omega }}_{y_{\psi }}^{\alpha r}$, the parameter $a_2$ and $a_3$ yield low RMS errors in the range from 0 to 0.2. In other words, for north detection, it is sufficient to use ${\overline{\omega }}_{x_{\psi }}^c$ and ${\overline{\omega }}_{x_{\psi }}^{\alpha r}$.

3.6. Resampling Utilizing the Nonparametric Bootstrap Method

The due north estimation is then performed employing measurement datasets, and the relationship between the number of laps and the estimation error was statistically analyzed. For statistical evaluation, 5,000 bootstrap samples were generated using a nonparametric bootstrap method with resampling with replacement [11]. The compact 3–axis turntable measures every 22.${\mathrm{5 }}^{\circ }$ from the starting point, completes one lap, and then measures again at the starting point, obtaining data for a total of 17 directions. The samples estimated by the compact 3–axis turntable are ${\mathbf{X}}_{(17,77)}(=[X_{0,0}\dots X_{0,76}]\dots [X_{16,0}\dots X_{16,76}])$ and the bootstrap samples estimated 5,000 times by the nonparametric bootstrap method ${\mathbf{X}}_{(17,5000)}^{*}(=[X_{0,0}^{*}\dots X_{0,4999}^{*}]\dots [X_{16,0}^{*}\dots X_{16,4999}^{*}])$ If it assume this, then resampling by the nonparametric bootstrap method is shown in the following Algorithm 1.

Algorithm 1 Resampling Utilizing the Nonparametric Bootstrap Method

Require:  $\mathbf{X}(=[X_{0,0}\dots X_{0,76}]\dots [X_{16,0}\dots X_{16,76}])$ Ensure:  ${\mathbf{X}}^{*}(=[X_{0,0}^{*}\dots X_{0,4999}^{*}]\dots [X_{16,0}^{*}\dots X_{16,4999}^{*}])$  1: for$a\leftarrow 1$to 17 do  2:     for $b\leftarrow 1$ to 5000 do  3:         for $i\leftarrow 1$ to 77 do  4:            $i\leftarrow \sim U(=\{0,\dots ,76\left\}\right)$  5:            $S_b\leftarrow X[a,i]$  6:         end for  7:         $X^{*}[a,b]\leftarrow Median\left(\mathbf{S}\right)$  8:     end for  9: end for 10: return  ${\mathbf{X}}^{*}$

Here, i is determined utilizing the "random" function in Python’s NumPy library [12], with random numbers generated utilizing PCG64 and an initial seed of 9. Employing bootstrap sample ${\mathbf{X}}^{*}$, multiple lap trials were then simulated through 10,000 sampling with replacement, and the number of laps required to achieve an RMS error of less than $1^{\circ }$ was investigated.

4. Verification of Accuracy of the Compact 3–Axis Turntable Around the $\mathbf{z}$- Axis

The performance test was conducted to verify whether the compact 3–axis turntable can continuously rotate at a specified angular increment during operation. In this test, control signals corresponding to $22.5^{\circ }$ increments about the z- axis were repeatedly sent until the device completed 100 laps. The initial position and the position after 100 laps were examined. Alignment markers, indicated by red circles in Figure 5, were utilized to visually assess rotational accuracy. Verification employed image editing software (Adobe Photoshop (version 26.11.3)), difference Compositing (Difference mode). Difference mode works according to the color information in each picture and subtracts either the blend color from the base color [13]. Therefore, if the alignment marks are in the same location before and after the start, the image will be displayed in black. By aligning these markers before the test, any deviation at the end of the test could be visually confirmed. Figure 6 shows the Figure 5 and the initial image are combined utilizing a difference compositing. The figure shows there are different positions of alignment markers. Figure 7 shows the before start image and after the start image are combined utilizing a difference compositing. The figure shows that the alignment markers remained nearly unchanged in position, indicating that the compact 3–axis turntable achieved rotation about yaw $\psi$ consistent with the control signals.

5. Experimental Results and Discussions

5.1. Experimental Environment

The experiment was conducted at the "Education and Research Building A" of the National Defense Academy of Japan. Figure 8 shows the building where the experiment took place. Utilizing the two yellow pins in the figure, the azimuth angle was calculated to be $\psi ={83.802}^{\circ }$, based on a map and an online azimuth calculation tool provided by the Geospatial Information Authority of Japan [14]. To align the x-axis of the compact 3–axis turntable with $\psi ={83.802}^{\circ }$, the device was rotated about yaw $\psi$. After the rotation, the position was recorded utilizing an alignment markers. Next, to orient the compact 3–axis turntable toward East ($\psi ={90}^{\circ }$), it needed to be rotated by ${6.198}^{\circ }$, which corresponds to 35.260 steps about yaw $\psi$. The device was then rotated 35 steps about yaw $\psi$, and the position was recorded utilizing the alignment markers.

5.2. Detection of Due North

The experiment was conducted by rotating the compact 3–axis turntable in increments of ${22.5}^{\circ }$ about yaw $\psi$, orienting it in 16 directions to complete one full lap. The lap beginning was set to rotated ${11.25}^{\circ }$ to the right of East ($\psi ={101.25}^{\circ }$), to simulate a scenario in which due north was not known. In this case, due north falls between the measurement directions. The obtained results were divided into two halves, and the phase a is estimated by Equation 2. Given the confirmed accuracy of z- axis rotation, due north can be estimated from the phase shift in the fitted cosine curve.

Figure 9 shows the example of the compact 3–axis turntable was rotated in 16 directions at ${22.5}^{\circ }$ intervals, starting from $\psi ={101.25}^{\circ }$. In the figure, the blue dots, the purple dots, the red dots, the yellow lines, the light blue dots, and the green dots respectively show average values of ${\omega }_x^c$, ${\omega }_x^{\alpha r}$, ${\omega }_x^f$, the estimated cosine wave, the estimated due north, and the theoretical values. This result is an example of the due north was successfully estimated, even when it was not known beforehand.

However, due north could not be estimated in all trials. Therefore, the number of laps required to estimate due north with an accuracy of $1^{\circ }$ RMS error must be verified. Employing 5,000 laps of data generated by the nonparametric bootstrap method described in Chapter 3.6, multiple lap trials were simulated by performing 10,000 sampling with replacement.

The samples estimated by the proposed method and the resampled samples by the bootstrap method are shown in Figure 10–Figure 11, respectively. In Figure 10, the black dashed lines show the critical value of two–side t–test significance level set to 5 %. Compared to Figure 11, the resampled samples by the bootstrap method can be said to be within the acceptance region when the significant level of the experimentally obtained sample is set to 5%. The RMS error of due north estimation via simulated multiple laps are shown in Figure 12. In the figure, the vertical axis represents the RMS error, and the horizontal axis represents the number of laps. From the figure, it can be seen that the RMS is $1^{\circ }$ after 17 laps. The time required for the stepping motor to complete one lap is approximately 40 s, and since each measurement takes 25 s and 17 measurements are taken, the total time required for one laps are 465 s. Therefore, the proposed method achieved an accuracy of $1^{\circ }$ RMS error in approximately 2.2 h, demonstrating its effectiveness in such environments. This result shows that the required measurement time was reduced by approximately 1 h compared to our previous study [5], and suggests that the proposed method can contribute to practical operational systems. The following two reasons can be cited for this result:

• The values became more stable by increasing the measurement time per direction compared to the method used in previous studies.
• In Equation 12, instead of referencing values from other sensors as substitutes for outliers during weighted averaging, missing data was used, therefore preventing the results from being affected even if the values from other sensors are not worth using.

6. Conclusion

This paper proposes a method for improving the initial azimuth estimation time of a north finding system employing low–cost sensors and a compact 3–axis turntable, capable of operation in non–horizontal, magnetically disturbed, and GNSS–unavailable environments. The system comprises a compact 3–axis turntable, with each axis capable of ${360}^{\circ }$ rotation, and a device equipped with low–cost MEMS sensors mounted on a single–board computer. Our previous study [5] has limitations, specifically in three areas. The time required to detect due north is long. The offset error and the scale error of the accelerometer are estimated each time before measurement, and estimates could be obtained more quickly if the time required for this process were reduced. Restoration extraction was performed on simulations involving multiple laps. It requires a more detailed discussion of multiple laps. Therefore, to address these issues, this paper introduces the following improvements of signal processing.

• The measurement duration in each direction was increased from 7.5 s to 25 s; this extended duration contributed to reducing the number of laps required for due north detection.
• The offset error and the scale error of acceleration were estimated as linear functions with temperature.
• Quaternion is estimated from the dot product and the cross product by the acceleration vector of the $l2$-norm normalized and the reference attitude vector. The attitude is then estimated through this transformation.
• Instead of employing signals for angular velocity about the x-axis and acceleration about the y-axis, the parameters were determined utilizing signals for angular velocity and acceleration about both the x- and y-axes.
• To evaluate statistically, 5,000 bootstrap samples were generated utilizing resampling with replacement. The due north estimation was then performed employing these datasets, and the relationship between laps and estimation error was statistically analyzed.
• When calculate weighted average of the angular velocities for each direction, the values from the angular velocity sensor were used when the average value estimated from the accelerometer was an outlier. However, this resulted in a high dependence on the angular velocity sensor values. Therefore, if either the values estimated from the accelerometer or the angular velocity sensor were outliers, they were treated as missing values.

To verify the accuracy of the compact 3–axis turntable around the z- axis and proposed method, this paper conducted two types verification. First, to verify rotation around the z- axis, a rotation command of $22.5^{\circ }$ was continuously sent, and images taken before rotation and after 100 laps were compared utilizing a difference compositing. As the result, the alignment markers remained nearly unchanged in position, indicating that the compact 3–axis turntable achieved rotation about yaw $\psi$ consistent with the control signals. Second, to evaluate the system, an experiment on the detection of due north was conducted. The measured 77 laps data were resampled utilizing a bootstrap method to construct a dataset of 5,000 laps. Employing this dataset, multiple lap trials were then simulated through 10,000 sampling with replacement, and the number of laps required to achieve an RMS error of less than $1^{\circ }$ was investigated. The device was rotated in increments of ${22.5}^{\circ }$ to complete a lap. starting from starting from $\psi ={101.25}^{\circ }$, simulating a scenario in which due north was not known in advance. As a result, the proposed method achieved an accuracy of $1^{\circ }$ RMS error in approximately 2.2 h, demonstrating its effectiveness in such environments. This result shows that the required measurement time was reduced by approximately 1 h compared to our previous study [5], and suggests that the proposed method can contribute to practical operational systems. The following two reasons can be cited for this result:

• The values became more stable by increasing the measurement time per direction compared to the method used in previous studies.
• In Equation 12, instead of referencing values from other sensors as substitutes for outliers during weighted averaging, missing data was used, therefore preventing the results from being affected even if the values from other sensors are not worth using.

Appendix A.1. Overview

The compact 3–axis turntable consists of single–board computer (Raspberry Pi Zero 2W) manufactured by Raspberry Pi Ltd. (England, United Kingdom), low–cost MEMS sensors including an accelerometer (ADXL355) manufactured by Analog Devices (Wilmington, Massachusetts, United States of America), gyro sensor (MPU9250) manufactured by TDK Co., Ltd. (Tokyo, Japan), and the compact 3–axis turntable. The compact 3–axis turntable employs a low–cost stepper motor (28BYJ-48) manufactured by ShenZhenShi HuaRen ChuangYi Keji Limited Company (Guangdong, China), capable of rotating ${\mathrm{360 }}^{\circ }$ around the x-, y-, and z- axes. The power for the stepper motors and single–board computer is supplied separately from a Universal Serial Bus (USB) adapter, which acts as an Alternating Current to Direct Current (AC–DC) converter, each providing a 5 V 1 A power supply. The power supply for the sensors is supplied by a DC–DC converter included in the single–board computer. The compact 3–axis turntable, coordinate system, and rotation direction are illustrated in Figure A1. As illustrated in the figure, a stepper motor is positioned around the MEMS sensor therefore, it is not possible to employ a magnetic sensor due to the constant disruption of the magnetic field by the motor drive. The red arrows in the figure indicate the coordinate system employed in the compact 3–axis turntable, which is a right-handed system with the z- axis pointing downward. The orange arrows in the figure indicate the positive direction of rotation roll $\varphi$, pitch $\theta$, and yaw $\psi$ denote rotations about the x-, y-, and z- axes, respectively. The stepper motor (28BYJ-48) utilized possesses a step angle of 5.${\mathrm{63 }}^{\circ }$ with an output shaft, gear reduction ratio 1/64, a mass of approximately 35 g, and a rotational torque of 800 gf-cm [15]. In the compact 3–axis turntable, the roll $\varphi$ and pitch $\theta$ are configured to complete a ${\mathrm{360 }}^{\circ }$ rotation within 512 steps, while the angular velocity about yaw $\psi$ is configured to complete a ${\mathrm{360 }}^{\circ }$ rotation within 2048 steps. This is achieved through the utilization of gears in the device. Therefore, roll $\varphi$ and pitch $\theta$ rotate $7.03\times {10}^{-1}°$ in one step and yaw $\psi$ rotates 1.76$\times {10}^{-1}°$ in one step. Rotation command is transmitted via Pulse Width Modulation (PWM) signals from the single–board computer’s four General Purpose Input/Output (GPIO) pins. The PWM signal cycles were determined to be 0.01 s through a process of trial and error.

Figure A1. Overview of compact 3–axis turntable, coordinate system, and rotation direction[5].

Figure A1. Overview of compact 3–axis turntable, coordinate system, and rotation direction[5].

Appendix A.2. Allan Variance

The MPU9250 gyro sensor incorporates the same sensor core as the MPU6050, for which previous studies have demonstrated the capability to detect Earth’s rotation [16]. In this paper, the gyro sensor scale was configured to $\pm 250°/\mathrm{s}$, with a scale factor of $131\mathrm{LSB}/{(}^{\circ }/\mathrm{s})$ [17]. The accelerometer was configured to a scale of $\pm 2\mathrm{g}$, with a scale factor of $256000\mathrm{LSB}/\mathrm{g}$ [18].

Allan variance was employed to evaluate the error components of the gyro sensor and accelerometer. This facilitates the identification of sensor characteristics such as random walk, bias instability, and drift [19]. The measurement was conducted over a period of 2 h. Before the measurement, the system was configured such that the rotation angles ${\varphi }_n^{\alpha }$ and ${\theta }_n^{\alpha }$ remained below 1 step ($7.03\times {10}^{-1}°$), and it was oriented toward the East ($\psi ={90}^{\circ }$). Representative Allan variance plots for the gyro sensor and accelerometer are shown in Figure A2–Figure A3,respectively. In Figures, the green, blue, and purple lines show the slopes corresponding to random walk, bias instability, and drift, respectively.

From Figure A2, it can be seen that the ${\omega }_x$ shows a random walk of $2.42\times {10}^{-2}°/\mathrm{s}$, bias instability of $2.22\times {10}^{-3}°/\mathrm{s}$, and drift at $2685\mathrm{s}$. The ${\omega }_y$ shows a random walk of $2.92\times {10}^{-2}°/\mathrm{s}$, bias instability of $6.14\times {10}^{-3}°/\mathrm{s}$, and drift at $73\mathrm{s}$. In summary, the ${\omega }_x$ demonstrates an error of approximately $2.42\times {10}^{-2}°/\mathrm{s}$, a resolution of approximately $2.22\times {10}^{-3}°/\mathrm{s}$, and reliable measurements can be obtained up to $2685\mathrm{s}$ before drift occurs. In contrast, the ${\omega }_y$ should be limited to measurements within $73\mathrm{s}$, since drift onset is 73 s. Considering Earth’s rotation of $360°$ per day namely, approximately $86400\mathrm{s}$, the corresponding angular velocity is about $4.17\times {10}^{-3}°/\mathrm{s}$. At the measurement site in Yokosuka, Kanagawa Prefecture (latitude $35.26°$N), the local rotation rate is approximately $3.40\times {10}^{-3}°/\mathrm{s}$. Accordingly, the system configuration allows detection of Earth’s rotation employing the gyro sensor’s ${\omega }_x$, but not with the ${\omega }_y$.

From Figure A3, it can be seen that the ${\alpha }_x$ shows a random walk of $3.53\times {10}^{-4}\mathrm{m}/{\mathrm{s}}^2$, bias instability of $8.35\times {10}^{-5}\mathrm{m}/{\mathrm{s}}^2$, and drift at $3801\mathrm{s}$. The ${\alpha }_y$ shows a random walk of $6.25\times {10}^{-4}\mathrm{m}/{\mathrm{s}}^2$, bias instability of $6.51\times {10}^{-5}\mathrm{m}/{\mathrm{s}}^2$, and drift at $3385\mathrm{s}$. In summary, the ${\alpha }_x$ demonstrates an error of approximately $3.53\times {10}^{-4}\mathrm{m}/{\mathrm{s}}^2$, a resolution of approximately $8.35\times {10}^{-5}\mathrm{m}/{\mathrm{s}}^2$, and reliable measurement can be obtained up to $3801\mathrm{s}$ before drift occurs. The ${\alpha }_y$ demonstrates an error of approximately $6.25\times {10}^{-4}\mathrm{m}/{\mathrm{s}}^2$, a resolution of approximately $6.51\times {10}^{-5}\mathrm{m}/{\mathrm{s}}^2$, and reliable measurement can be obtained up to $3385\mathrm{s}$ before drift occurs.

According to Equations A3–A4, the resulting attitude estimation errors from the system are approximately $\pm 3.65\times {10}^{-3}°$ for roll $\varphi$ and $\pm 2.06\times {10}^{-3}°$ for pitch $\theta$, both of which are considered negligible.

Figure A2. Allan variance results of the gyro Sensor:The figure shows x- and y- axes from the top[5].

Figure A2. Allan variance results of the gyro Sensor:The figure shows x- and y- axes from the top[5].

Figure A3. Allan variance results of the accelerometer:The figure shows x- and y- axes from the top[5].

Figure A3. Allan variance results of the accelerometer:The figure shows x- and y- axes from the top[5].

Appendix B.1. Ellipsoid Estimation to Calculate the Offset Error and the Scale Error in Accelerometer

To level the compact 3–axis turntable, an accelerometer is employed. Accelerometer is subject to the offset error and the scale error, which can be corrected utilizing measurements taken in both horizontal and vertical rotations on the 3D planes. In other words, when the accelerometer is rotated once around both the x- and y- axes, the tim–series signals of the x-, y-, and z-axes form an ellipsoid in 3D space. Theoretically, these signals should form a sphere centered at $[x,y,z]=[0,0,0]$ with radii equal to g along the x-, y-, and z-axes. Deviations from this ideal sphere indicate the presence of the offset error and the scale error. By estimating the center and radii of the resulting ellipsoid, the offset error and the scale error of the accelerometer can be determined. Let roll $\varphi$ and pitch $\theta$ denote the orientation of the compact 3–axis turntable, and let $step$ denote the measurement interval of a triaxial ellipsoid, signals from the accelerometer as ${\mathbf{\alpha }}_n^s(={[{\alpha }_{x_n}^s,{\alpha }_{y_n}^s,{\alpha }_{z_n}^s]}^{\top })$, Averaged acceleration signal at each point as ${\mathbf{\alpha }}_{(i,k)}^e(={[{\alpha }_{{(i,k)}_x}^e,{\alpha }_{{(i,k)}_y}^e,{\alpha }_{{(i,k)}_z}^e]}^{\top })$, The flow of ellipsoid estimation is demonstrated in Algorithm A1. The procedure involves measuring data by rotating the x- axis, fixed at an arbitrary angle, around the y- axis in $step$ steps increments and stopping at each position. Subsequently, the x- axis is rotated in $step$ steps increments, and the aforementioned measurement is repeated $⌈{\displaystyle \frac{512}{step}}⌉$ times, yielding a total of ${⌈{\displaystyle \frac{512}{step}}⌉}^2$ data points. As a result, a point cloud is generated for each rotation angle in 3D space.

Algorithm A1 Rotation and Measurement Employing the Compact 3–Axis Turntable for Ellipsoid Estimation.

Require:  Signal from acceleration sensor$\mathbf{\alpha }(={[{\alpha }_x,{\alpha }_y,{\alpha }_z]}^{\top })$ Ensure:  Averaged acceleration signal at each point ${\mathbf{\alpha }}_{(i,k)}^e(={[{\alpha }_{{(i,k)}_x}^e,{\alpha }_{{(i,k)}_y}^e,{\alpha }_{{(i,k)}_z}^e]}^{\top })$  1: $i\leftarrow 0,k\leftarrow 0,\varphi \leftarrow 0,\theta \leftarrow 0$  2: for$i\leftarrow 1$to$⌈{\displaystyle \frac{512}{step}}⌉$do  3:     for $k\leftarrow 1$to$⌈{\displaystyle \frac{512}{step}}⌉$ do  4:         for $n\leftarrow 1$ to 30 do  5:            ${\mathbf{\alpha }}_n\leftarrow {[{\alpha }_x,{\alpha }_y,{\alpha }_z]}^{\top }$  6:         end for  7:         ${\displaystyle \overline{\mathbf{\alpha }}=\frac{{\sum }_{n=0}^{29}{\mathbf{\alpha }}_n}{30}}$  8:         ${\displaystyle {\mathbf{\alpha }}_j^e\leftarrow \overline{\mathbf{\alpha }}}$  9:         $\theta \leftarrow \min \left(\theta +360{\displaystyle \frac{step}{512}},360\right)$ ▹ Rotation command around y- axis 10:     end for 11:     $\varphi \leftarrow \min \left(\varphi +360{\displaystyle \frac{step}{512}},360\right)$ ▹ Rotation command around x- axis 12: end for 13: return  ${\mathbf{\alpha }}_{(i,k)}^e$

Let the offset error of the acceleration ${\mathbf{\alpha }}_n^c(={[{\alpha }_{x_n}^c,{\alpha }_{y_n}^c,{\alpha }_{z_n}^c]}^{\top })$, the scale error of the acceleration ${\mathbf{\alpha }}_n^r(={[{\alpha }_{x_n}^r,{\alpha }_{y_n}^r,{\alpha }_{z_n}^r]}^{\top })$. The ellipsoid is estimated from the ${\mathbf{\alpha }}_{(i,k)}^e$ from the Algorithm A1 utilizing the nonlinear least squares method (the "minimize" function in Python’s SciPy library) following Equation A1. Based on the estimated radii and center of the ellipsoid, the offset error and the scale error of the accelerometer are calculated.

$$\begin{array}{c}\hfill {\displaystyle \underset{{\alpha }_x^c,{\alpha }_y^c,{\alpha }_z^c,{\alpha }_x^r,{\alpha }_y^r,{\alpha }_z^r}{argmin}{\displaystyle \sum _{i=0}^{⌈\frac{512}{step}⌉}}{\displaystyle \sum _{k=0}^{⌈\frac{512}{step}⌉}}\left({\displaystyle 1-\frac{{\left({\alpha }_{{(i,k)}_x}^e-{\alpha }_x^c\right)}^2}{{{\alpha }_x^r}^2}-\frac{{\left({\alpha }_{{(i,k)}_y}-{\alpha }_y^c\right)}^2}{{{\alpha }_y^r}^2}-\frac{{\left({\alpha }_{{(i,k)}_z}-{\alpha }_z^c\right)}^2}{{{\alpha }_z^r}^2}}\right)}\end{array}$$

(A1)

As the step interval increases, the calibration time decreases; however, it may also lead to reduced accuracy. To verify the acceptable step interval, measurements were taken 10 times each for a total of 31 different step intervals, ranging from 4 to 124 steps.

Figure A4 shows the point clouds of the acceleration obtained at 4-step and 40-step intervals, respectively. In these figures, the blue dots represent signals from the accelerometer, and the green sphere indicates the estimated ellipsoid.

Figure A4. Point clouds of the acceleration[5]: (a) 4–step interval. (b) 40–step interval.

Figure A4. Point clouds of the acceleration[5]: (a) 4–step interval. (b) 40–step interval.

Figure A5 shows the standard deviations of the centers and radii of the resulting ellipsoids. In the figure, the blue, orange, and green dots show the standard deviations of the x-, y-, and z-axes of the ellipsoid centers, respectively. Similarly, the red, purple, and brown dots show the standard deviations of the x-, y-, and z-axes of the ellipsoid radii. From the figure, it can be observed that when the step interval is set to 40, all standard deviations remain below $5.00\times {10}^{-4}\mathrm{m}/{\mathrm{s}}^2$, which is comparable to the error levels in the x- and y-axes obtained from the Allan variance. Utilizing the ellipsoid obtained from these measurements, the compact 3–axis turntable is leveled by rotating it about roll $\varphi$ and pitch $\theta$ such that the corresponding angles are reduced to less than $7.03\times {10}^{-1}°$ namely, one step about roll $\varphi$ and pitch $\theta$, based on the accelerometer signals corrected for the offset error and the scale error.

Figure A5. Standard deviations of the centers and radii of the resulting ellipsoids[5].

Figure A5. Standard deviations of the centers and radii of the resulting ellipsoids[5].

Appendix B.2. Angular Velocity From Acceleration

The angular velocity ${\mathbf{\omega }}_n^{\alpha }$ based on the accelerometer signals can be calculated by utilizing the following equation.

$$\begin{array}{c}\hfill {\mathbf{\omega }}_n^{\alpha }=\left[\begin{array}{c}{\omega }_{x_n}^{\alpha }\\ {\omega }_{y_n}^{\alpha }\end{array}\right]=\left[\begin{array}{c}{\displaystyle \frac{{\varphi }_n^c-{\varphi }_{n-1}^c}{\Delta t}}\\ {\displaystyle \frac{{\theta }_n^c-{\theta }_{n-1}^c}{\Delta t}}\end{array}\right]\end{array}$$

(A2)

Appendix B.3. Attitude Estimation From Acceleration in S–frame

Attitude estimation in S–frame is expressed utilizing acceleration signals by the following equations.

$$\begin{array}{cc}\hfill & {\varphi }_n^{\alpha }=-{sin}^{-1}\left({\displaystyle \frac{{\alpha }_{y_n}^r}{\mathrm{g}}}\right)\left(-1<{\displaystyle \frac{{\alpha }_{y_n}^r}{\mathrm{g}}}<1\right)\hfill \end{array}$$

(A3)

$$\begin{array}{cc}\hfill & {\theta }_n^{\alpha }={sin}^{-1}\left({\displaystyle \frac{{\alpha }_{x_n}^r}{\mathrm{g}}}\right)\left(-1<{\displaystyle \frac{{\alpha }_{x_n}^r}{\mathrm{g}}}<1\right)\hfill \end{array}$$

(A4)

Appendix B.4. Outlier Correction Utilizing the RKF [6]

To remove outliers in the acceleration ${\mathbf{\alpha }}_n$, angular velocity ${\mathbf{\omega }}_n$, and ${\mathbf{\omega }}_n^{\alpha }$, the RKF is utilized. For outlier correction of acceleration, let the state $\mathbf{x}={[{\alpha }_x,{\alpha }_y]}^{\top }$, and the observation $\mathbf{y}={[{\alpha }_x,{\alpha }_y]}^{\top }$. For outlier correction of angular velocity, let the state $\mathbf{x}={[{\omega }_x,{\omega }_y]}^{\top }$, and the observation $\mathbf{y}={[{\omega }_x,{\omega }_y]}^{\top }$. Their stat–space model are represented by the following Equation A5.

$$\begin{array}{c}\hfill \left\{\begin{array}{c}{\mathbf{x}}_n={\mathbf{x}}_{n-1}+{\mathbf{v}}_n\hfill \\ {\mathbf{y}}_n={\mathbf{x}}_n+{\mathbf{w}}_n+{\mathbf{z}}_n\hfill \end{array}\right.\end{array}$$

(A5)

where, ${\mathbf{v}}_n$ is the system noise vector and is the two-dimensional Gaussian white noise sequence $N(\mathbf{0},\mathbf{Q})$. ${\mathbf{w}}_n$ is the observation noise vector and is the two-dimensional Gaussian white noise sequence $N(\mathbf{0},\mathbf{R})$. ${\mathbf{z}}_n$ is the outlier vector, $\mathbf{R}$ is a variance-covariance matrix of observation noise and are represented by Equation A7.

$$\begin{array}{cc}\hfill & \mathbf{Q}=q\mathrm{diag}({\sigma }_x^2,{\sigma }_y^2)\hfill \end{array}$$

(A6)

$$\begin{array}{cc}\hfill & \mathbf{R}=\left[\begin{array}{cc}{\sigma }_x^2& {\sigma }_{xy}\\ {\sigma }_{xy}& {\sigma }_y^2\end{array}\right]\hfill \end{array}$$

(A7)

Note that the coefficient q is an experimentally determined parameter: it is set to 1.0 for the accelerometer, $0.001$ for the angular velocity obtained from the gyro sensor, and $0.01$ for the angular velocity estimated from the accelerometer.

The state estimation of Equation A5 can be done by utilizing the RKF algorithm shown in Equations A8–A15.

[One-step-ahead prediction]

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{n|n-1}={\widehat{\mathbf{x}}}_{n-1|n-1}\hfill \end{array}$$

(A8)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{n|n-1}={\mathbf{P}}_{n-1|n-1}+\mathbf{Q}\hfill \end{array}$$

(A9)

[Filtering]

$$\begin{array}{cc}\hfill & {\Sigma }_n^2={\mathbf{P}}_{n|n-1}+\mathbf{R}\hfill \end{array}$$

(A10)

$$\begin{array}{cc}\hfill & \mathbf{L}={\mathbf{P}}_{n|n-1}{\left({\Sigma }_n^2\right)}^{-1}\hfill \end{array}$$

(A11)

$$\begin{array}{cc}\hfill & {\mathbf{e}}_n={\mathbf{y}}_n-{\widehat{\mathbf{x}}}_{n|n-1}\hfill \end{array}$$

(A12)

$$\begin{array}{cc}\hfill & {\widehat{z}}_{k,n}=\left\{\begin{array}{cc}{e}_{k_n}-{\Sigma }_{k{k}_n}\hfill & \mathrm{if}{e}_{k_n}\ge {\Sigma }_{k{k}_n}\hfill \\ e_{k_n}+{\Sigma }_{k{k}_n}\hfill & \mathrm{if}{e}_{k_n}<-{\Sigma }_{k{k}_n}\hfill \\ 0\hfill & \mathrm{otherwise}\hfill \end{array}\right.(k=\{x,y\})\hfill \end{array}$$

(A13)

$$\begin{array}{cc}\hfill & {\widehat{\mathbf{x}}}_{n|n}={\widehat{\mathbf{x}}}_{n|n-1}+\mathbf{L}({\mathbf{e}}_n-{\widehat{\mathbf{z}}}_n)\hfill \end{array}$$

(A14)

$$\begin{array}{cc}\hfill & {\mathbf{P}}_{n|n}=({\mathbf{I}}_2-\mathbf{L}){\mathbf{P}}_{n|n-1}\hfill \end{array}$$

(A15)