FUNCTIONAL ITERATIVE INTEGRATION-BASED METHOD AND SYSTEM FOR INERTIAL NAVIGATION SOLUTION

Abstract

A functional iterative integration-based method for an inertial navigation solution includes: fitting a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval; iteratively calculating Chebyshev polynomial coefficients of an attitude quaternion by using the obtained Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and performing polynomial truncation on a result obtained from each iterative calculation according to a preset order; iteratively calculating Chebyshev polynomial coefficients of a velocity/position by using the obtained Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and performing polynomial truncation on a result obtained from each iterative calculation according to a preset order; and obtaining attitude/velocity/position information on the corresponding time interval.

Claims

1. A functional iterative integration-based method for an inertial navigation solution, comprising: a fitting step: fitting a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval to obtain Chebyshev polynomial coefficients of the angular velocity and Chebyshev polynomial coefficients of the specific force; an attitude iterative solution step: iteratively calculating Chebyshev polynomial coefficients of an attitude quaternion by using the Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and performing polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the attitude quaternion according to a first preset order; a velocity/position iterative solution step: iteratively calculating Chebyshev polynomial coefficients of a velocity/position by using the Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and performing polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the velocity/position according to a second preset order; and an attitude/velocity/position acquisition step: obtaining attitude/velocity/position information on the time interval according to the Chebyshev polynomial coefficients of the velocity/position, the Chebyshev polynomial coefficients of the attitude quaternion and a Chebyshev polynomial corresponding to the velocity/position and the attitude quaternion.

2. The functional iterative integration-based method according to claim 1, wherein, the gyroscope-measured values comprise a measured value of the angular velocity or a measured value of an angular increment, and the accelerometer-measured values comprise a measured value of the specific force or a measured value of a velocity increment.

3. The functional iterative integration-based method according to claim 1, wherein, in the fitting step, the time interval is divided into at least two sub-time intervals to perform sequential calculations.

4. The functional iterative integration-based method according to claim 1, wherein, in the attitude iterative solution step or the velocity/position iterative solution step, the Chebyshev polynomial coefficients of the attitude quaternion or the Chebyshev polynomial coefficients of the velocity/position is iteratively calculated until a maximum number of iterative calculations is reached or a predetermined convergence condition is satisfied.

5. The functional iterative integration-based method according to claim 1, further comprising: a navigation step: performing a navigation according to the attitude/velocity/position information on the time interval.

6. A functional iterative integration-based system for an inertial navigation solution, comprising: a fitting module, wherein the fitting module is configured to fit a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval to obtain Chebyshev polynomial coefficients of the angular velocity and Chebyshev polynomial coefficients of the specific force; an attitude iterative solution module, wherein the attitude iterative solution module is configured to iteratively calculate Chebyshev polynomial coefficients of an attitude quaternion by using the Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and perform polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the attitude quaternion according to a first preset order; a velocity/position iterative solution module, wherein the velocity/position iterative solution module is configured to iteratively calculate Chebyshev polynomial coefficients of a velocity/position by using the Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and perform polynomial truncation on a result obtained from each iterative calculation of the Chebyshev polynomial coefficients of the velocity/position according to a second preset order; and an attitude/velocity/position acquisition module, wherein the attitude/velocity/position acquisition module is configured to obtain attitude/velocity/position information on the time interval according to the Chebyshev polynomial coefficients of the velocity/position, the Chebyshev polynomial coefficients of the attitude quaternion and a Chebyshev polynomial corresponding to the velocity/position and the attitude quaternion.

7. The functional iterative integration-based system according to claim 6, wherein, the gyroscope-measured values comprise a measured value of the angular velocity or a measured value of an angular increment, and the accelerometer-measured values comprise a measured value of the specific force or a measured value of a velocity increment.

8. The functional iterative integration-based system according to claim 6, wherein, the fitting module divides the time interval into at least two sub-time intervals to perform sequential calculations.

9. The functional iterative integration-based system according to claim 6, wherein, the attitude iterative solution module and the velocity/position iterative solution module perform the each iterative calculation until a maximum number of iterative calculations is reached or a predetermined convergence condition is satisfied.

10. The functional iterative integration-based system according to claim 6, further comprising: a navigation module, wherein the navigation module is configured to perform a navigation according to the attitude/velocity/position information on the time interval.

Description

BRIEF DESCRIPTION OF THE DRAWING

[0030] Other features, objectives and advantages of the present invention will be clearly described with reference to the detailed description of the non-restrictive embodiments and the drawing.

[0031] FIGURE is a solution flow chart of the present invention.

DETAILED DESCRIPTION OF THE EMBODIMENTS

[0032] The present invention will be described in detail below with reference to the specific embodiments. The following embodiments will help those skilled in the art to further understand the present invention, but should not be construed as limiting the present invention in any form. It should be noted that for those having ordinary skill in the art, several changes and improvements can be made without departing from the concept of the present invention. These changes and improvements shall fall within the scope of protection of the present invention.

[0033] The functional iterative integration-based method for inertial navigation solution provided by the present invention can select different calculation reference coordinate systems. In consideration of the computational burden, the present invention uses the earth coordinate system as the calculation reference coordinate system for illustration.

[0034] As shown in FIGURE, the present invention provides a function iterative integration-based method and system for inertial navigation solution. The method includes:

[0035] A fitting step: A Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force are fitted according to gyroscope-measured values and accelerometer-measured values on a time interval. The gyroscope-measured values include a measured value of the angular velocity or a measured value of an angular increment. The accelerometer-measured values include a measured value of the specific force (non-gravitational acceleration) or a measured value of a velocity increment.

[0036] An attitude iterative solution step: Chebyshev polynomial coefficients of an attitude quaternion is iteratively calculated by using Chebyshev polynomial coefficients of the angular velocity obtained in the fitting step and an integral equation of the attitude quaternion, and polynomial truncation is performed on a result obtained from each iterative calculation according to a preset order. The attitude iterative process continues until the maximum number of iterative calculations is reached or the predetermined convergence condition is satisfied.

[0037] A velocity/position iterative solution step: Chebyshev polynomial coefficients of a velocity/position is iteratively calculated by using the Chebyshev polynomial coefficients of the specific force obtained in the fitting step, the Chebyshev polynomial coefficients of the attitude quaternion obtained in the attitude iterative solution step and an integral equation of the velocity/position, and polynomial truncation is performed on a result obtained from each iterative calculation according to a preset order. The velocity/position iterative process continues until the maximum number of iterative calculations is reached or the predetermined convergence condition is satisfied.

[0038] An attitude/velocity/position acquisition step: Attitude/velocity/position information on the corresponding time interval is obtained according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

[0039] A Chebyshev polynomial of a first type is defined on an interval [1 1] and given by the following iterative relationship:

F.sub.0(x)=1,F.sub.1(x)=x,F.sub.i+1(x)=2F.sub.i(x)F.sub.i1(x); [0040] wherein F.sub.i(x) represents the i.sup.th order Chebyshev polynomial of the first type.

[0041] Fitting step: The Chebyshev polynomial function of the angular velocity and the Chebyshev polynomial function of the specific force are fitted according to the gyroscope-measured values and the accelerometer-measured values on the time interval.

[0042] For the measured value {tilde over ()}.sub.t.sub.k of the angular velocity or the measured value {tilde over ()}.sub.t.sub.k of the angular increment obtained by N gyroscopes at t.sub.k, and the measured value {tilde over (f)}.sub.t.sub.k of the specific force or the measured value {tilde over (v)}.sub.t.sub.k of the velocity increment obtained by N accelerometers, k=1, 2, . . . N, let

[00001]$t=\frac{t_N}{2}.Math.\left(1+\right),$

wherein represents a mapped time variable, and the original time interval is mapped to [1 1]. The angular velocity is approximately fitted by the following Chebyshev polynomials not exceeding N1 order:

[00002]$\begin{array}{cc}{\hat{}}^b=\underset{i=0}{\overset{n_{}}{.Math.}}.Math..Math.c_i.Math.F_i\left(\right),n_{}N-1;& \left(1\right)\end{array}$

[0043] the specific force is approximately fitted by the following Chebyshev polynomials not exceeding N1 order:

[00003]$\begin{array}{cc}{\hat{f}}^b=\underset{i=0}{\overset{n_f}{.Math.}}.Math.d_i.Math.F_i\left(\right),n_{f}N-1;& \left(2\right)\end{array}$

[0044] wherein n.sub. and n.sub.f represent the order of the Chebyshev polynomial of the angular velocity and the order of the Chebyshev polynomial of the specific force, respectively; c.sub.i and d.sub.i represent a coefficient vector of the i.sup.th order Chebyshev polynomial of the angular velocity and a coefficient vector of the i.sup.th order Chebyshev polynomial of the specific force, respectively; and r represents the mapped time variable.

[0045] For the angular velocity/specific force measurement, the coefficients c.sub.i and d.sub.i are determined by solving the following equations:

[00004]$\begin{array}{cc}{\left(n_{}\right)\left[\begin{array}{cccc}{c}_0& c_1& \mathrm{.Math.}& c_{n_{}}\end{array}\right]}^T={\left[\begin{array}{cccc}{\stackrel{~}{}}_{t_1}& {\stackrel{~}{}}_{t_2}& \mathrm{.Math.}& {\stackrel{~}{}}_{t_N}\end{array}\right]}^T.Math..Math.{\left(n_f\right)\left[\begin{array}{cccc}{d}_0& d_1& \mathrm{.Math.}& d_{n_f}\end{array}\right]}^T={\left[\begin{array}{cccc}{\tilde{f}}_{t_1}& {\tilde{f}}_{t_2}& \mathrm{.Math.}& {\tilde{f}}_{t_N}\end{array}\right]}^T;& \left(3\right)\end{array}$

[0046] for the angular increment/velocity increment measurement, the coefficients c.sub.i and d.sub.i are determined by solving the following equations:

[00005]$\begin{array}{cc}{\left(n_{}\right)\left[\begin{array}{cccc}{c}_0& c_1& \mathrm{.Math.}& c_{n_{}}\end{array}\right]}^T={\left[\begin{array}{cccc}.Math..Math.{\stackrel{~}{}}_{t_1}& .Math..Math.{\stackrel{~}{}}_{t_2}& \mathrm{.Math.}& .Math..Math.{\stackrel{~}{}}_{t_N}\end{array}\right]}^T.Math..Math.{\left(n_f\right)\left[\begin{array}{cccc}{d}_0& d_1& \mathrm{.Math.}& d_{n_f}\end{array}\right]}^T={\left[\begin{array}{cccc}.Math..Math.{\tilde{v}}_{t_1}& .Math..Math.{\tilde{v}}_{t_2}& \mathrm{.Math.}& .Math..Math.{\tilde{v}}_{t_N}\end{array}\right]}^T;& \left(3\right)\end{array}$

[0047] wherein the superscript T represents an operation of vector transpose or matrix transpose, and the matrices and are defined as follows:

[00006]$\begin{array}{cc}\left(n\right)=\left[\begin{array}{cccc}1& F_1\left({}_1\right)& \mathrm{.Math.}& F_n\left({}_1\right)\\ 1& F_1\left({}_2\right)& \mathrm{.Math.}& F_n\left({}_2\right)\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ 1& F_1\left({}_N\right)& \mathrm{.Math.}& F_n\left({}_N\right)\end{array}\right],& \left(5\right)\\ \left(n\right)=\left[\begin{array}{cccc}{G}_{0,\left[\begin{array}{cc}{}_0& {}_1\end{array}\right]}& G_{1,\left[\begin{array}{cc}{}_0& {}_1\end{array}\right]}& \mathrm{.Math.}& G_{n,\left[\begin{array}{cc}{}_0& {}_1\end{array}\right]}\\ G_{0,\left[\begin{array}{cc}{}_1& {}_2\end{array}\right]}& G_{1,\left[\begin{array}{cc}{}_1& {}_2\end{array}\right]}& \mathrm{.Math.}& G_{n,\left[\begin{array}{cc}{}_1& {}_2\end{array}\right]}\\ \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}& \mathrm{.Math.}\\ G_{0,\left[\begin{array}{cc}{}_{N-1}& {}_N\end{array}\right]}& G_{1,\left[\begin{array}{cc}{}_{N-1}& {}_N\end{array}\right]}& \mathrm{.Math.}& G_{n,\left[\begin{array}{cc}{}_{N-1}& {}_N\end{array}\right]}\end{array}\right];& \end{array}$

[0048] wherein function G.sub.i,[.sub.k1.sub..sub.k.sub.] is defined as follows:

[00007]$\begin{array}{cc}{G}_{i,\left[\begin{array}{cc}{}_{k-1}& {}_k\end{array}\right]}=\{\begin{array}{cc}\left(\frac{i.Math.F_{i+1}\left({}_k\right)}{i^2-1}-\frac{{}_k.Math.F_i\left({}_k\right)}{i-1}\right)-\left(\frac{i.Math.F_{i+1}\left({}_{k-1}\right)}{i^2-1}-\frac{{}_{k-1}.Math.F_i\left({}_{k-1}\right)}{i-1}\right),& i1\\ \frac{{}_k^2-{}_{k-1}^2}{2},& i=1\end{array}.& \left(6\right)\end{array}$

[0049] Attitude iterative solution step: The Chebyshev polynomial coefficients of the attitude quaternion is iteratively calculated by using the Chebyshev polynomial coefficients of the angular velocity obtained in the fitting step and an integral equation of the attitude quaternion, and polynomial truncation is performed on the result obtained from each iterative calculation according to a preset order.

[0050] Assuming that in the l.sup.th iterative calculation, the Chebyshev polynomial of the attitude quaternion is expressed as:

[00008]$\begin{array}{cc}{q}_l=\underset{i=0}{\overset{m_q}{.Math.}}.Math.b_{l,i}.Math.F_i\left(\right);& \left(7\right)\end{array}$

[0051] wherein m.sub.q represents the predetermined truncation order, and b.sub.l,i represents the coefficient of the i.sup.th Chebyshev polynomial in the l.sup.th iterative calculation. When l=0, q.sub.0(t)q(0), wherein q(0) represents the initial attitude quaternion. The Chebyshev polynomial coefficients of the attitude quaternion can be calculated iteratively as follows:

[00009]$\begin{array}{cc}{q}_{l+1}=q\left(0\right)+\frac{t_N}{8}.Math.\left(\underset{i=0}{\overset{m_q}{.Math.}}.Math.\underset{j=0}{\overset{n_{}}{.Math.}}.Math.b_{l,i}{c}_j\left(G_{i+j,\left[\begin{array}{cc}-1& \end{array}\right]}+G_{.Math.i-j.Math.,\left[\begin{array}{cc}-1& \end{array}\right]}\right)-2.Math.\underset{i=0}{\overset{m_q}{.Math.}}.Math.{}_e{b}_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}\right)=\underset{i=0}{\overset{m_q+m_{}+1}{.Math.}}.Math.b_{l+1,i}.Math.F_i\left(\right).Math.\stackrel{\mathrm{polynomial}.Math..Math.\mathrm{truncation}}{}.Math.\underset{i=0}{\overset{m_q}{.Math.}}.Math.b_{l+1,i}.Math.F_i\left(\right),& \left(8\right)\end{array}$

[0052] until the convergence condition is satisfied or the preset maximum number of iterative calculations is reached. The convergence condition is set according to the Euclidean distance deviation between the Chebyshev polynomial coefficients of two iterative calculations, i.e.,

[00010]${\left(\underset{i=0}{\overset{m_q}{.Math.}}.Math.{.Math.b_{l+1,i}-b_{l,i}.Math.}^2\right)}^{1/2}.$

The final result obtained from iterative calculations of the attitude is expressed as

[00011]$q=\underset{i=0}{\overset{m_q}{.Math.}}.Math.b_i.Math.F_i\left(\right),$

which is used as the input for the subsequent velocity and position calculation.

[0053] Velocity/position iterative solution step: The Chebyshev polynomial coefficients of the velocity/position is iteratively calculated by using the Chebyshev polynomial coefficients of the specific force obtained in the fitting step, the Chebyshev polynomial coefficients of the attitude quaternion obtained in the attitude iterative solution step and an integral equation of the velocity/position, and polynomial truncation is performed on a result obtained from each iterative calculation according to a preset order.

[0054] According to the Chebyshev polynomial of the specific force and the Chebyshev polynomial of the attitude quaternion, the integral of the specific force transformation is expressed as follows:

[00012]$\begin{array}{cc}{I}_f\left(\right)=\frac{1}{4}.Math.\underset{i=0}{\overset{m_q}{.Math.}}.Math.\underset{j=0}{\overset{n_f}{.Math.}}.Math.\underset{k=0}{\overset{m_q}{.Math.}}.Math.b_i{d}_j{b}_k^{*}\left(G_{i+j+k,\left[\begin{array}{cc}-1& \end{array}\right]}+G_{.Math.i+j-k.Math.,\left[\begin{array}{cc}-1& \end{array}\right]}+G_{.Math.i+j.Math.-k,\left[\begin{array}{cc}-1& \end{array}\right]}+G_{.Math..Math.i-j.Math.-k.Math.,\left[\begin{array}{cc}-1& \end{array}\right]}\right).& \left(9\right)\end{array}$

[0055] Assuming that in the l.sup.th iterative calculation, the Chebyshev polynomial of the velocity/position is expressed as follows:

[00013]$\begin{array}{cc}{v}_l^e=\underset{i=0}{\overset{m_v}{.Math.}}.Math.s_{l,i}.Math.F_i\left(\right),p_l^e=\underset{i=0}{\overset{m_p}{.Math.}}.Math.{}_{l,i}.Math.F_i\left(\right);& \left(10\right)\end{array}$

[0056] wherein m.sub.v and m.sub.p represent the predetermined truncation order of the velocity and the predetermined truncation order of the position, respectively; s.sub.l,i and .sub.l,i represent a coefficient of the i.sup.th Chebyshev polynomial of the velocity and a coefficient of the i.sup.th Chebyshev polynomial of the position in the l.sup.th iterative calculation, respectively. When l=0, v.sub.0.sup.e(t)v.sup.e(0), p.sub.0.sup.e(t)p.sup.e(0), wherein v.sup.e(0) and p.sup.e(0) represent an initial velocity and an initial position, respectively.

[0057] Considering that the gravity model is generally given in the geographic coordinate system and gravity is a position-correlated nonlinear function, the gravity vector in the earth coordinate system is approximated by the Chebyshev polynomials as follows:

[00014]$\begin{array}{cc}{g}^e\left(p_l^e\right)=C_n^e.Math.g^n\left(\mathrm{ecef}.Math..Math.2.Math.\mathrm{lla}\left(\underset{i=0}{\overset{m_p}{.Math.}}.Math.{}_{l,i}.Math.F_i\left(\right)\right)\right)\underset{i=0}{\overset{m_g}{.Math.}}.Math.{}_{l,i}.Math.F_i\left(\right);& \left(11\right)\end{array}$

[0058] wherein C.sub.n.sup.e represents an attitude matrix (also a position-correlated nonlinear function) of the earth coordinate system relative to the geographic coordinate system, m.sub.g represents the maximum order of the Chebyshev polynomial of gravity, and ecef2lla() represents a transformation from rectangular coordinates of the earth to geographic coordinates (longitude, latitude and altitude). The Chebyshev polynomial coefficient .sub.l,i of the gravity vector is calculated as follows:

[00015]$\begin{array}{cc}{}_{l,i}\frac{2-{}_{0.Math.i}}{P}.Math.\underset{k=0}{\overset{P-1}{.Math.}}.Math.\cos .Math.\frac{i\left(k+1/2\right).Math.}{P}.Math.C_n^e.Math.g^n\left(\mathrm{ecef}.Math..Math.2.Math.\mathrm{lla}\left(\underset{i=0}{\overset{m_p}{.Math.}}.Math.{}_{l,i}.Math.F_i\left(\cos .Math.\frac{\left(k+1/2\right).Math.}{P}\right)\right)\right);& \left(12\right)\end{array}$

[0059] wherein .sub.0i represents the Kronecker delta function (when i=1, output 1; otherwise, output 0). The accuracy of approximation of gravity increases as the number P of terms increases.

[0060] Formulas (9) and (12) are substituted to iteratively calculate the Chebyshev polynomial coefficients of the velocity/position as follows:

[00016]$\begin{array}{cc}\begin{array}{c}{v}_{l+1}^e=.Math.v^e\left(0\right)+\frac{t_N}{2}.Math.\left(I_f-2.Math.\underset{i=0}{\overset{m_v}{.Math.}}.Math.{}_e{s}_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}+\underset{i=0}{\overset{m_g}{.Math.}}.Math.{}_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}\right)\\ =.Math.\underset{i=0}{\overset{\mathrm{ma}.Math..Math.x.Math.\left\{2.Math.m_q+n_f,m_v,m_g\right\}+1}{.Math.}}.Math.s_{l+1,i}.Math.F_i\left(\right).Math.\stackrel{\mathrm{polynomial}.Math..Math.\mathrm{truncation}}{}.Math.\underset{i=0}{\overset{m_v}{.Math.}}.Math.s_{l+1,i}.Math.F_i\left(\right)\end{array}.Math..Math.\begin{array}{c}.Math.p_{l+1}^e=.Math.p^e\left(0\right)+\frac{t_N}{2}.Math.\underset{i=0}{\overset{m_v}{.Math.}}.Math.s_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}\\ =.Math.\underset{i=0}{\overset{m_v+1}{.Math.}}.Math.{}_{l+1,i}.Math.F_i\left(\right).Math.\stackrel{\mathrm{polynomial}.Math..Math.\mathrm{truncation}}{}.Math.\underset{i=0}{\overset{m_p}{.Math.}}.Math.{}_{l+1,i}.Math.F_i\left(\right),\end{array}& \left(13\right)\end{array}$

[0061] until the convergence condition is satisfied or the preset maximum number of iterative calculations is reached.

[0062] The sequence of the velocity and the position in formula (13) can be reversed, and the velocity (position) result obtained from the same iterative calculation can be directly used in the subsequent position (velocity) calculation. For example, formula (13) can be calculated in the following sequence:

[00017]$\begin{array}{cc}\begin{array}{c}.Math.p_{l+1}^e=.Math.p^e\left(0\right)+\frac{t_N}{2}.Math.\underset{i=0}{\overset{m_v}{.Math.}}.Math.s_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}\\ =.Math.\underset{i=0}{\overset{m_v+1}{.Math.}}.Math.{}_{l+1,i}.Math.F_i\left(\right).Math.\stackrel{\mathrm{polynomial}.Math..Math.\mathrm{truncation}}{}.Math.\underset{i=0}{\overset{m_p}{.Math.}}.Math.{}_{l+1,i}.Math.F_i\left(\right),\end{array}.Math..Math..Math.\mathrm{and}& \left(14\right)\\ \begin{array}{c}{v}_{l+1}^e=.Math.v^e\left(0\right)+\frac{t_N}{2}.Math.\left(I_f-2.Math.\underset{i=0}{\overset{m_v}{.Math.}}.Math.{}_e{s}_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}+\underset{i=0}{\overset{m_g}{.Math.}}.Math.{}_{l,i}.Math.G_{i,\left[\begin{array}{cc}-1& \end{array}\right]}\right)\\ =.Math.\underset{i=0}{\overset{\mathrm{ma}.Math..Math.x.Math.\left\{2.Math.m_q+n_f,m_v,m_g\right\}+1}{.Math.}}.Math.s_{l+1,i}.Math.F_i\left(\right).Math.\stackrel{\mathrm{polynomial}.Math..Math.\mathrm{truncation}}{}.Math.\underset{i=0}{\overset{m_v}{.Math.}}.Math.s_{l+1,i}.Math.F_i\left(\right),\end{array}& \left(15\right)\end{array}$

[0063] It should be noted that the gravity vector of formula (15) approximately uses position p.sub.l+1.sup.e (corresponding to .sub.l+1,i) calculated by formula (14) instead of p.sub.l.sup.e in the previous iterative calculation.

[0064] Attitude/velocity/position acquisition step: The attitude/velocity/position information on the corresponding time interval is determined according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

[0065] With respect to the inertial navigation solution on a long time interval, the time interval can be divided into a plurality of sub-time intervals to perform sequential calculations.

[0066] Navigation step: Navigation is performed according to the attitude/velocity/position information on the corresponding time interval.

[0067] According to the above-mentioned functional iterative integration-based method for inertial navigation solution, the present invention further provides a functional iterative integration-based system for inertial navigation solution, which includes:

[0068] a fitting module, configured to fit a Chebyshev polynomial function of an angular velocity and a Chebyshev polynomial function of a specific force according to gyroscope-measured values and accelerometer-measured values on a time interval;

[0069] an attitude iterative solution module, configured to iteratively calculate Chebyshev polynomial coefficients of an attitude quaternion by using the obtained Chebyshev polynomial coefficients of the angular velocity and an integral equation of the attitude quaternion, and perform polynomial truncation on a result obtained from each iterative calculation according to a preset order;

[0070] a velocity/position iterative solution module, configured to iteratively calculate Chebyshev polynomial coefficients of a velocity/position by using the obtained Chebyshev polynomial coefficients of the specific force, the Chebyshev polynomial coefficients of the attitude quaternion and an integral equation of the velocity/position, and perform polynomial truncation on a result obtained from each iterative calculation according to a preset order; and

[0071] an attitude/velocity/position acquisition module, configured to obtain attitude/velocity/position information on the corresponding time interval according to the obtained Chebyshev polynomial coefficients of the attitude/velocity/position and the corresponding Chebyshev polynomial.

[0072] Those skilled in the art shall understand that, in addition to implementing the system provided by the present invention and its various devices, modules, and units in a pure computer-readable program code manner, they may also be implemented to realize the same functions in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, embedded microcontrollers, and the like, by performing logic programming on the steps of the method. Therefore, the system provided by the present invention and its various devices, modules and units can be regarded as hardware components, and the devices, modules, and units included in the system for implementing various functions can also be regarded as structures within the hardware components. Besides, the devices, modules, and units for realizing various functions can also be regarded as not only software modules but also the structures within the hardware components for implementing the method.

[0073] The specific embodiments of the present invention have been described above. It should be understood that the present invention is not limited to the foregoing specific embodiments. Those skilled in the art can make various changes or modifications within the scope of the claims without affecting the essence of the present invention. When not in conflict, the embodiments of the present invention and the features in the embodiments can be combined with each other arbitrarily.