Method for updating strapdown inertial navigation solutions based on launch-centered earth-fixed frame

Abstract

The present invention relates to a method for updating strapdown inertial navigation solutions based on a launch-centered earth-fixed (LCEF) frame (g frame). The present invention uses the g frame as a navigation reference frame of a medium-to-short-range surface-to-surface missile. This is beneficial to establish a relative relationship between the missile and the ground so as to keep the same missile parameters required by a missile control and guidance system. The calculation of a navigation algorithm in the g frame is moderate, which is suitable for an embedded system.

Claims

1. A method for updating strapdown inertial navigation solutions using a launch-centered earth-fixed (LCEF) frame (g frame), comprising the following steps: step 1: using a surface-to-surface missile (SSM) as a body to establish a relative relationship between the SSM and a ground to keep identical missile parameters required by a missile control and guidance system, and establishing a body frame (b frame), which points front, top, and right; step 2: expressing a navigation equation in the g frame as: $\left[\begin{array}{c}{\stackrel{.}{P}}^g\\ {\stackrel{.}{V}}^g\\ {\stackrel{.}{R}}_b^g\end{array}\right]=\left[\begin{array}{c}{V}^g\\ R_b^g{f}^b-2{\Omega }_{\mathrm{ag}}^g{V}^g+g^g\\ R_b^g\left({\Omega }_{\mathrm{ab}}^b-{\Omega }_{\mathrm{ag}}^b\right)\end{array}\right]$ wherein, P.sup.g, V.sup.g and R.sub.b.sup.g are position, velocity and attitude matrices of the body in the g frame, and corresponding equations are position, a velocity navigation equation and an attitude navigation equation; f.sup.b is a measured value of an accelerometer triad; g.sup.g is a gravity of the body in the g frame; Ω.sub.ab.sup.b is an anti-symmetric matrix corresponding to a measured value ω.sub.ab.sup.b of a gyroscope triad; Ω.sub.ag.sup.b is an anti-symmetric matrix corresponding to a rotational angular velocity ω.sub.ab.sup.b of the g frame relative to an a frame; step 3: performing attitude update on the SSM, comprising the following sub-steps: step 3-1: resolving the attitude navigation equation in step 2 by a quaternion method:
q.sub.b(m).sup.g(m)=q.sub.g(m-1).sup.g(m)q.sub.b(m-1).sup.g(m-1)q.sub.b(m).sup.g(m-1) where, q.sub.b(m-1).sup.g(m-1) is a transformation quaternion from the b frame to the g frame at a t.sub.m-1 moment, that is, an attitude quaternion at t.sub.m-1; q.sub.b(m).sup.g(m) is an attitude quaternion at t.sub.m; q.sub.g(m-1).sup.g(m) is a transformation quaternion of the g frame from t.sub.m-1 to t.sub.m; q.sub.b(m).sup.b(m-1) is a transformation quaternion calculated from an angular increment from t.sub.m-1 to t.sub.m; step 3-2: calculating with an equivalent rotation vector method, wherein an equivalent rotation vector ζ.sub.m of the g frame from t.sub.m-1 to t.sub.m is expressed as: ${ϛ}_m={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ag}}^g\left(t\right)\mathrm{dt}\approx {\omega }_{\mathrm{ag}}^{g}T$ an equivalent rotation vector Φ.sub.m of the body frame relative to an inertial frame from t.sub.m-1 to t.sub.m is expressed as: ${\Phi }_m={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ab}}^b\left(t\right)dt$ step 3-3: applying the angular increment measured by the gyroscope triad to an actual project to calculate: $\begin{array}{cc}{\Phi }_m=\Delta {\theta }_1+\Delta {\theta }_2+\frac{2}{3}\left(\Delta {\theta }_1\times \Delta {\theta }_2\right)& \left(0.1\right)\end{array}$ wherein $\Delta {\theta }_1={\int }_{t_{m-1}}^{t_{m-1}+\frac{T}{2}}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau ,\Delta {\theta }_2={\int }_{t_{m-1}+\frac{T}{2}}^{t_m}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau ;$ a rotation vector Φ and a corresponding quaternion q(h) have the following calculation relationship: $\begin{array}{cc}q\left(h\right)=\cos \frac{\Phi }{2}+\frac{\Phi }{\Phi }\sin \frac{\Phi }{2}& \left(0.2\right)\end{array}$ therefore, quaternions q.sub.g(m-1).sup.g(m) and q.sub.b(m)(.sup.b(m-1) corresponding to ζ.sub.m and Φ.sub.m are obtained, and substituted into q.sub.b(m).sup.g(m)=q.sub.g(m-1).sup.g(m)q.sub.b(m-1).sup.g(m-1)q.sub.b(m).sup.b(m-1) to complet attitude update; step 4: performing velocity update on the SSM, comprising the following sub-steps: step 4-1: integrating the velocity navigation equation in step 2 over a period [t.sub.m-1,t.sub.m]:
V.sub.m.sup.g=V.sub.m-1.sup.g+ΔV.sub.sf(m).sup.g+ΔV.sub.cor/g(m).sup.g wherein, V.sub.m.sup.g and V.sub.m-1.sup.g are velocities of the SSM in the g frame at t.sub.m and t.sub.m-1; ΔV.sub.sf(m).sup.g and ΔV.sub.cor/g(m).sup.g are a specific force velocity increment and a velocity increment of a harmful acceleration of a navigation coordinate system in the period T=t.sub.m−t.sub.m-1; Step 4-2: substituting ΔV.sub.cor/g(m).sup.g with a value at t.sub.m-1/2=(t.sub.m-1+t.sub.m)/2:
ΔV.sub.cor/g(m).sup.g≈[−2ω.sub.ag(m-1/2).sup.g×V.sub.m-1/2.sup.g+g.sub.m-1/2.sup.g]T; performing matrix chain multiplication and decomposition on ΔV.sub.sf(m).sup.g:
ΔV.sub.sf(m).sup.g=R.sub.b(m-1).sup.g(m-1)ΔV+R.sub.b(m-1).sup.g(m-1)(ΔV.sub.rot(m)+ΔV.sub.scul(m))−(ΔV′.sub.rot(m)+ΔV′.sub.scul(m)) wherein, R.sub.b(m-1).sup.g(m-1) is an attitude matrix at t.sub.m-1; ΔV is an integral of a measured value of the accelerometer triad in T; ΔV.sub.rot(m) and ΔV′.sub.rot(m) are rotation error compensation quantities of velocity; ΔV.sub.scul(m) and ΔV′.sub.scul(m) are scull error compensation quantities; step 4-3: expressing ΔV.sub.sf(m).sup.g by an angular velocity increment and a velocity increment at t.sub.m-1/2 and t.sub.m, and setting: $\{\begin{array}{c}\Delta {\theta }_1={\int }_{t_{m-1}}^{t_{m-1}+T/2}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau \\ \Delta {\theta }_2={\int }_{t_{m-1}+T/2}^{t_m}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau \\ \Delta V_1={\int }_{t_{m-1}}^{t_{m-1}+T/2}{f}^b\left(\tau \right)d\tau \\ \Delta V_2={\int }_{t_{m-1}+T/2}^{t_m}{f}^b\left(\tau \right)d\tau \\ \Delta \theta ={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau \\ \Delta V={\int }_{t_{m-1}}^{t_m}{f}^b\left(\tau \right)d\tau \end{array}\{\begin{array}{c}{\mathrm{\Delta \theta }}_1^{\prime }={\int }_{t_{m-1}}^{t_{m-1}+T/2}{\omega }_{\mathrm{ag}}^g\left(\tau \right)d\tau \approx \frac{T}{2}{\omega }_{\mathrm{ag}}^g\\ {\mathrm{\Delta \theta }}_2^{\prime }={\int }_{t_{m-1}+T/2}^{t_m}{\omega }_{\mathrm{ag}}^g\left(\tau \right)d\tau \approx \frac{T}{2}{\omega }_{\mathrm{ag}}^g\\ \Delta V_1^{\prime }={\int }_{t_{m-1}}^{t_{m-1}+T/2}{R}_{b\left(m-1\right)}^{g\left(m-1\right)}{f}^b\left(\tau \right)d\tau =R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V_1\\ \Delta V_2^{\prime }={\int }_{t_{m-1}+T/2}^{t_m}{R}_{b\left(m-1\right)}^{g\left(m-1\right)}{f}^b\left(\tau \right)d\tau =R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V_2\\ {\mathrm{\Delta \theta }}^{\prime }={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ag}}^g\left(\tau \right)d\tau ={\mathrm{\Delta \theta }}_1^{\prime }+{\mathrm{\Delta \theta }}_2^{\prime }\\ \Delta V^{\prime }={\int }_{t_{m-1}}^{t_m}{R}_{b\left(m-1\right)}^{g\left(m-1\right)}{f}^b\left(\tau \right)d\tau =R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V\end{array}$ wherein, a two-subsample velocity rotation error compensation algorithm is as follows: $\Delta V_{\mathrm{rot}\left(m\right)}=\frac{1}{2}\Delta \theta \times \Delta V$ $\Delta V_{\mathrm{rot}\left(m\right)}^{\prime }=\frac{1}{2}A{\theta }^{\prime }\times \Delta V^{\prime }$ a two-subsample velocity scull error compensation algorithm is as follows: $\begin{array}{cc}\Delta V_{\mathrm{scul}\left(m\right)}=\frac{2}{3}\left(\Delta {\theta }_1\times \Delta V_2+\Delta V_1\times \Delta {\theta }_2\right)& \end{array}$ $\begin{array}{cc}\Delta V_{\mathrm{scul}\left(m\right)}^{\prime }=\frac{2}{3}\left(\Delta {\theta }_1^{\prime }\times \Delta V_2^{\prime }+\Delta V_1^{\prime }\times \Delta {\theta }_2^{\prime }\right)& \end{array}$ $\begin{array}{cc}\mathrm{and}& \end{array}$ $\begin{array}{cc}\Delta V_{\mathrm{rot}\left(m\right)}^{\prime }+\Delta V_{\mathrm{scul}\left(m\right)}^{\prime }\approx \frac{T}{6}{\omega }_{\mathrm{ag}}^g\times \left[R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(\Delta V_1+5\Delta V_2\right)\right]& \end{array}$ wherein, a complete algorithm of the specific force velocity increment in the g frame is as follows: $\Delta V_{\mathrm{sf}\left(m\right)}^g=R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V-\frac{T}{6}{\omega }_{\mathrm{ag}}^g\times \left[R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(\Delta V_1+5\Delta V_2\right)\right]+R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(\Delta V_{\mathrm{rot}\left(m\right)}+\Delta V_{\mathrm{scul}\left(m\right)}\right)$ step 5: performing position update on the SSM, comprising the following sub-steps: step 5-1: obtaining the following equation from the velocity navigation equation in step 2: wherein $V^g\left(t\right)=V_{m-1}^g+\Delta V_{\mathrm{sf}}^g\left(t\right)+\Delta V_{\mathrm{cor}/g}^g\frac{t-t_{m-1}}{T},\mathrm{and}{t}_{m-1}\le t\le t_m;$ step 5-2: integrating both sides of V.sup.g(t) in step 5-1 in [t.sub.m-1, t.sub.m] to obtain: $P_m^g=P_{m-1}^g+\left[V_{m-1}^g+\frac{1}{2}\Delta V_{\mathrm{cor}/g\left(m\right)}^g\right]T+\Delta P_{\mathrm{sf}\left(m\right)}^g$ wherein, P.sub.m.sup.g and P.sub.m-1.sup.g are positions of the SSM in the g frame at t.sub.m and t.sub.m-1;
ΔP.sub.sf(m).sup.g=R.sub.b(m-1).sup.g(m-1)(S.sub.Δvm+ΔP.sub.rotm+ΔP.sub.scrlm)−(ΔP′.sub.rotm+ΔP′.sub.scrlm) $S_{\Delta \mathrm{vm}}=T\left[\frac{5}{6}\Delta V_1+\frac{1}{6}\Delta V_2\right]$ indicates a secondary integral of a specific force; $\Delta P_{\mathrm{rotm}}=T\left(\Delta {\theta }_1\times \left[\frac{5}{18}\Delta V_1+\frac{1}{6}\Delta V_2\right]+\Delta {\theta }_2\times \left[\frac{1}{6}\Delta V_1+\frac{1}{18}\Delta V_2\right]\right)$ $\Delta P_{\mathrm{rotm}}^{\prime }=T\left(\Delta {\theta }_1^{\prime }\times \left[\frac{5}{18}\Delta V_1^{\prime }+\frac{1}{6}\Delta V_2^{\prime }\right]+\Delta {\theta }_2^{\prime }\times \left[\frac{1}{6}\Delta V_1^{\prime }+\frac{1}{18}\Delta V_2^{\prime }\right]\right)$ indicates a rotation effect compensation quantity in position calculation; $\Delta P_{\mathrm{scrlm}}=T\left({\mathrm{\Delta \theta }}_1\times \left[\frac{11}{90}\Delta V_1+\frac{1}{10}\Delta V_2\right]+\Delta {\theta }_2\times \left[\frac{1}{90}\Delta V_2-\frac{7}{30}\Delta V_1\right]\right)$ $\Delta P_{\mathrm{scrlm}}^{\prime }=T\left(\Delta {\theta }_1^{\prime }\times \left[\frac{11}{90}\Delta V_1^{\prime }+\frac{1}{10}\Delta V_2^{\prime }\right]+\Delta {\theta }_2^{\prime }\times \left[\frac{1}{90}\Delta V_2^{\prime }-\frac{7}{30}\Delta V_1^{\prime }\right]\right)$ indicates a scroll effect compensation quantity in position calculation; step 5-3: finally obtaining a complete algorithm of the ΔP.sub.sf(m).sup.g position increment in the g frame: $\Delta P_{\mathrm{sf}\left(m\right)}^g=R_{b(m\text{-1)}}^{g(m\text{-1)}}\left(S_{\Delta \mathrm{vm}}+\Delta P_{\mathrm{rotm}}+\Delta P_{\mathrm{scrlm}}\right)-\frac{T^2}{6}{\omega }_{\mathrm{ag}}^g\times \left(R_{b\left(m-1\right)}^{g\left(m-1\right)}\left[\Delta V_1+\Delta V_2\right]\right)$ according to the result in step 5-2 and $\Delta P_{\mathrm{rotm}}^{\prime }+\Delta P_{\mathrm{scrlm}}^{\prime }\approx \frac{T^2}{6}{\omega }_{\mathrm{ag}}^g\times \left(R_{b\left(m-1\right)}^{g\left(m-1\right)}\left[\Delta V_1+\Delta V_2\right]\right)$ by utilizing the angular increment, velocity increment, and linear and angular vibrations of the SSM in flight, the strapdown inertial navigation solutions are updated in terms of attitude, velocity, and position in the g frame, thereby reducing data errors between the SSM and missile guidance and control systems in order to increase the SSM's hit rate.

Description

BRIEF DESCRIPTION OF DRAWINGS

(1) FIG. 1: Mechanization of strapdown inertial navigation in an LCEF frame.

DETAILED DESCRIPTION

(2) Referring to FIG. 1, coordinate systems in the present invention are defined as follows:

(3) 1) Earth-centered inertial (ECI) frame (i frame).

(4) 2) Earth-centered earth-fixed (ECEF) frame (e frame).

(5) 3) Body frame (b frame), which is a forward-up-right body frame. A surface-to-surface missile (SSM) serves as a body in the present invention.

(6) 4) Launch-centered earth-fixed (LCEF) frame (g frame). An origin of coordinates o is at a launch point; ox is in a horizontal plane of the launch point, and points to a launch aiming direction; oy is along a vertical direction of the launch point; oxyz is a right-handed coordinate system. The g frame is fixed on the earth and rotates with the earth.

(7) 5) Launch-centered inertial (LCI) frame (a frame), which coincides with the g frame at the moment of launching, and then remains unchanged in an inertial space.

(8) The method for updating strapdown inertial navigation solutions of the SSM based on the g frame includes attitude update, velocity update and position update.

(9) Step 1: Calculate a corresponding rotation vector according to a gyroscope triad angular increment of the SSM in an update period T, a rotational angular velocity ω.sub.ag.sup.g of the g frame (g frame) relative to the LCI frame (a frame) and the update period T; calculate a corresponding quaternion according to a relationship between the rotation vector and the quaternion; finally, complete attitude update by an quaternion recursion equation.

(10) Step 2: Calculate a velocity increment of a harmful acceleration according to a velocity V.sub.m-1.sup.g of the SSM at a previous moment, the rotational angular velocity ω.sub.ag.sup.g of the g frame relative to the a frame and a gravity g.sup.g of the SSM; calculate a specific force velocity increment according to the gyroscope triad angular increment and an accelerometer triad velocity increment of the SSM in the update period; add to the velocity V.sub.m-1.sup.g of the previous moment to complete velocity update.

(11) Step 3: Calculate an integral of the velocity V.sub.m-1.sup.g of the previous moment and the velocity increment of the harmful acceleration obtained in step 2 with T; calculate an integral of the specific force velocity increment based on the angular increment and velocity increment in the update period; finally, sum the integral and a position P.sub.m-1.sup.g of the missile at the previous moment to complete position update.

(12) The algorithm of the present invention is described in detail below.

(13) A navigation equation in the g frame is expressed as:

(14) $\begin{array}{cc}\left[\begin{array}{c}{\stackrel{.}{P}}^g\\ {\stackrel{.}{V}}^g\\ {\stackrel{.}{R}}_b^g\end{array}\right]=\left[\begin{array}{c}{V}^g\\ R_b^g{f}^b-2{\Omega }_{\mathrm{ag}}^g{V}^g+g^g\\ R_b^g\left({\Omega }_{\mathrm{ab}}^b-{\Omega }_{\mathrm{ag}}^b\right)\end{array}\right]& \left(1.1\right)\end{array}$

(15) In the equation, P.sup.g, V.sup.g and R.sub.b.sup.g are position, velocity and attitude matrices of the SSM in the g frame, and the corresponding equations are position, velocity and attitude navigation equations; f.sup.b is a measured value of an accelerometer triad; g.sup.g is a gravity of the SSM in the g frame; ω.sub.ab.sup.b is an anti-symmetric matrix corresponding to a measured value ω.sub.ab.sup.b of a gyroscope triad; and ω.sub.ag.sup.b is an anti-symmetric matrix corresponding to the rotational angular velocity ω.sub.ag.sup.b of the g frame relative to the a frame.

(16) (1) Attitude Update

(17) The attitude navigation equation is resolved using a quaternion method; a numerical recursion of an attitude update quaternion equation is performed:
q.sub.b(m).sup.g(m)=q.sub.g(m-1).sup.g(m)q.sub.b(m-1).sup.g(m-1)q.sub.b(m).sup.b(m-1)  (1.2)

(18) In the equation, q.sub.b(m-1).sup.g(m-1) is a transformation quaternion from the b frame to the g frame at a t.sub.m-1 moment, that is, an attitude quaternion at t.sub.m-1; q.sub.b(m).sup.g(m) is an attitude quaternion at a t.sub.m moment; q.sub.g(m-1).sup.g(m) is a transformation quaternion of the g frame from t.sub.m-1 to t.sub.m; q.sub.b(m).sup.b(m-1) is a transformation quaternion calculated from the angular increment from t.sub.m-1 to t.sub.m.

(19) Because there is a non-commutative error of rotation when the rigid body makes limited rotation, an equivalent rotation vector method is used. When the angular increment is used to calculate an equivalent vector, this non-commutative error is appropriately compensated, and the rotation vector algorithm is used to obtain an optimized algorithm in a cone motion environment. The calculation method is as follows:

(20) (1) The calculation of q.sub.g(m-1).sup.g(m) uses the rotational angular velocity ω.sub.ag.sup.g(t) (t.sub.m-1≤t≤t.sub.m); t.sub.m−t.sub.m-1=T, and the equivalent rotation vector of the g frame from t.sub.m-1 to t.sub.m is ζ.sub.m:

(21) $\begin{array}{cc}{ϛ}_m={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ag}}^g\left(t\right)\mathrm{dt}\approx {\omega }_{\mathrm{ag}}^{g}T& \left(1.3\right)\end{array}$

(22) (2) The calculation of q.sub.b(m).sup.b(m-1) uses the rotational angular velocity ω.sub.ab.sup.b(t), which is the angular velocity measured by the gyroscope triad; the equivalent rotation vector of the body frame relative to the inertial frame from the t.sub.m-1 moment to t.sub.m is Φ.sub.m:

(23) 0$\begin{array}{cc}{\Phi }_m={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ab}}^b\left(t\right)dt& \left(1.4\right)\end{array}$

(24) In an actual project, the angular increment measured by the gyroscope triad is used to calculate:
Φ.sub.m=Δθ.sub.1+Δθ.sub.2+⅔(Δθ.sub.1×Δθ.sub.2)  (1.5)

(25) In the equation,

(26) $\Delta {\theta }_1={\int }_{t_{m-1}}^{t_{m-1}+\frac{T}{2}}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau ,\Delta {\theta }_2={\int }_{t_{m-1}+\frac{T}{2}}^{t_m}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau .$
The rotation vector Φ and the corresponding quaternion q(h) have the following calculation relationship:

(27) $\begin{array}{cc}q\left(h\right)=\cos \frac{\Phi }{2}+\frac{\Phi }{\Phi }\sin \frac{\Phi }{2}& \left(1.6\right)\end{array}$

(28) The quaternions q.sub.g(m-1).sup.g(m) and q.sub.b(m).sup.b(m-1) corresponding to ζ.sub.m and Φ.sub.m are calculated respectively, and are substituted into (1.2) to complete attitude update.

(29) (2) Velocity Update

(30) The velocity navigation equation is integrated over a period [t.sub.m-1, t.sub.m] to obtain the following recursive form
V.sub.m.sup.g=V.sub.m-1.sup.g+ΔV.sub.sf(m).sup.g+ΔV.sub.cor/g(m).sup.g  (1.7)

(31) In the equation, V.sub.m.sup.g and V.sub.m-1.sup.g are the velocity of the SSM in the g frame at t.sub.m and t.sub.m-1, and

(32) $\begin{array}{cc}\Delta V_{\mathrm{sf}\left(m\right)}^g={\int }_{t_{m-1}}^{t_m}{R}_b^g\left(t\right)f^b\left(t\right)\mathrm{dt}& \left(1.8\right)\\ \Delta V_{\mathrm{cor}/g\left(m\right)}^g={\int }_{t_{m-1}}^{t_m}\left[-2{\omega }_{\mathrm{ag}}^g\left(t\right)\times V^g\left(t\right)+g^g\left(t\right)\right]\mathrm{dt}& \left(1.9\right)\end{array}$

(33) ΔV.sub.sf(m).sup.g and ΔV.sub.cor/g(m).sup.g are respectively the specific force velocity increment and the velocity increment of the harmful acceleration of the navigation coordinate system in period T=t.sub.m−t.sub.m-1.

(34) The integrand of ΔV.sub.cor/g(m).sup.g is a slow time-varying variable, and the value at t.sub.m-1/2=(t.sub.m-1−t.sub.m) is used to approximate equation (1.9) as:
ΔV.sub.cor/g(m).sup.g=[−2ω.sub.ag(m-1/2).sup.g×V.sub.m-1/2.sup.g+g.sub.g(m-1/2).sup.g]T  (1.10)

(35) The specific force velocity increment ΔV.sub.sf(m).sup.g is calculated by matrix chain multiplication and decomposition and approximate expansion:
ΔV.sub.sf(m).sup.g=R.sub.b(m-1).sup.g(m-1)ΔV+R.sub.b(m-1).sup.g(m-1)(ΔV.sub.rot(m)+ΔV.sub.scul(m))−(ΔV′.sub.rot(m)+ΔV′.sub.scul(m))  (1.11)

(36) R.sub.b(m-1).sup.g(m-1) is the attitude matrix at t.sub.m-1; ΔV is the integral of the measured value of the accelerometer triad in T; and ΔV.sub.rot(m) and ΔV′.sub.rot(m) are rotation error compensation quantities of velocity, which are caused due to the spatial rotation change of a specific force direction during the resolving period; ΔV.sub.scul(m) and ΔV′.sub.scul(m) are scull error compensation quantities.

(37) ΔV.sub.scul(m) is expressed by the angular velocity increment and the velocity increment at t.sub.m-1/2 and t.sub.m:

(38) $\{\begin{array}{c}\Delta {\theta }_1={\int }_{t_{m-1}}^{t_{m-1}+T/2}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau \\ \Delta {\theta }_2={\int }_{t_{m-1}+T/2}^{t_m}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau \\ \Delta V_1={\int }_{t_{m-1}}^{t_{m-1}+T/2}{f}^b\left(\tau \right)d\tau \\ \Delta V_2={\int }_{t_{m-1}+T/2}^{t_m}{f}^b\left(\tau \right)d\tau \\ \Delta \theta ={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ab}}^b\left(\tau \right)d\tau \\ \Delta V={\int }_{t_{m-1}}^{t_m}{f}^b\left(\tau \right)d\tau \end{array}\{\begin{array}{c}{\mathrm{\Delta \theta }}_1^{\prime }={\int }_{t_{m-1}}^{t_{m-1}+T/2}{\omega }_{\mathrm{ag}}^g\left(\tau \right)d\tau \approx \frac{T}{2}{\omega }_{\mathrm{ag}}^g\\ {\mathrm{\Delta \theta }}_2^{\prime }={\int }_{t_{m-1}+T/2}^{t_m}{\omega }_{\mathrm{ag}}^g\left(\tau \right)d\tau \approx \frac{T}{2}{\omega }_{\mathrm{ag}}^g\\ \Delta V_1^{\prime }={\int }_{t_{m-1}}^{t_{m-1}+T/2}{R}_{b\left(m-1\right)}^{g\left(m-1\right)}{f}^b\left(\tau \right)d\tau =R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V_1\\ \Delta V_2^{\prime }={\int }_{t_{m-1}+T/2}^{t_m}{R}_{b\left(m-1\right)}^{g\left(m-1\right)}{f}^b\left(\tau \right)d\tau =R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V_2\\ {\mathrm{\Delta \theta }}^{\prime }={\int }_{t_{m-1}}^{t_m}{\omega }_{\mathrm{ag}}^g\left(\tau \right)d\tau ={\mathrm{\Delta \theta }}_1^{\prime }+{\mathrm{\Delta \theta }}_2^{\prime }\\ \Delta V^{\prime }={\int }_{t_{m-1}}^{t_m}{R}_{b\left(m-1\right)}^{g\left(m-1\right)}{f}^b\left(\tau \right)d\tau =R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V\end{array}$

(39) A two-subsample velocity rotation error compensation algorithm is:

(40) $\begin{array}{cc}\Delta V_{\mathrm{rot}\left(m\right)}=\frac{1}{2}\Delta \theta \times \mathrm{\Delta V}& \left(1.12\right)\\ \Delta V_{\mathrm{rot}\left(m\right)}^{\prime }=\frac{1}{2}A{\theta }^{\prime }\times \Delta V^{\prime }& \left(1.13\right)\end{array}$

(41) A two-subsample scull error compensation algorithm is:

(42) $\begin{array}{cc}\Delta V_{\mathrm{scul}\left(m\right)}=\frac{2}{3}\left(\Delta {\theta }_1\times \Delta V_2+\Delta V_1\times \Delta {\theta }_2\right)& \left(1.14\right)\end{array}$$\begin{array}{cc}\Delta V_{\mathrm{scul}\left(m\right)}^{\prime }=\frac{2}{3}\left(\Delta {\theta }_1^{\prime }\times \Delta V_2^{\prime }+\Delta V_1^{\prime }\times \Delta {\theta }_2^{\prime }\right)& \left(1.15\right)\end{array}$$\begin{array}{cc}\mathrm{and}& \end{array}$$\begin{array}{cc}\Delta V_{\mathrm{rot}\left(m\right)}^{\prime }+\Delta V_{\mathrm{scul}\left(m\right)}^{\prime }\approx \frac{T}{6}{\omega }_{\mathrm{ag}}^g\times \left[R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(\Delta V_1+5\Delta V_2\right)\right]& \left(1.16\right)\end{array}$

(43) Thus, the complete algorithm of the specific force velocity increment in the g frame is obtained, that is, Equation (1.11) can be expressed as:

(44) $\begin{array}{cc}\Delta V_{\mathrm{sf}\left(m\right)}^g=R_{b\left(m-1\right)}^{g\left(m-1\right)}\Delta V-\frac{T}{6}{\omega }_{\mathrm{ag}}^g\times \left[R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(\Delta V_1+5\Delta V_2\right)\right]+R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(\Delta V_{\mathrm{rot}\left(m\right)}+\Delta V_{\mathrm{scul}\left(m\right)}\right)& \left(1.17\right)\end{array}$

(45) (3) Position Update

(46) According to the position navigation equation, the following equation is obtained:
{dot over (P)}.sup.g(t)=V.sup.g(t)  (1.10)

(47) Due to the short time period [t.sub.m-1,t.sub.m], the velocity compensation term of the harmful acceleration changes very slowly during this time period, and its integral can be approximated as a linear function of time. According to Equation (1.7), both sides of (1.21) are integrated in [t.sub.m-1,t.sub.m]:

(48) $\begin{array}{cc}{V}^g\left(t\right)=V_{m-1}^g+\Delta V_{\mathrm{sf}}^g\left(t\right)+\Delta V_{\mathrm{cor}/g}^g\frac{t-t_{m-1}}{T}\left(t_{m-1}\le t\le t_m\right)& \left(1.21\right)\end{array}$

(49) to obtain:

(50) $\begin{array}{cc}{P}_m^g=P_{m-1}^g+\left[V_{m-1}^g+\frac{1}{2}\Delta V_{\mathrm{cor}/g\left(m\right)}^g\right]T+\Delta P_{\mathrm{sf}\left(m\right)}^g& \left(1.22\right)\end{array}$

(51) In the equation, P.sub.m.sup.g and P.sub.m-1.sup.g are the position of the SSM in the g frame at t.sub.m and t.sub.m-1.
ΔP.sub.sf(m).sup.g=R.sub.b(m-1).sup.g(m-1)(S.sub.Δvm+ΔP.sub.rotm+ΔP.sub.scrlm)−(ΔP′.sub.rotm+ΔP′.sub.scrlm)  (1.23)

(52) And:

(53) 0$\begin{array}{cc}{S}_{\Delta \mathrm{vm}}=T\left[\frac{5}{6}\Delta V_1+\frac{1}{6}\Delta V_2\right]& \left(1.24\right)\end{array}$

(54) indicates a secondary integral increment of the specific force.

(55) $\begin{array}{cc}\Delta P_{\mathrm{rotm}}=T\left(\Delta {\theta }_1\times \left[\frac{5}{18}\Delta V_1+\frac{1}{6}\Delta V_2\right]+\Delta {\theta }_2\times \left[\frac{1}{6}\Delta V_1+\frac{1}{18}\Delta V_2\right]\right)& \left(1.25\right)\\ \Delta P_{\mathrm{rotm}}^{\prime }=\tau \left({\mathrm{\Delta \theta }}_1^{\prime }\times \left[\frac{5}{18}\Delta V_1^{\prime }+\frac{1}{6}\Delta V_2^{\prime }\right]+{\mathrm{\Delta \theta }}_2^{\prime }\times \left[\frac{1}{6}\Delta V_1^{\prime }+\frac{1}{18}\Delta V_2^{\prime }\right]\right)& \left(1.26\right)\end{array}$

(56) indicates a rotation effect compensation quantity in position calculation.

(57) $\begin{array}{cc}\Delta P_{\mathrm{scrlm}}=T\left({\mathrm{\Delta \theta }}_1\times \left[\frac{11}{90}\Delta V_1+\frac{1}{10}\Delta V_2\right]+\Delta {\theta }_2\times \left[\frac{1}{90}\Delta V_2-\frac{7}{30}\Delta V_1\right]\right)& \left(1.27\right)\\ \Delta P_{\mathrm{scrlm}}^{\prime }=T\left(\Delta {\theta }_1^{\prime }\times \left[\frac{11}{90}\Delta V_1^{\prime }+\frac{1}{10}\Delta V_2^{\prime }\right]+\Delta {\theta }_2^{\prime }\times \left[\frac{1}{90}\Delta V_2^{\prime }-\frac{7}{30}\Delta V_1^{\prime }\right]\right)& \left(1.28\right)\end{array}$

(58) indicates a scroll effect compensation quantity in position calculation. And

(59) $\begin{array}{cc}\Delta P_{\mathrm{rotm}}^{\prime }+\Delta P_{\mathrm{scrlm}}^{\prime }\approx \frac{T^2}{6}{\omega }_{\mathrm{ag}}^g\times \left(R_{b\left(m-1\right)}^{g\left(m-1\right)}\left[\Delta V_1+\Delta V_2\right]\right)& \left(1.29\right)\end{array}$

(60) Thus, the complete algorithm of the ΔP.sub.sf(m).sup.g position increment in the g frame is obtained:

(61) $\begin{array}{cc}\Delta P_{\mathrm{sf}\left(m\right)}^g=R_{b\left(m-1\right)}^{g\left(m-1\right)}\left(S_{\Delta \mathrm{vm}}+\Delta P_{\mathrm{rotm}}+\Delta P_{s\mathrm{crlm}}\right)-\frac{T^2}{6}{\omega }_{\mathrm{ag}}^g\times \left(R_{b\left(m-1\right)}^{g\left(m-1\right)}\left[\Delta V_1+\Delta V_2\right]\right).& \left(1.3\right)\end{array}$