Coning, sculling and scrolling error compensation method for strapdown inertial navigation system

Abstract

The invention proposes a method to compensate for coning, sculling, and scrolling errors in a strapdown navigation system using a calculation technique that avoids using trigonometric and rounding functions. This method minimizes calculation errors and reduces computational load by eliminating the need for performing directional vector product multiplication twice. The technical solution used to compensate for motion errors that cause angle calculation errors is based on a function that approximates the calculation errors between methods, allowing the selection of the appropriate calculation method and rounding degree. The solution to compensate the velocity and position motion errors is achieved by above accurate angle error compensation.

Description

TECHNICAL FIELD COVERED

The invention covers coning, sculling and scrolling error compensation method for strapdown inertial navigation system. Specifically, the mentioned method utilizes data processing techniques.

TECHNICAL STATUS OF THE INVENTION

Strapdown inertial navigation system (SINS) is a navigation system that calculates the state (position, velocity, angle) of a vehicle in the future based on its initial state (dead-recknoing technique). With civil devices that do not need high precision or complicated moving orbits, or can optionally use integrated auxiliary sensor devices such as GNSS and ground information stations, the closed-loop combination of observers (such as the Kalman filter) can maintain or reduce the systematic error to an acceptable level with the help of auxiliary sensor updating process. However, in reality, devices that require high independence often move with complex trajectories to avoid obstacles and are not dependent on external auxiliary sensors (due to possible interference). Therefore, ensuring proactive systematic error in all situations is essential.

The problem of controlling the allocation of positioning errors is a critical aspect in the design of navigation systems with a given total error requirement. Inertial positioning errors are categorized into three main components: sensor error, initialization error, and computational error. Efforts to mitigate and estimate errors arising from sensors and initialization are always prioritized, as they are typically the primary contributors to the total error. However, neglecting computational errors can limit the operational range of the system. The computational errors discussed here include not only rounding errors due to hardware limitations (which are uncorrectable and constitute a small proportion) but also errors related to the rounding methods used during the integration of differential equations of strapdown inertial navigation system. In low-cost commercial systems, under conditions where GPS is not used and complex movements occur, the total strapdown inertial navigation system error will be greater than the estimated error caused by sensor and initialization errors. The movements referred to here include those causing angular computation errors (hereafter called coning motion), those causing velocity computation errors (hereafter called sculling motion), and those causing position computation errors (hereafter called scrolling motion).

The techniques for compensating errors caused by movements that generate angular, velocity, and position computational errors have been addressed since the introduction of strapdown inertial navigation systems in the 1970s. However, due to the limitations in high-frequency computation and the lack of effective approaches, there has been little progress in compensating for these errors over the years. These approaches have not been optimized for all types of movements and primarily focus on compensating for specific movements, such as pure coning, sculling, and scrolling motions that create angular, velocity, and position computational errors, respectively.

Current technical solutions for compensating errors in strapdown inertial navigation systems have two main drawbacks:

Complexity and computational load: These solutions often use trigonometric functions that are difficult to implement in embedded systems or employ rough approximation functions that result in large errors. Alternatively, they might use functions involving double vector products, which significantly increase computational load.

High-frequency computation requirements: To reduce computational errors, these solutions typically require high-frequency calculations. This is to compensate for the large errors that arise from low-frequency computations. Some methods use time-based functions to approximate acceleration and angular velocity over short periods for integration, resulting in a high computational burden. High-frequency computation solutions depend on the hardware capabilities of the inertial sensors to provide high-frequency acceleration or angular velocity data.

Motion Error Compensation Method for Angular Computational Errors:

Transforming the coordinate system (a) to another coordinate system (b) with the same origin by rotation can be represented using a set of Euler angles (roll, pitch, yaw), a 3×3 direction cosine matrix (DCM-C), or a four-dimensional vector (quaternion-q). With a basic understanding, these quantities can be converted into one another. The quaternion representing the transformation of coordinate system (b) obtained by rotating system (a) around a fixed unit vector e (which can be temporarily fixed) by an angle α is given by:

$\begin{array}{cc}q=\cos \frac{\alpha }{2}+e\sin \frac{\alpha }{2}=\left[q_0{q}_v\right],& \left(1\right)\end{array}$ $\mathrm{Here}{q}_0=\cos \frac{\alpha }{2},q_v=e\sin \frac{\alpha }{2}.$

The differential form of the equation (1) is obtained as follows:

$\begin{array}{cc}\stackrel{.}{q}=\frac{1}{2}q\mathrm{°\omega },& \left(2\right)\end{array}$

where ω is instantaneous angular velocity vector in the b-coordinate system (b).

From the equation (2) we have:

$\begin{array}{cc}\omega =2q^{*}°\stackrel{.}{q}=\stackrel{.}{\alpha }e+\sin \alpha \stackrel{.}{e}-\left(1-\cos \alpha \right)e\times \stackrel{.}{e}& \left(3\right)\end{array}$

Equation (3) shows that if vector e is unchanged during the t₁ to t₂ period of time, we have:

$\alpha ={\int }_{t_1}^{t_2}\mathrm{wdt}.$

here w=|ω|.

However, the positioning calculation in equation (2) requires the integration of the angular velocity vector w over time rather than its magnitude. Note that the vector e can change over time. Without loss of generality, we define the vector ϕ as follows:

$\begin{array}{cc}\varphi =f\left(\alpha \right)e.& \left(4\right)\end{array}$

On the other hand, from equation (2) combined with equations (1) and (3), we have:

$\begin{array}{cc}\stackrel{.}{\alpha }=\omega ·e;\stackrel{.}{e}=\frac{1}{2}e\times \omega +\frac{1}{2}e\times \left(e\times \omega \right)\cot \frac{\alpha }{2}& \left(5\right)\end{array}$

From equation (4), we obtain:

$\begin{array}{cc}\stackrel{.}{\varphi }=\frac{\partial f}{\partial \alpha }\stackrel{.}{\alpha }e+f\left(\alpha \right)\stackrel{.}{e}\stackrel{\mathrm{def}}{=}{f}_{\alpha }^{\prime }\left(\alpha \right)\stackrel{.}{\alpha }e+f\left(\alpha \right)\stackrel{.}{e}.& \left(6\right)\end{array}$

Combining equations (5) and (6), we obtain:

$\begin{array}{cc}\stackrel{.}{\varphi }=f_{\alpha }^{\prime }\left(\alpha \right)\omega +\frac{1}{2}\varphi \times \omega +\frac{1}{f^2\left(\alpha \right)}\left(f_{\alpha }^{\prime }\left(\alpha \right)-\frac{1}{2}f\left(\alpha \right)\cot \frac{\alpha }{2}\right)\varphi \times \left(\varphi \times \omega \right).& \left(7\right)\end{array}$

Currently, technical solutions commonly used to address the problem of compensating coning errors can be classified into two types:

The first approach is the rotation vector method:

With the traditional solution approach

$\begin{array}{cc}f\left(\alpha \right)=\alpha ,& \left(8\right)\end{array}$

Vector ϕ from here will be call the rotation vector:

$\begin{array}{cc}\varphi =\alpha e.& \left(9\right)\end{array}$

when f(a)=α, we get:

$\begin{array}{cc}{f}_{\alpha }^{\prime }\left(\alpha \right)=1.& \left(10\right)\end{array}$

From equations (1) and (9), we have:

$\begin{array}{cc}q=\cos \frac{❘\varphi ❘}{2}+\frac{\varphi }{❘\varphi ❘}\sin \frac{❘\varphi ❘}{2}& \left(11\right)\end{array}$

Substituting equations (8) and (10) into equation (7), we obtain:

$\begin{array}{cc}\stackrel{.}{\varphi }=\omega +\frac{1}{2}\varphi \times \omega +\frac{1}{{❘\varphi ❘}^2}\left(1-\frac{❘\varphi ❘\sin ❘\varphi ❘}{2\left(1-\cos ❘\varphi ❘\right)}\right)\varphi \times \left(\varphi \times \omega \right)\approx \omega +\frac{1}{2}\varphi \times \omega +\frac{1}{12}\varphi \times \left(\varphi \times \omega \right)& \left(12\right)\end{array}$

The relationship between the direction cosine matrix (DCM) and the four-dimensional vector (quaternion) is as follows:

$\begin{array}{cc}C=\left(q_0^2-q_v^T{q}_v\right)I+2q_v{q}_v^T-2q_0{q}_v\times .& \left(13\right)\end{array}$

Given equations (13) and (11), for a small dt, we can approximate as follows:

$\begin{array}{cc}C=\cos ❘\varphi ❘I+\frac{1-\cos ❘\varphi ❘}{{❘\varphi ❘}^2}{\left(\varphi \times \right)}^2-\frac{\sin ❘\varphi ❘}{❘\varphi ❘}\varphi \times \approx I-\frac{\sin ❘\varphi ❘}{❘\varphi ❘}\varphi \times +\frac{1-\cos ❘\varphi ❘}{{❘\varphi ❘}^2}{\left(\varphi \times \right)}^2.& \left(14\right)\end{array}$

The second approach is the Rodrigues vector:

Rodrigues vector is defined as follows:

$\begin{array}{cc}\varphi =2\tan \frac{\alpha }{2}e.& \left(15\right)\end{array}$

In this case, we have:

$\begin{array}{cc}{f}_{\alpha }^{\prime }\left(\alpha \right)=\frac{1}{{\cos }^2\frac{\alpha }{2}}=1+{\tan }^2\frac{\alpha }{2},& \left(16\right)\end{array}$

Combining equations (15) and (1), we obtain:

$\begin{array}{cc}q=\frac{2+\varphi }{\sqrt{4+{❘\varphi ❘}^2}},& \left(17\right)\end{array}$

Substituting equations (15) and (16) into equation (7), we have:

$\begin{array}{cc}\dot{\varphi }=\left(1+\frac{{\varphi }^2}{4}\right)\omega +\frac{1}{2}\varphi \times \omega +\frac{1}{4}\varphi \times \left(\varphi \times \omega \right)\approx \omega +\frac{1}{2}\varphi \times \omega +\frac{1}{4}\varphi \times \left(\varphi \times \omega \right)& \left(18\right)\end{array}$

Similarly, using equations (13) and (17), we have:

$\begin{array}{cc}C=\left(\frac{4-{❘\varphi ❘}^2}{4+{❘\varphi ❘}^2}\right)I+\frac{2{\left(\varphi \times \right)}^2}{4+{❘\varphi ❘}^2}-\frac{4\varphi \times }{4+{❘\varphi ❘}^2}\approx I-\frac{4}{4+{❘\varphi ❘}^2}\varphi \times +\frac{2}{4+{❘\varphi ❘}^2}{\left(\varphi \times \right)}^2& \left(19\right)\end{array}$

It is evident that both approaches in equations (12) and (18) involve expressions for ϕ with three components, with the third component typically requiring the most computational effort. Additionally, the expression for the DCM matrix always contains three components.

Velocity and Position Error Compensation Method:

In this section, the author summarizes results from other researchers, particularly from Paul G. Savage's series [Paul G. Savage, “Strapdown Analytics,” Strapdown Associates, 2000, 2nd edition, two parts]. The main objective is to highlight the components that the navigation system needs to compensate for in differential equation calculations. These errors are generated by movements, referred to as coning, sculling, and scrolling.

Computation of a strapdown inertial navigation system on different coordinate reference frames (such as ECI, ECEF, NED, etc.) are based on convenience in calculations, habits, and the integration of various data sources. Similarly, calculations involving Euler angles, DCM differential equations, or quaternions are performed.

For example, when computing in the NED frame we have:

For Euler angles, derived from the DCM matrix using equation (13), the quaternion is calculated using the rotation vector differential equation (7).

For the velocity:

$\begin{array}{cc}{v}_m^N=v_{m-1}^N+v_g^N\left(t_m\right)+C_{B_{m-1}}^N{v}_{\mathrm{SF}}^{B_{m-1}}\left(t_m\right)& \left(20.1\right)\end{array}$ $\mathrm{Here}{v}_g^N\left(t\right)={\int }_{t_{m-1}}^t{g}^N\mathrm{dt};$ $v_{\mathrm{SF}}^{B_{m-1}}\left(t\right)={\int }_{t_{m-1}}^t{\stackrel{.}{v}}_{\mathrm{SF}}^{B_{m-1}}\left(t\right)\mathrm{dt}$

N—Navigation frame, ν—velocity, C—Direction cosine matrix, g—gravity, SF—specific force, b or B—body frame, i—inertial frame, m—sample following the time t.

For the position:

$\begin{array}{cc}{p}_m^N=p_{m-1}^N+v_{m-1}^N{T}_m+p_g^N\left(t_m\right)+C_{B_{m-1}}^N{p}_{\mathrm{SF}}^{B_{m-1}}\left(t_m\right)& \left(20.2\right)\end{array}$

In which

$p_g^N\left(t\right)={\int }_{t_{m-1}}^t{v}_g^N\mathrm{dt};$ $p_{\mathrm{SF}}^{B_{m-1}}\left(t\right)={\int }_{t_{m-1}}^t{\stackrel{.}{p}}_{\mathrm{SF}}^{B_{m-1}}\left(t\right)\mathrm{dt};$

p—position, T_m—the duration from t_m-1 to t_m.

With average understanding, the solution to the positioning equations for velocity and position in the body frame, assuming that the orientation of the rotation vector is constant and that the direction of acceleration, angular velocity, and their ratios remain unchanged, as well as the lack of correlation between the DCM matrix and the velocity vector, is given by:

$\begin{array}{cc}{v}_{\mathrm{SF}}^{B_{m-1}}\left(t\right)=\left[I+f_2\left(\alpha \right)\alpha \times +f_3\left(\alpha \right){\left(\alpha \times \right)}^2\right]v;& \left(21.1\right)\end{array}$ $\begin{array}{cc}{p}_{\mathrm{SF}}^{B_{m-1}}\left(t\right)=\left[I+2f_3\left(\alpha \right)\alpha \times +2f_4\left(\alpha \right){\left(\alpha \times \right)}^2\right]S_v;& \left(21.2\right)\end{array}$

In which

$\alpha \equiv {\int }_{t_{m-1}}^t{\omega }_{\mathrm{ib}}^b\mathrm{dt};v\equiv {\int }_{t_{m-1}}^t{f}_{\mathrm{ib}}^b\mathrm{dt};S_v\equiv {\int }_{t_{m-1}}^t\mathrm{vdt};f_1\left(\alpha \right)=\sin \alpha /\alpha ;$ $f_2\left(\alpha \right)=\left(1-\cos \alpha \right)/{\alpha }^2;f_3\left(\alpha \right)=\left(1-f_1\left(\alpha \right)\right)/{\alpha }_2;$ $f_4\left(\alpha \right)=\left(1/2-f_2\left(\alpha \right)\right)/{\alpha }_2;f_{\mathrm{ib}}^b-$

The measured value returned by the accelerometer, ω_ib^b—The measured value returned by the gyroscope;

For low-cost navigation systems, the calculations are typically performed according to equation (21).

Under general assumptions and with a basic understanding, the system of navigation equations can be expressed in the following generalized form:

$\begin{array}{cc}{v}_{\mathrm{SF}}^{B_{m-1}}\left(t\right)=\left[I+f_2\left(\Phi \right)\Phi \times +f_3\left(\Phi \right){\left(\Phi \times \right)}^2\right]\eta ;& \left(22.2\right)\end{array}$ $\begin{array}{cc}{p}_{\mathrm{SF}}^{B_{m-1}}\left(t\right)=\left[I+2f_3\left(\Phi \right)\Phi \times +2f_4\left(\Phi \right){\left(\Phi \times \right)}^2\right]\zeta ;& \left(22.3\right)\end{array}$ $\mathrm{Here}{f}_1\left(\Phi \right)=\sin \Phi /\Phi =1-{\Phi }^2/3!+{\Phi }^4/5!-\dots ;$ $f_2\left(\Phi \right)=\left(1-\cos \Phi \right)/{\Phi }^2=1/2!-{\Phi }^2/4!+{\Phi }^4/6!-\dots ;$ $f_3\left(\Phi \right)=\left(1-f_1\left(\Phi \right)\right)/{\Phi }^2=1/3!-{\Phi }^2/5!+\dots ;$ $f_4\left(\Phi \right)=\left(1-f_2\left(\Phi \right)\right)/{\Phi }^2=1/4!-{\Phi }^2/6!+\dots ;$

In which Φ=|Φ| and is defined in (12) and (14). The components Φ, η and ζ are calculated as follows:

$\begin{array}{cc}\stackrel{.}{\Phi }=\omega +\frac{1}{2}\varphi \times \omega +f_5\varphi \times \left(\varphi \times \omega \right);& \left(23.1\right)\end{array}$ $\begin{array}{cc}\stackrel{.}{\eta }=f_{\mathrm{ib}}^b+\frac{1}{2}\left(\varphi \times f_{\mathrm{ib}}^b+\stackrel{.}{\Phi }\times \eta \right)+f_5\varphi \times \left(\varphi \times f_{\mathrm{ib}}^b-\stackrel{.}{\Phi }\times \eta \right)+f_3\left(\varphi \times \stackrel{.}{\Phi }\right)\times \eta -\frac{1}{2}{f}_3\varphi \times \left[\left(\varphi \times \stackrel{.}{\Phi }\right)\times \eta \right]+f_4{\varphi }^2\stackrel{.}{\Phi }\times \eta +f_6\varphi \times \left[\left(\varphi \times \stackrel{.}{\Phi }\right)\times \eta \right]+\frac{1}{2}{f}_3\varphi ·\mathrm{\omega \varphi }\times \eta +f_7\left(\varphi \right)\times \left\{\varphi \times \left(\varphi \times \stackrel{.}{\Phi }\right)\times \eta \right\}+f_8{\varphi }^2\left(\varphi \times \stackrel{.}{\Phi }\right)\times \eta -f_8\varphi ·\mathrm{\omega \varphi }\times \left(\varphi \times \eta \right)& \left(23.2\right)\end{array}$ $\begin{array}{cc}\stackrel{.}{\zeta }=\eta +\frac{1}{6}\left(\varphi \times \eta -2\stackrel{.}{\varphi }\times \zeta \right)+f_9\varphi \times \left(\varphi \times \eta -2\stackrel{.}{\varphi }\times \zeta \right)+2f_4\left(\varphi \times \stackrel{.}{\Phi }\right)\times \zeta -f_{10}\varphi \times \left[\left(\varphi \times \stackrel{.}{\varphi }\right)\times \zeta \right]-f_{11}{\varphi }^2\varphi \times \eta +f_{12}{\varphi }^2\stackrel{.}{\varphi }\times \eta +f_{13}\varphi \times \left[\varphi \times \left(\stackrel{.}{\varphi }\times \zeta \right)\right]+f_{14}\varphi ·\mathrm{\omega \varphi }\times \zeta +f_{15}\varphi \times \left\{\varphi \times \left[\left(\varphi \times \stackrel{.}{\varphi }\right)\times \zeta \right]\right\}+f_{16}{\varphi }^2\varphi \times \left(\stackrel{.}{\varphi }\times \eta \right)-f_{17}\varphi ·\mathrm{\omega \varphi }\times \left(\varphi \times \zeta \right)& \left(23.3\right)\end{array}$ $\mathrm{Where}{f}_5=\frac{1}{{\varphi }^2}\left[1-\frac{\varphi \sin \varphi }{2\left(1-\cos \varphi \right)}\right];f_6=\frac{1}{{\varphi }^2}\left(1-\frac{1}{2}{f}_1-f_2\right);$ $f_7=f_3{f}_5;f_8=\frac{1}{2{\varphi }^4}\left[5+\cos \varphi -\frac{3\varphi \sin \varphi }{\left(1-\cos \varphi \right)}\right];$ $f_9=f_3-f_2{h}_1+h_2\left(1-f_3{\varphi }^2\right);$ $f_{10}=2f_4{h}_2;f_{11}=\frac{1}{{\varphi }^2}\left[h_1\left(1-f_3{\varphi }^2\right)-f_2\left(1-h_2{\varphi }^2\right)+\frac{1}{6}\right];$ $f_{12}=\frac{1}{{\varphi }^2}\left(\frac{1}{6}+f_3\right);f_{13}=2\left(2f_4{h}_1-f_3{h}_2\right);$ $f_{14}=\frac{2}{{\varphi }^4}\{\left(1+f_1-4f_2\right)h_1{\varphi }^2-\left[f_2{\varphi }^2-3\left(1-f_1\right)\right]\left(1-h_2{\varphi }^2\right)];$ $f_{15}=2f_4{h}_2;$ $f_{16}=\frac{2}{{\varphi }^2}\left\{f_3-2f_4-h_1\left(f_2-f_3\right)+h_2\left[1-\left(f_3-2f_4\right){\varphi }^2\right]\right\};$ $f_{17}=-\frac{2}{{\varphi }^2}\left\{\left[f_2{\varphi }^2-3\left(1-f_1\right)\right]h_1+\left(1+f_1-4f_2\right)\left(1-h_2{\varphi }^2\right)\right\};$ $h_1=\frac{f_3}{2\left(f_2^2+f_3^2{\varphi }^2\right)};h_2=\frac{f_3^2-f_2{f}_4}{f_2^2+f_3^2{\varphi }^2};$

Note that the components on the right side of expression (23) starting from the second one are categorized as follows: In equation (23.1), they are referred to as the coning component. In equation (23.2), they are referred to as the velocity component, and the components in equation (23.3) are referred to as the position component. If we revisit equation (31), we see that these errors are not compensated for.

Thus, after solving equation (22) in its general form and discretizing it, we obtain the following:

$v_m^N=v_{m-1}^N+v_{g_m}^N+C_{B_{m-1}}^N{v}_{{\mathrm{SF}}_m}^{B_{m-1}}$ $p_m^N=p_{m-1}^N+v_{m-1}^N{T}_m+p_{g_m}^N+C_{B_{m-1}}^N{p}_{{\mathrm{SF}}_m}^{B_{m-1}}$ $C_{B_m}^N=f\left({\Phi }_m\right)$ $v_{{\mathrm{SF}}_m}^{B_{m-1}}=\left[I+f_2\left({\Phi }_m\right){\Phi }_m\times +f_3\left({\Phi }_m\right){\left({\Phi }_m\times \right)}^2\right]{\eta }_m$ $p_{{\mathrm{SF}}_m}^{B_{m-1}}=\left[I+2f_2\left({\Phi }_m\right){\Phi }_m\times +2f_4\left({\Phi }_m\right){\left({\Phi }_m\times \right)}^2\right]{\zeta }_m$ ${\Phi }_m\equiv {\int }_{t_{m-1}}^{t_m}\stackrel{.}{\Phi }\mathrm{dt};{\eta }_m\equiv {\int }_{t_{m-1}}^{t_m}\stackrel{.}{\eta }\mathrm{dt};{\zeta }_m\equiv {\int }_{t_{m-1}}^t\stackrel{.}{\zeta }\mathrm{dt}$

In which Φ_m, η_m and ζ_m are calculated by solving the differential equation (23) combined with the use of the rounding function as follows:

• • f₃≈⅙; f₄≈ 1/24; f₅≈ 1/12; f₆≈⅛;
  • f₉≈ 1/36; f₁₀≈ 1/36; f₁₁≈ 1/540;
  • f₁₂≈ 1/60; f₁₃≈ 5/108; f₁₄≈ 1/30;

In practice, there have been many specific implementations that can be applied to embedded software. However, the piecewise computation of each part in equation (23) is quite challenging, and the algorithm is not yet truly optimized for all types of motion systems.

TECHNICAL NATURE OF THE INVENTION

The existing methods for compensating motion errors that cause angular, velocity, and position computation errors, as discussed in the technical state of the invention, have certain limitations. The objective of this invention is to utilize a new set of variables in the formula for computing the compensation for motion errors, simplifying the calculation of angular error compensation by reducing the reliance on double vector multiplication or trigonometric functions, and controlling accuracy by using error approximation functions to compare methods. Another goal is to improve the accuracy of the compensation for angular computation errors, which can then be applied to the calculation of velocity and position compensation according to reusable formulas mentioned in the technical state of the invention.

Given the frequency of motion that causes angular, velocity, and position computation errors, it is crucial to choose a computation frequency that is at least twice this frequency, as per the Nyquist sampling theorem. Ensuring the computation frequency meets Nyquist's requirements, the proposed method solves the differential equations at the same frequency as the navigation computation frequency using methods like the Dormand-Prince fifth or eighth-order, instead of older solutions that required higher computation frequencies for error compensation or building time-based functions for acceleration and angular velocity to perform integration.

To achieve the above objectives, the proposed technical solution includes the following steps: step 1: select the computation function; step 2: choose the computation frequency and a method for solving the system of differential equations based on the analysis of the motion frequency of the object; step 3: initialize initial values for strapdown inertial navigation system in the chosen reference frame; step 4: calculate the coning error compensation; step 5: calculate the quaternion, DCM matrix, and Euler angles; step 6: compute the rotation vector value; step 7: compute the sculling and scrolling error compensation; step 8: calculate velocity and position outputs in the computation coordinate system.

BRIEF DESCRIPTION OF THE DRAWINGS

FIG. 1: Illustrates the steps involved in the proposed method.

FIG. 2: Shows a chart comparing the error between different approximation methods.

FIG. 3: Displays a chart comparing Euler angle calculations using various methods for compensating errors that cause angular computation errors.

FIG. 4: Presents a chart comparing rotation angle calculations for compensation of motion errors causing angular computation errors using different compensation methods.

FIG. 5: Provides a chart comparing velocity calculations using various methods for compensating motion errors causing angular computation errors.

FIG. 6: Depicts a chart comparing position calculations using different methods for compensating motion errors causing angular computation errors.

DETAILED DESCRIPTION OF THE INVENTION

The proposed method for compensating motion errors that lead to angular computation errors involves:

The approach of this method is as follows, choose f(α) in the final expression of the left-hand side of (7), such that:

$\begin{array}{cc}{f}_{\alpha }^{\prime }\left(\alpha \right)-\frac{1}{2}f\left(\alpha \right)\cot \frac{\alpha }{2}=0& \left(24\right)\end{array}$

In this case, the differential expression of ϕ in (7) will have only two components. To solve the differential equation, we select f(α) as follows:

$\begin{array}{cc}f\left(\alpha \right)=\sin \frac{\alpha }{2}& \left(25\right)\end{array}$

Similarly, we have:

$\begin{array}{cc}{f}_{\alpha }^{\prime }\left(\alpha \right)=\frac{1}{2}\cos \frac{\alpha }{2}& \left(26\right)\end{array}$ $\begin{array}{cc}\varphi =\sin \frac{\alpha }{2}e& \left(27\right)\end{array}$ $\begin{array}{cc}q=\sqrt{1-{❘\varphi ❘}^2}+\varphi & \left(28\right)\end{array}$

(28) is obtained using the small angle α and (7) has the following form:

$\begin{array}{cc}\stackrel{.}{\varphi }=\frac{1}{2}\sqrt{1-{❘\varphi ❘}^2}\omega +\frac{1}{2}\varphi \times \omega & \left(29\right)\end{array}$

Using the Taylor approximation function:

$\begin{array}{c}{\left(1+x\right)}^{\frac{1}{2}}=1+\frac{1}{2}x-\frac{1}{2.4}{x}^2+\frac{1.3}{2.4\text{.6}}{x}^3-\dots v{ó}^{\prime }i-1<x\le 1\\ =1+\frac{1}{2}x-\frac{1}{2.4}{x}^2+O\left(x^3\right)\end{array}$

(29) with the second-order Taylor approximation, can be approximated as:

$\begin{array}{cc}\dot{\varphi }\approx \frac{1}{2}\left(1+\varphi \times \right)\omega -\frac{1}{4}{\varphi }^2\left(1+\frac{1}{4}{\varphi }^2\right)\omega & \left(30\right)\end{array}$

Similar to (14), (19), from (13), (28) we obtain:

$\begin{array}{cc}C=\left(1-2{❘\varphi ❘}^2\right)I-2\sqrt{1-{❘\varphi ❘}^2}\varphi \times +2{\left(\varphi \times \right)}^2.& \left(31\right)\end{array}$

(31) can be approximated as follows:

$\begin{array}{cc}C\approx I-2\varphi \times +2{\left(\varphi \times \right)}^2.& \left(32\right)\end{array}$

Comparing expressions (29-30) with the results of the first method (12) and the second method (18) discussed in the technical background of the invention, we observe that expressions (29-30) do not contain trigonometric components or double vector multiplications. Thus, these expressions offer a computational advantage for embedded systems.

To compare the accuracy of the proposed method for compensating motion-induced angular computation errors, as well as the rounding order, the proposed method employs the following error approximation functions to evaluate effectiveness:

$f_0\left(x\right)=❘\frac{1}{2}\sqrt{1-{❘x❘}^2}-\frac{1}{2}❘,$ $f_1\left(x\right)=❘\frac{1}{2}\sqrt{1-{❘x❘}^2}-\frac{1}{2}\left(1-\frac{1}{2}{❘x❘}^2\right)❘,$ $f_2\left(x\right)=❘\frac{1}{2}\sqrt{1-{❘x❘}^2}-\frac{1}{2}\left(1-\frac{1}{2}{❘x❘}^2-\frac{1}{8}{❘x❘}^4\right)❘,$ $g\left(x\right)=❘\frac{1}{12}-\frac{1}{{❘x❘}^2}\left[1-\frac{❘x❘\sin \left(\left|x\right|\right)}{2\left(1-\cos \left(x\right)\right)}\right]❘,$ $h\left(x\right)=g\left(x\right){❘x❘}^2,$ $k\left(x\right)=\frac{{❘x❘}^2}{4},$

Refer to the comparison results shown in FIG. 2, in which

$f_0\left(\sin \frac{x}{2}\right),f_1\left(\sin \frac{x}{2}\right),f_2\left(\sin \frac{x}{2}\right)-$

The error approximation functions use zero-order, first-order, and second-order Taylor series expansions as indicated in expression (29) of the proposed method. These are labeled accordingly in FIG. 2 as SuF0, SuF1 và SuF2; g(x)—The error approximation functions in the first method, as shown in equation (12), are labeled as “Savage” in FIG. 2 (named after the author who used the rotation vector method).

$k\left(2\tan \frac{x}{2}\right)$

The error approximation functions in the second method, as shown in equation (18), are labeled as “Wu” in FIG. 2. In FIG. 2, it is evident that using second-order approximations or exact functions results in the smallest computational errors for compensating motion-induced angular computation errors.

The proposed method for compensating motion-induced velocity and position errors involves:

There are various methods for solving differential equations, such as Picard, Taylor, Euler, and Runge-Kutta. Each method has its own advantages and disadvantages. In this proposed solution, the ODE8 (Dormand-Prince) method is used to solve the differential equations.

$\begin{array}{cc}\Phi =\frac{2a\sin \left({\Phi }_p\right)}{{\Phi }_p}{\Phi }_p;& \left(33.1\right)\end{array}$ $\begin{array}{cc}\dot{\eta }\approx f_{ib}^b+\frac{1}{12}\left(6+\varphi \times \right)\left(\varphi \times f_{ib}^b-\dot{\Phi }\times \eta \right)+\frac{1}{6}\left(\varphi \times \dot{\Phi }\right)\times \eta & \left(33.2\right)\end{array}$ $\begin{array}{cc}\dot{\zeta }\approx \eta +\frac{1}{36}\left(6+\varphi \times \right)\left(\varphi \times \eta -2\dot{\varphi }\times \zeta \right)+\frac{1}{12}\left(\varphi \times \dot{\varphi }\right)\times \zeta & \left(33.3\right)\end{array}$

Φ_p—The compensation for motion-induced angular computation errors is calculated in the section above using (29).

The following description provides a detailed implementation of the invention with reference to the accompanying drawings (FIG. 1). The proposed implementations are presented in a clear and comprehensive manner to convey the concepts fully to those with average knowledge in the field. Although the concepts of the invention are illustrated in various forms, they should not be construed as being limited to the specific implementations described here. It should be understood that the concepts of the invention encompass all modifications, equivalents, and alternatives that fall within the scope of the invention.

The method for compensating motion-induced errors that cause angular, velocity, and position computation errors in strapdown inertial navigation systems is specified through the following steps:

Step 1: Select the computation function for compensating motion-induced errors that result in angular computation errors, based on the required angular computation error and the computational load (time). Use the theoretical error computation chart to guide this selection.

Depending on the desired computation error (accuracy), the computational load (time), or convenience for embedded system implementation, choose between an approximation function or an exact function from equation (29). Equation (29) has been theoretically computed and presented in graphical form (FIG. 2). For embedded systems, using approximation methods for multiplication is more convenient than square root operations. With computer support for error computation and computational load (time), an approximation function for compensating motion-induced angular computation errors will be provided.

Step 2: choose the computation frequency and the method for solving the system of differential equations based on the analysis of the object's motion frequency. The computation frequency depends on the motion frequency of the mechanical object, which is typically known before designing the application or environment. Alternatively, frequency spectrum analyzers can be used to determine the object's motion frequency. For strapdown inertial sensor systems with a cut-off frequency f₁, or additional vibration damping with a damping frequency f₂. the computation frequency is usually 3-5 times greater than the minimum of f₁ and f₂. The proposed but not limited differential equation solving method is the Dormand-Prince method of order 5 or higher.

Step 3: initialize initial values for strapdown inertial navigation system in the chosen reference frame. This step involves initializing the starting values for solving the differential equations system (29, 33.2, 33.3). Providing incorrect initial values will result in initialization errors in the system.

The variables requiring initial values are the initial position, velocity, and angle of the object before computation. Based on the initial values of position, velocity, and angle, the initial values for Φ, η and ζ will be computed using the related equations (28, 22.2, and 22.3). The angular velocity (ω), and acceleration (f_ib^b) will be continuously updated by the Inertial Measurement Unit (IMU). As mentioned in the introduction, the final total error will be divided into three parts: errors due to incorrect initialization (such as errors in position, velocity, and angle for the object in the positioning system initialization method, typically using high-precision master devices), sensor errors (i.e., measurement errors of the IMU sensors), and errors addressed in this invention, which are errors resulting from the computation of differential equations (describing the object's dynamics).

Step 4: calculate the coning error compensation. After selecting the function in step 1, the frequency and method for solving the differential equations in step 2, and the initial values in step 3, compute the coning error compensation.

The choice of function in step 1 depends on the required error and the computational load (time) for each computation cycle. The choice of computation frequency depends on the cut-off frequency used in the inertial sensor (i.e., the sensor's limit) and the design or application of the device containing the inertial sensor.

For example, in the case when using the second-order approximation (30), a computation frequency of 100 Hz is applied for positioning devices on marine vessels (with the primary sea wave frequency concentrated between 0 and 0.3 Hz, the ship's turning motion frequency being less than 1 Hz, and the engine's vibration frequency being 50 Hz, although with much lower amplitude compared to the sea wave and motion frequencies due to the inertial sensor's location away from the engine on the ship). The proposed solution utilizes the widely known ODE8 (Dormand-Prince method of order 8). Based on the previous value Φ (at time t) and the angular velocity ω from the IMU sensor (at time t), the value of Φ at the next step (at the time t+0.01 second) is computed. This value represents the compensation for motion-induced errors affecting angular computation that needs to be calculated.

Step 5: calculate the quaternions, direction cosine matrix (DCM), and Euler angles using the coning error compensation obtained in step 4.

The purpose of this step is to calculate the Euler angles, the direction cosine matrix (DCM), or the quaternion for the next time instance (at t+0.01 seconds) after determining the motion error compensation for angular computation errors obtained in step 4 (at t+0.01 seconds). These relationships are described by equations (28) and (31).

Step 6: compute the rotation vector Φ according to equation (33.1) and the rate of change of the angular velocity vector {dot over (Φ)} according to equation (12).

The purpose of this step is to compute the values of the angular velocity vector and its rate of change (at time t) based on the motion error compensation for angular computation errors obtained in step 4 (at time t) to be used in the calculation of motion error compensation for velocity and position performed in step 7.

Step 7: compute the sculling and scrolling error compensation by solving the differential equations (33.2) and (33.3). This is done based on the calculation frequency, the numerical method for solving differential equations chosen in step 2, and the initial values set in step 3.

Leveraging the high accuracy (controlled in step 1) in the motion error compensation for angular computation errors calculated in step 4 (at time t), the motion error compensation for velocity and position is computed in this step by using a combination of buffers (angular velocity vector and its rate of change) from step 6 (at time t). The ODE8 (Dormand-Prince) method of order 8 is used to solve the differential equations (33.2 and 33.3) to compute the motion error compensation for velocity and position at time t+0.01 seconds.

Step 8: calculate the output values for velocity and position in the computation coordinate system. For the NED (North-East-Down) coordinate system, this involves using the DCM matrix obtained in step 5 and the velocity and position compensation calculated in Step 7.

The purpose of this step is to update the new values for position, velocity, and angles (at time t+0.01 seconds) after calculating the compensation for errors due to motion affecting the angle, velocity, and position using the relationships provided in equations (28, 22.2, and 22.3).

In the subsequent computation cycles (at times t+0.02 seconds and following), steps 4 through 8 are performed sequentially until the positioning process is completed (providing the position, velocity, and angle of the device containing the inertial measurement unit over time).

The methods outlined are detailed purely as technical solutions intended to describe the invention and are not intended to limit the scope of the invention. Although the invention is described in detail with reference to the methods outlined, it should be understood that those skilled in the art may create variations of the technical solutions described or equivalently substitute the technical features of the solutions without departing from the spirit and scope of the proposed technical solution as described in the implementation methods of the invention.

EXAMPLE IMPLEMENTATION OF THE INVENTION

FIGS. 3 to 6 depict the results of implementing the invention using a simulated dataset of acceleration and angular velocity, which illustrates the motion-induced errors affecting angular calculations, velocity, and position as follows:

${\omega }_{ib}^b=-2\pi \Omega \sin \left(\alpha \right)\left[\tan \left(\frac{\alpha }{2}\right)\sin \left(2\pi \Omega t\right)-\cos \left(2\pi \Omega t\right)\right];$ $f_{ib}^b=g\left[\sin \left(2\pi \Omega t+\frac{\pi }{5}\right)\sin \left(2\pi \Omega t+\frac{2\pi }{3}\right)\sin \left(2\pi \Omega t+\frac{9\pi }{7}\right)\right]$

in which, g=9.81 m/s², α=10°, Ω=1 Hz.

Referring to FIGS. 3 through 6, the errors in calculations between various methods for Euler angles, quaternion values, velocity, and position, caused by motion-induced errors, are demonstrated; The lines labeled SuFn, SuF2, SuF1, and SuF0 represent the results from using the exact function for calculating motion-induced errors in angular computation (29), or the second-order, first-order, and zero-order approximations based on Taylor series expansion. Wu denotes the results from using the error compensation function for angular computation as per equation (18), while Savage and Savage F2 refer to the results from using the error compensation function for angular computation according to equation (12), with equation (12) lacking the third component on the right-hand side of the expression. The comparison results for velocity and position are benchmarked against the system with inputs from equations (33.2) and (33.3), where the error compensation due to motion-induced angular computation is theoretically calculated without errors.

Table 1 provides an example illustrating how different motion frequencies that cause errors in angular computation, velocity, and position, as well as different angular velocities, result in varying positioning output errors. This also serves as a basis for selecting the system's calculation frequency.

TABLE 1
Comparison of position calculation errors between two
methods using input pairs α and Ω
Method	α, °; Ω, Hz	50; 1	10; 10	10; 1
Su F2	X-axis position error, km	14, 6	25, 7	0, 008
	Y-axis position error, km	1, 3	0, 006	0, 0001
	Z-axis position error, km	23, 2	1, 5	0, 001
Savage F2	X-axis position error, km	1770, 1	56, 0	29, 5
	Y-axis position error, km	230, 2	0, 2	0, 4
	Z-axis position error, km	1750, 2	4, 2	6, 0

The results from FIGS. 3 to 6 and table 1 show that the proposed method has lower errors compared to other methods within the same differential equation-solving approach.

Claims

1. A coning, sculling and scrolling error compensation method for strapdown navigation system comprising the following steps: step 1: select a calculation function; in this step, a compensation for errors caused by motion affecting angle calculation errors is determined based on a required angle calculation accuracy and a computational load (time) achieved using theoretical error calculation charts;choice of calculation function, whether an approximation or exact function of the differential equation below, depends on a desired accuracy, a computational load, and a convenience of implementing the computation in embedded systems,