Self-Adaptive Horizontal Attitude Measurement Method based on Motion State Monitoring

Abstract

Disclosed is a self-adaptive horizontal attitude measurement method based on motion state monitoring. Based on a newly established state space model, a horizontal attitude angle is taken as a state variable, an angular velocity increment Δω.sup.b for compensating a random constant zero offset is taken as a control input of a system equation, and a specific force f.sup.b for compensating the random constant zero offset is taken as a measurement quantity. Meanwhile, judgment conditions for a maneuvering state of a carrier are improved, and maneuvering information of the carrier is judged by comprehensively utilizing acceleration information and angular velocity information output by a micro electro mechanical system inertial measurement unit (MEMS-IMU), whereby a measurement noise matrix of a filter can be automatically adjusted, thereby effectively reducing the influence of carrier maneuvering on the calculation of a horizontal attitude. The method has no special requirement on the maneuvering state of the carrier, and can ensure that the system has high attitude measurement precision in different motion states without an external information assistance.

Claims

1. A self-adaptive horizontal attitude measurement method based on motion state monitoring, comprising the following steps: step 1: initially aligning a strapdown inertial navigation system, completing the calculation of a random constant zero offset of a device and the calculation of initial horizontal attitude angles, comprising a roll angle .sub.0 and a pitch angle ϕ.sub.0 , and then entering a navigation working mode; step 2: initializing a Kalman filter, and taking the initial horizontal attitude angles .sub.0 and ϕ.sub.0 obtained in step 1 as initial values of a Kalman filtering state quantity X.sub.0=[.sub.0 ϕ.sub.0].sup.T, an initial mean square error being P.sub.0; step 3: sampling micro electro mechanical system inertial measurement unit (MEMS-IMU) data at a k.sup.th time, and compensating a random constant zero offset thereof to obtain a compensated specific force f.sub.k.sup.b and angular velocity ω.sub.k.sup.b; step 4: performing a Kalman filtering one-step prediction by using an angular velocity increment Δω.sub.k.sup.b at the k.sup.th time as a known deterministic input u.sub.k−1, wherein Δω.sub.k.sup.b=ω.sub.k.sup.b.Math.T, T being a calculation period; step 5: judging a maneuvering state of a carrier by using the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k obtained in step 3, and self-adaptively adjusting a Kalman filtering measurement noise covariance matrix R.sub.k, wherein N is the size of a data window; step 6: when the specific force is f.sub.k.sup.b=[f.sub.x,k f.sub.y,k f.sub.z,k].sup.T at the k.sup.th time, selecting a measurement vector Z.sub.k=[f.sub.x,k f.sub.y,k].sup.T to perform measurement update so as to realize the correction of the state quantity, wherein f.sub.x,k, f.sub.y,k, and f.sub.z,k are components of the specific force f.sub.k.sup.b in x-axis, y-axis, and z-axis directions of a carrier system respectively; and step 7: taking an estimated value of the state quantity at the k.sup.th time as an initial value of a state quantity at a next time, and repeatedly performing steps 3 to 6 until a navigation working state ends.

2. The self-adaptive horizontal attitude measurement method based on motion state monitoring according to claim 1, wherein the Kalman filtering one-step prediction in step 4 comprises: $\{\begin{array}{c}{\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}+B_{k-1}{u}_{k-1}\\ P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+Q_{k-1}\end{array}$ where {circumflex over (X)}.sub.k−1 is a state optimal estimation at a k−1.sup.th time, P.sub.k−, is a mean square error matrix of a state estimation at the k−1.sup.th time, {circumflex over (X)}.sub.k/k−1 is a state one-step prediction at a k.sup.th time, Φ.sub.k/k−1=I.sub.2 is a state one-step transition matrix at the k.sup.th time, I.sub.2 is a second-order unit matrix, $B_k=\left[\begin{array}{ccc}0& \cos {\varphi }_{k-1}& -\sin {\varphi }_{k-1}\\ 1& \sin {\varphi }_{k-1}\tan {\vartheta }_{k-1}& \cos {\varphi }_{k-1}\tan {\vartheta }_{k-1}\end{array}\right]$ is an input coefficient matrix at the k.sup.th time, ϕ.sub.k−1 and .sub.k−1 are horizontal attitude optimal estimations at the k−1.sup.th time, u.sub.k−1 is an angular velocity increment Δω.sub.k.sup.b at the k.sup.th time, P.sub.k/k−1 is a state one-step prediction mean square error matrix at the k.sup.th time, and Q.sub.k is a system noise variance matrix.

3. The self-adaptive horizontal attitude measurement method based on motion state monitoring according to claim 1, wherein in step 5, the judging a maneuvering state of a carrier by using the specific force f.sup.b and the angular velocity ω.sup.b obtained in step 3 comprises: calculating the maneuvering state of the carrier by using the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k obtained in step 3, and acquiring a maneuvering vector T.sub.k to realize maneuvering judgment, T.sub.k satisfying: $T_k=\left(\frac{1}{{\sigma }_f^2}.Math.f_k^b-g\frac{{\overline{f}}_k}{.Math.{\overline{f}}_k.Math.}.Math.+\frac{1}{{\sigma }_{\omega }^2}.Math.{\stackrel{\_}{\omega }}_k.Math.\right)$ $\mathrm{where}{\overline{\omega }}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{.Math.}}{\omega }_i^b\mathrm{and}{\overline{f}}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{.Math.}}{f}_i^b$ are mean values obtained by performing moving average on the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k in step 3, respectively, the size of a data window being N, g being a local gravitational acceleration, and σ.sub.f and σ.sup.ωbeing weighting coefficients respectively.

4. The self-adaptive horizontal attitude measurement method based on motion state monitoring according to claim 3, wherein the self-adaptively adjusting a Kalman filtering measurement noise covariance matrix R.sub.k in step 5 comprises: $R_k=\left[\begin{array}{cc}{\sigma }_r^2+T_{k,1}& 0\\ 0& {\sigma }_r^2+T_{k,2}\end{array}\right]$ where T.sub.k,i is an i.sup.th element of T.sub.k, and σ.sub.r.sup.2 is a variance corresponding to an accelerometer noise.

5. The self-adaptive horizontal attitude measurement method based on motion state monitoring according to claim 1, wherein an update equation of the measurement update in step 6 is: $$\begin{gathered} \{\begin{array}{c}{K}_k=P_{k/k-1}{H_k^T\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k/k-1}\right)\\ P_k=\left(I-K_k{H}_k\right)P_{k/k-1}\end{array} \\ \mathrm{where}{H}_k=\left\{\begin{array}{cc}-g.Math.\cos {\vartheta }_k& 0\\ -g.Math.\sin {\varphi }_k\cos {\vartheta }_k& g.Math.\cos {\varphi }_k\cos {\vartheta }_k\end{array}\right\} \end{gathered}$$ is a measurement matrix at time k, g is a local gravity acceleration, ϕ.sub.k and .sub.k are one-step prediction values of a horizontal attitude at time k, K.sub.k is a filtering gain at time k, {circumflex over (X)}.sub.k is a state estimation at time k, and P.sub.k is a state estimation mean square error matrix at time k.

Description

BRIEF DESCRIPTION OF FIGURES

[0021] FIG. 1 is an implementation flowchart of the disclosure.

[0022] FIG. 2 is a flowchart of an implementation algorithm of the disclosure.

[0023] FIG. 3 is an algorithm calculation error according to the disclosure.

DETAILED DESCRIPTION

[0024] The disclosure will now be further described with reference to the accompanying drawings and specific embodiments.

[0025] The disclosure is implemented as follows:

[0026] An inertial measurement element of a strapdown inertial navigation system is fully pre-heated, and the calculation of a random constant zero offset of a device and the calculation of initial horizontal attitude angles are completed. Then a navigation working state may be entered. A horizontal attitude angle is taken as a state variable, an angular velocity increment Δω.sup.b output by an MEMS-IMU is taken as a control input of a system equation, and a specific force f.sup.b output by the MEMS-IMU is taken as a measurement quantity. A Kalman filtering equation is established, and maneuvering information of the carrier is judged by comprehensively utilizing acceleration information and angular velocity information output by the MEMS-IMU, whereby a measurement noise matrix of a filter can be automatically adjusted, thereby effectively reducing the influence of carrier maneuvering on the calculation of a horizontal attitude. The specific steps are as follows:

[0027] In step 1, an inertial measurement element of a strapdown inertial navigation system is fully pre-heated, the calculation of a random constant zero offset of a device and the calculation of initial horizontal attitude angles (a roll angle .sub.0 and a pitch angle ϕ.sub.0) are completed, and the element enters a navigation working state.

[0028] In step 2, the initial horizontal attitude angles .sub.0 and ϕ.sub.0 obtained in step 1 are taken as initial values of a Kalman filtering state quantity X.sub.0=[.sub.0 ϕ.sub.0].sup.T, an initial mean square error is P.sub.0, and a Kalman filter is initialized.

[0029] In step 3, MEMS-IMU data is sampled at a k.sup.th time, and a random constant zero offset thereof is compensated to obtain a compensated specific force f.sub.k.sup.b and angular velocity ω.sub.k.sup.b.

[0030] In step 4, a Kalman filtering one-step prediction is performed by using an angular velocity increment Δω.sub.k.sup.b at the k.sup.th time as a known deterministic input u.sub.k−1, where Δω.sub.k.sup.b=ω.sub.k.sup.b.Math.T, T being a calculation period.

[0031] In step 5, a maneuvering state of a carrier is judged by using the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k obtained in step 2, and a Kalman filtering measurement noise covariance matrix R.sub.k is self-adaptively adjusted, where N is the size of a data window.

[0032] In step 6, the specific force f.sub.k.sup.b at the k.sup.th time is used as a measurement vector Z.sub.k for measurement update so as to realize the correction of the state quantity.

[0033] In step 7, an estimated value of the state quantity at the k.sup.th time is taken as an initial value of a state quantity at a next time, and steps 3 to 6 are repeatedly performed until a navigation working state ends.

[0034] A correlated equation for Kalman filtering one-step prediction in step 4 is established:

[00006]$\{\begin{array}{c}{\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}+B_{k-1}{u}_{k-1}\\ P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+Q_{k-1}\end{array}$

[0035] where {circumflex over (X)}.sub.k−1 is a state optimal estimation at a k−1.sup.th time, P.sub.k−1 is a mean square error matrix of a state estimation at the k−1.sup.th time, {circumflex over (X)}.sub.k/k−1 is a state one-step prediction at a k.sup.th time, Φ.sub.k/k−1=I.sub.2 is a state one-step transition matrix at the k.sup.th time, I.sub.2 is a second-order unit matrix,

[00007]$B_k=\left[\begin{array}{ccc}0& \cos {\varphi }_{k-1}& -\sin {\varphi }_{k-1}\\ 1& \sin {\varphi }_{k-1}\tan {\vartheta }_{k-1}& \cos {\varphi }_{k-1}\tan {\vartheta }_{k-1}\end{array}\right]$

is an input coefficient matrix at the k.sup.th time, ϕ.sub.k−1 and .sub.k−1 are horizontal attitude optimal estimations at the k−1.sup.th time, u.sub.k−1 is an angular velocity increment Δω.sub.k.sup.b at the k.sup.th time, P.sub.k/k−1 is a state one-step prediction mean square error matrix at the k.sup.th time, and Q.sub.k is a system noise variance matrix.

[0036] In step 5, the maneuvering state of the carrier is judged, the maneuvering state of the carrier is calculated according to the specific force f.sup.b and the angular velocity ω.sup.b obtained in step 3, and a maneuvering vector T.sub.k is acquired to realize maneuvering judgment:

[00008]$T_k=\left(\frac{1}{{\sigma }_f^2}.Math.f_k^b-g\frac{{\overline{f}}_k}{.Math.{\overline{f}}_k.Math.}.Math.+\frac{1}{{\sigma }_{\omega }^2}.Math.{\stackrel{\_}{\omega }}_k.Math.\right)$$\mathrm{where}{\overline{\omega }}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{.Math.}}{\omega }_i^b\mathrm{and}{\overline{f}}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{.Math.}}{f}_i^b$

are mean values obtained by performing moving average on the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k in step 3, respectively, the size of a data window being N, g being a local gravitational acceleration, and σ.sub.f and σ.sub.ω are weighting coefficients respectively.

[0037] A self-adaptive rule for the Kalman filtering measurement noise covariance matrix R.sub.k in step 5 is as follows:

[00009]$R_k=\left[\begin{array}{cc}{\sigma }_r^2+T_{k,1}& 0\\ 0& {\sigma }_r^2+T_{k,2}\end{array}\right]$

[0038] where T.sub.k,i is an i.sup.th element of T.sub.k, and σ.sub.r.sup.2 is a variance corresponding to an accelerometer.

[0039] In step 6, measurement update is performed. The specific force f.sub.k.sup.b=[f.sub.x,k f.sub.y,k f.sub.z,k].sup.T at the k.sup.th time in step 3 is used as a measurement vector Z.sub.k=[f.sub.x,k f.sub.y,k].sup.T for measurement update. An update equation is as follows:

[00010]$$\begin{gathered} \{\begin{array}{c}{K}_k=P_{k/k-1}{H_k^T\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k/k-1}\right)\\ P_k=\left(I-K_k{H}_k\right)P_{k/k-1}\end{array} \\ \mathrm{where}{H}_k=\left\{\begin{array}{cc}-g.Math.\cos {\vartheta }_k& 0\\ -g.Math.\sin {\varphi }_k\cos {\vartheta }_k& g.Math.\cos {\varphi }_k\cos {\vartheta }_k\end{array}\right\} \end{gathered}$$

is a measurement matrix at time k, ϕ.sub.k and .sub.k are one-step prediction values of a horizontal attitude at time k, K.sub.k is a filtering gain at time k, {circumflex over (X)}.sub.k is a state estimation at time k, and P.sub.k is a state estimation mean square error matrix at time k.

[0040] With reference to FIGS. 1 and 2, a specific embodiment of the disclosure includes the following steps:

[0041] In step 1, an inertial measurement element of a strapdown inertial navigation system is fully pre-heated.

[0042] In step 2, data of MEMS-IMU under a static state during a period of time is acquired, and it is considered that an average output value of an MEMS-IMU during the period of time is a random constant zero offset of a device, constant zero offset errors for a gyroscope and an accelerometer are corrected and compensated, and a specific force f.sup.b and an angular velocity ω.sup.b after compensating the constant zero offset errors are obtained.

[0043] In step 3, the strapdown inertial navigation system is initially aligned to obtain initial horizontal attitude angles (a roll angle .sub.0 and a pitch angle ϕ.sub.0) of a carrier system (b system) relative to a navigation coordinate system (n system, the navigation coordinate system herein being a geographical coordinate system), and then a navigation working state starts to be entered.

[0044] In step 4, a Kalman filtering equation is established:

[00011]$\begin{array}{cc}\{\begin{array}{c}{X}_k={\Phi }_{k/k-1}{X}_{k-1}+B_{k-1}{u}_{k-1}+W_{k-1}\\ Z_k=H_k{X}_k+V_k\end{array}& \left(1\right)\end{array}$

[0045] where X.sub.k is a state vector, Φ.sub.k/k−1 is a state one-step transition matrix, B.sub.k−1 is an input coefficient matrix, I.sub.2 is a second-order unit matrix, u.sub.k−1 is a known deterministic input, Z.sub.k is a measurement vector, H.sub.k is a measurement matrix, W.sub.k−1 is a system noise vector, and V.sub.k is a measurement noise vector.

[0046] In step 5, horizontal attitude angles are selected as a state quantity X=[ ϕ].sup.T (a roll angle and a pitch angle ϕ), the initial horizontal attitude angles .sub.0 and ϕ.sub.0 obtained in step 3 are taken as initial values of the state quantity X.sub.0=[.sub.0 ϕ.sub.0].sup.T, an initial mean square error is P.sub.0, and a Kalman filter is initialized.

[0047] In step 6, MEMS-IMU data is sampled at a k.sup.th time, and a random constant zero offset thereof is compensated to obtain a compensated specific force f.sub.k.sup.b and angular velocity ω.sub.k.sup.b.

[0048] In step 7, a Kalman filtering one-step prediction is performed by using an increment Δω.sub.k.sup.b=ω.sub.k.sup.b.Math.T (T being a calculation period) within a ω.sub.k.sup.b sampling interval at the k.sup.th time in step 6 as a known deterministic input u.sub.k−1:

[00012]$\begin{array}{cc}\{\begin{array}{c}{\hat{X}}_{k/k-1}={\Phi }_{k/k-1}{\hat{X}}_{k-1}+B_{k-1}{u}_{k-1}\\ P_{k/k-1}={\Phi }_{k/k-1}{P}_{k-1}{\Phi }_{k/k-1}^T+Q_{k-1}\end{array}& \left(2\right)\end{array}$

[0049] where {circumflex over (X)}.sub.k−1 is a state optimal estimation at a k−1.sup.th time, P.sub.k−1 is a mean square error matrix of a state estimation at the k−1.sup.th time, {circumflex over (X)}.sub.k/k−1 is a state one-step prediction at a k.sub.th time, Φ.sub.k/k−1=I.sub.2 is a state one-step transition matrix at the k.sup.th time,

[00013]$B_k=\left[\begin{array}{ccc}0& \cos {\varphi }_{k-1}& -\sin {\varphi }_{k-1}\\ 1& \sin {\varphi }_{k-1}\tan {\vartheta }_{k-1}& \cos {\varphi }_{k-1}\tan {\vartheta }_{k-1}\end{array}\right]$

is an input coefficient matrix at the k.sup.th time, ϕ.sub.k−1 and .sub.k−1 are horizontal attitude optimal estimations at the k−1.sup.th time, u.sub.k−1 is an angular velocity increment Δω.sub.k.sup.b at the k.sup.th time, P.sub.k/k−1 is a state one-step prediction mean square error matrix at the k.sup.th time, and Q.sub.k is a system noise variance matrix.

[0050] In step 8, a maneuvering state of a carrier is judged by using the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k obtained in step 6, and a maneuvering vector T.sub.k is acquired, thereby self-adaptively adjusting a Kalman filtering measurement noise covariance matrix R.sub.k.

[00014]$\begin{array}{cc}{T}_k=\left(\frac{1}{{\sigma }_f^2}.Math.f_k^b-g\frac{{\overline{f}}_k}{.Math.{\overline{f}}_k.Math.}.Math.+\frac{1}{{\sigma }_{\omega }^2}.Math.{\stackrel{\_}{\omega }}_k.Math.\right)& \left(3\right)\end{array}$$\mathrm{where}{\overline{\omega }}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{.Math.}}{\omega }_i^b\mathrm{and}{\overline{f}}_k=\frac{1}{N}\underset{i=k-N+1}{\overset{k}{.Math.}}{f}_i^b$

are mean values obtained by performing moving average on the specific force f.sup.b and the angular velocity ω.sup.b from time k−N+1 to time k in step 6, respectively, the size of a data window is N, g is a local gravitational acceleration, σ.sub.f is a weighting coefficient of the specific force (usually 0.5-5 for vessels and 1-3 for vehicles), and σ.sub.ω is a weighting coefficient of the angular velocity (usually 1-5 for vessels and 0.5-1 for vehicles).

[0051] A self-adaptive adjustment rule for the Kalman filtering measurement noise covariance matrix R.sub.k is as follows:

[00015]$\begin{array}{cc}{R}_k=\left[\begin{array}{cc}{\sigma }_r^2+T_{k,1}& 0\\ 0& {\sigma }_r^2+T_{k,2}\end{array}\right]& \left(4\right)\end{array}$

[0052] where T.sub.k,i is an i.sup.th element of T.sub.k, and σ.sub.r.sup.2 is a variance corresponding to an accelerometer.

[0053] In step 9, the specific force f.sub.k.sup.b=[f.sub.x,k f.sub.y,k f.sub.z,k].sup.T at the k.sup.th time in step 6 is used as a measurement vector Z.sub.k=[f.sub.x,k f.sub.y,k ].sup.T for measurement update. An update equation is as follows:

[00016]$$\begin{gathered} \begin{array}{cc}\{\begin{array}{c}{K}_k=P_{k/k-1}{H_k^T\left(H_k{P}_{k/k-1}{H}_k^T+R_k\right)}^{-1}\\ {\hat{X}}_k={\hat{X}}_{k/k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k/k-1}\right)\\ P_k=\left(I-K_k{H}_k\right)P_{k/k-1}\end{array} \\ \mathrm{where}{H}_k=\left\{\begin{array}{cc}-g.Math.\cos {\vartheta }_k& 0\\ -g.Math.\sin {\varphi }_k\cos {\vartheta }_k& g.Math.\cos {\varphi }_k\cos {\vartheta }_k\end{array}\right\}& \left(5\right)\end{array} \end{gathered}$$

is a measurement matrix at time k, g is a local gravity acceleration, ϕ.sub.k and .sub.k are one-step prediction values of a horizontal attitude at time k, K.sub.k is a filtering gain at time k, {circumflex over (X)}.sub.k is a state estimation at time k, and P.sub.k is a state estimation mean square error matrix at time k.

[0054] In step 10, an estimated value of the state quantity at the k.sup.th time is taken as an initial value of a state quantity at a next time, and steps 6 to 9 are repeatedly performed.

[0055] The self-adaptive horizontal attitude measurement method based on motion state monitoring has been completed so far.

[0056] In order to illustrate the effectiveness of an algorithm, the algorithm is simulated. Simulation conditions are set as follows. A zero offset of a gyroscope of each axis is set to 1°/h, and a zero offset of an accelerometer is set to 1×10.sup.−4 g. A motion state is set as follows. A carrier is in an oscillating motion state. The oscillation of each axis follows the sine function law. Oscillation amplitudes of rolling, pitching, and heading are all 10°, oscillation periods are all 10.sup.S, and initial attitude angles and phase angles are all 0°. The simulation results are shown in FIG. 3. The horizontal attitude measurement error is small, and the precision is high.

[0057] Although examples and drawings of the disclosure have been disclosed for illustrative purposes, those skilled in the art will appreciate that: various substitutions, changes and modifications are possible without departing from the spirit and scope of the disclosure and the appended claims, and therefore the scope of the disclosure is not limited to the disclosure of the examples and drawings.