CN104655131A

CN104655131A - Initial inertial navigation alignment method based on terated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF)

Info

Publication number: CN104655131A
Application number: CN201510063634.6A
Authority: CN
Inventors: 徐晓苏; 田泽鑫; 张涛; 刘义亭; 孙进; 姚逸卿
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2015-02-06
Filing date: 2015-02-06
Publication date: 2015-05-27
Anticipated expiration: 2035-02-06
Also published as: CN104655131B

Abstract

The invention discloses an initial alignment method based on an iterated strong tracking spherical simplex radial cubature Kalman filter (ISTSSRCKF). The method comprises the following steps: selecting the cubature point of a CKF by adopting an SSR rule; introducing a fading factor in a strong tracking filter into the time and measurement update equation of the CKF; and introducing a Gauss-Newton iterative algorithm and improving the corresponding information variance and covariance in the iterative process. According to the initial alignment problem under the conditions of large misalignment angle and base sway, the excessive sampling points in the high-order CKF are reduced during sampling by adopting the SSR rule, and the problem that the performance of the CKF is reduced when a model is inaccurate is solved by introducing a strong tracking algorithm. Latest measurement information is fully utilized in using the iterative process, so that the state estimation error is effectively reduced, and the initial alignment precision superior to standard CKF is obtained.

Description

Inertial navigation initial alignment method based on ISTSSRCKF

Technical Field

The invention mainly relates to the technical field of navigation, in particular to an inertial navigation initial alignment method based on ISTSRCKF.

Background

The initial alignment is firstly carried out before the inertial navigation system enters a working state, the inertial sensor, namely a gyroscope and an accelerometer, of the strapdown inertial navigation system are directly and fixedly connected with a carrier, and a computing platform is obtained through calculation to replace a physical platform, so that the initial alignment problem of the strapdown inertial navigation system is to solve an attitude matrix at an initial moment. The initial alignment of the SINS directly affects the navigation performance, i.e. navigation accuracy and reaction time, of the strapdown system. An Autonomous Underwater Vehicle (AUV) is a task controller which integrates artificial intelligence and other advanced computing technologies, and is a bottleneck problem for determining the technical development and application of the AUV in order to enable the AUV to complete a preset mission and not to leave an AUV navigation technology. In practical application environments, particularly under the conditions of large misalignment angle and severe shaking, a classical Kalman filtering method using a small linear misalignment angle as an alignment model cannot be used for filtering, only a nonlinear alignment model can be established, and the traditional nonlinear filtering algorithms such as EKF (extended Kalman Filter) and UKF (unscented Kalman Filter) have the defects of low alignment precision and unstable numerical value in a high-dimensional state and poor uncertain factor coping capability, so that the alignment method under the SINS nonlinear shaking base, which effectively copes with complex environments and has certain strong tracking capability, has important significance.

Disclosure of Invention

The purpose of the invention is as follows: in order to overcome the defects in the existing nonlinear alignment technology, the invention provides an inertial navigation initial alignment method based on the ISTSRCKF, which can improve the SINS alignment precision.

The technical scheme is as follows: in order to solve the technical problem, the invention provides an inertial navigation initial alignment method based on the ISTSRCKF, which comprises the following steps:

step 1: establishing an SINS shaking base alignment model under a large misalignment angle state, wherein the alignment model comprises a SINS nonlinear error model, a nonlinear filtering state model and a nonlinear filtering measurement model:

wherein, the process of establishing the SINS nonlinear error model is as follows:

step 1.1: a northeast geographic coordinate system is used as a navigation coordinate system n, a coordinate system formed by a carrier right front upper direction vector right hand rule is used as a carrier coordinate system b, and the real attitude angle isTrue velocity of

v_{s}^{n} = {[\begin{matrix} v_{e}^{n} & v_{n}^{n} & v_{u}^{n} \end{matrix}]}^{T},

The real geographic coordinate is p ═ L lambda H]^T(ii) a The attitude actually solved by SINS isAt a speed of

{\tilde{v}}_{s}^{n} = {[\begin{matrix} {\tilde{v}}_{e}^{n} & {\tilde{v}}_{n}^{n} & {\tilde{v}}_{u}^{n} \end{matrix}]}^{T},

The geographic coordinates are

Recording coordinate system established by geographical position resolved by SINS as calculationA navigation coordinate system n' is defined as SINS attitude angle and speed error respectivelyPhi and vⁿThe differential equation of (a) is as follows:

wherein phi is [ phi ]_e φ_n φ_u]^TV is the pitch angle, roll angle and course angle errorⁿ＝[v_e v_n v_u]^TFor east, north and sky speed errors,

b is the constant error of the lower triaxial gyro,is b is the random error of the lower three-axis gyroscope,

b is the constant error of the lower triaxial accelerometer,to be b is the random error of the lower three-axis accelerometer,is the actual output of the accelerometer,for the speed of the solution of the SINS,for the calculated angular velocity of rotation in the navigation coordinate system,for the purpose of calculating the angular velocity of rotation of the earth,for the calculated angular velocity of the navigation coordinate system relative to the earth's rotation, to correspond toThe error in the calculation of (a) is,n is a rotation angle phi in sequence_e、φ_n、φ_uObtaining a directional cosine matrix formed by n',is a state transition matrix from b series to n' series, i.e. a calculated attitude matrix, C_wFor coefficient matrix of Euler angle differential equation, superscript T represents transposition, matrixAnd C_wThe method specifically comprises the following steps:

the nonlinear filtering state model establishing process comprises the following steps:

step 1.2: taking the Euler platform error angle phi of the SINS_e、φ_n、φ_uVelocity error v_e、v_nAnd b is the gyro constant errorConstant error of accelerometer under b systemConstructing state vectors

Under the shaking baseAndapproximately zero, the state equation for the nonlinear filtering can be simplified as:

wherein,taking only the first two-dimensional state, ω (t) ═ ω_g ω_a 0_1×3 0_1×2]^TFor the zero-mean white gaussian noise process, the nonlinear filter state equation is simplified as:

the nonlinear measurement equation is established as follows:

step 1.3: the true velocity denoted by b is v^bThe actual speed under b isAttitude matrix handlebar resolved by SINSIs converted intoTo be provided withAndthe east and north velocity components in (b) are used as matching information sources, and then the measurement equation of the nonlinear filtering is:

taking the former two dimensions of z as an observed value, taking v as a zero-mean Gaussian white noise process, and summarizing the continuous nonlinear filtering measurement equation as:

z(t)＝h(x,t)+v(t)

step 2: with a sampling period T_sAs a filter period, and with T_sFor discretization of the step size, willDiscretization into x_k＝x_k-1+[f(x_k-1,t_k-1)+ω(t_k-1)]T_sThe equation of state can be found:

x_k＝f(x_k-1)+ω_k-1

discretizing z (t) h (x, t) + v (t) to z_k＝h(x_k,t_k)+v(t_k) The measurement equation can be obtained:

z_k＝h(x_k)+v_k

the following discrete nonlinear system is composed of a state equation and a measurement equation:

in the formula, x_kIs the state vector of the system at the moment k; z is a radical of_kMeasured value of system state at k moment; random system noise omega_k～N(0,Q_k) (ii) a Random observation noise v_k～N(0,R_k)；

And step 3: using current measurement value z_kSubtracting the measurement predicted value at the same timeObtaining the residual error of the current moment_kCalculating a suboptimal fading factor lambda according to the strong tracking principle_kThen using λ_kCorrecting the updating process of the filtering time;

and 4, step 4: time-updated predictive estimatesAnd the predicted variance P_k/k-1Performing an iteration process for the initial value, performing a maximum number of iterations N_maxPerforming secondary iteration to complete measurement updating;

wherein N is_maxThe number of iterations can be selected as required, and is taken as10。

And 5: euler plateau angle estimation obtained by filtering with filterAnd velocity estimateCorrecting SINS resolved attitude matrixAnd velocityTaking the corrected value as the initial value of the next strapdown calculation, and utilizing the currently obtained gyro constant error estimation valueAnd a constant error estimate of the accelerometerCorrecting the gyro output at the next momentAnd output of the accelerometerThe specific correction formula is calculated according to the following formula:

and if the precision reaches the preset initial alignment requirement, ending the process, otherwise, continuously and circularly executing the steps 2-5 until the initial alignment is ended.

Further, in 2), the step of performing time update according to the obtained discretization model in a framework of the cubature kalman filter specifically includes:

step 2.2: setting system state initial valueInitial error covariance matrix:

P₀＝diag{(1°)²(1°)²(10°)²(0.1)²(0.1)²

(0.01°/h)²(0.01°/h)²(0.01°/h)²(100μg)²(100μg)²}*10

and to P₀Cholesky decomposition is carried out to obtain the characteristic square root S of the initial error covariance matrix₀；

Step 2.3: sampling points are selected by adopting an SSR rule, and the following vectors are taken:

a_j＝[a_j,1 a_j,2 … a_j,n]^Tj is 1,2, …, n +1, where n is the number of state quantities, then

<math> <mrow> <msub> <mi>a</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>&equiv;</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mo>-</mo> <msqrt> <mfrac> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> <mrow> <mi>n</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>i</mi> <mo>+</mo> <mn>2</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>i</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mfrac> </msqrt> <mo>,</mo> </mtd> <mtd> <mi>i</mi> <mo><</mo> <mi>j</mi> </mtd> </mtr> <mtr> <mtd> <msqrt> <mfrac> <mrow> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>j</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> <mrow> <mi>n</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>j</mi> <mo>+</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mfrac> </msqrt> <mo>,</mo> </mtd> <mtd> <mi>i</mi> <mo>=</mo> <mi>j</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> <mo>,</mo> </mtd> <mtd> <mi>i</mi> <mo>></mo> <mi>j</mi> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

Xi is selected_iRepresents the ith volume point, and m is 2(n +1) volume points:

wherein n is the number of state quantities, and n is 10; shorthand non-linear multi-dimensional integralQ (f), SSR rules

Step 2.4: calculating a state one-step prediction valueCovariance matrix P of sum-step prediction errors_k/k-1: decomposing error covariance matrix P by Cholesky_k-1/k-1To obtain S_k-1/k-1：

P_k-1/k-1＝S_k-1/k-1S^T _k-1/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the equation of state:

X^* _i,k/k-1＝f(X_i,k-1/k-1)

estimating a state prediction value at the k moment:

estimating the state error covariance at time k:

further, in the step 3:

calculating a suboptimal fading factor lambda according to a strong tracking principle_kThe operation process is as follows:

using current measurement value z_kSubtracting the measurement predicted value at the same timeThe residual error at the current moment is obtained as follows:

taking a covariance matrix of an actual output sequence:

in the formula, rho is a forgetting factor, and the value range is generally that rho is more than or equal to 0.95 and less than or equal to 0.98;

calculating a matrix:

N_{k} = V_{k} - {P^{T}}_{xz, k / k - 1} P_{k / k - 1}^{- 1} Q_{k - 1} P_{k / k - 1}^{- 1} P_{xz, k / k - 1} - R_{k}

M_k＝P_zz,k/k-1+N_k-V_k

in the formula P_zz,k/k-1For outputting prediction covariance matrix, P_xz,k/k-1Is a cross variance matrix.

Calculating a suboptimal fading factor lambda_k：

<math> <mrow> <msub> <mi>λ</mi> <mi>k</mi> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <msub> <mi>λ</mi> <mn>0</mn> </msub> <mo>,</mo> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>λ</mi> <mn>0</mn> </msub> <mo>&GreaterEqual;</mo> <mn>1</mn> <mo>)</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mn>1</mn> <mo>,</mo> </mtd> <mtd> <mrow> <mo>(</mo> <msub> <mi>λ</mi> <mn>0</mn> </msub> <mo><</mo> <mn>1</mn> <mo>)</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> <msub> <mi>λ</mi> <mn>0</mn> </msub> <mo>=</mo> <mfrac> <mrow> <mi>tr</mi> <mo>[</mo> <msub> <mi>N</mi> <mi>k</mi> </msub> <mo>]</mo> </mrow> <mrow> <mi>tr</mi> <mo>[</mo> <msub> <mi>M</mi> <mi>k</mi> </msub> <mo>]</mo> </mrow> </mfrac> </mrow> </math>

Where tr [ □ ] represents the traces of the matrix.

Further, the iterative CKF algorithm in step 4:

step 4.1.1: using the formulas in step 2.4 to calculate the one-step predicted value of the stateCovariance matrix P of sum-step prediction errors_k/k-1；

Step 4.1.2: calculating an evanescent factor lambda_kDecomposition of P by Cholesky_k/k-1：

P_k/k-1＝S_k/k-1S^T _k/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the observation equation:

Z_i,k/k-1＝h(X_i,k/k-1)

estimating an observation predicted value at the k moment:

estimating an autocorrelation covariance matrix:

estimating cross-correlation covariance matrix:

calculating the fading factor lambda according to the step 3_k；

Step 4.1.3: using the determined fading factor lambda_kFor state prediction covariance matrix P_k/k-1The following transformations are performed:

step 4.2.1: the measurement is updated byAnd P_k/k-1An iterative process of initial values. Let the estimate and variance of the ith iteration beAnd(i＝0，1，2，…，N_max)：

step 4.2.2: new factorizations and volume points are generated, chol (□) represents the Cholesky decomposition of the matrix:

{\hat{S}}_{k}^{(i)} = chol (P_{k}^{(i)})

in which ξ_jRepresents the jth volume point;

step 4.2.3: with P in step 4.1.3_k/k-1Recalculating the formulas in step 4.1.2 to obtain P_zz,k/k-1、P_xz,k/k-1。

Calculate the state and variance estimates for the ith iteration:

W_{k}^{(i)} = P_{xz, k}^{(i)} {(P_{zz, k}^{(i)})}^{- 1}

{\hat{x}}_{k}^{(i + 1)} = {\hat{x}}_{k / k - 1} + W_{k}^{(i)} [z_{k} - h ({\hat{x}}_{k}^{(i)}) - {(P_{xz, k}^{(i)})}^{T} {(P_{k / k - 1})}^{- 1} ({\hat{x}}_{k / k - 1} - {\hat{x}}_{k}^{(i)})]

P_{k}^{(i + 1)} = P_{k / k - 1} - W_{k}^{(i)} (P_{zz, k}^{(i)}) {(W_{k}^{(i)})}^{T}

step 4.2.4: if the iteration number is N when the iteration is terminated, the state estimation and covariance at the moment k are as follows:

{\hat{x}}_{k} = {\hat{x}}_{k}^{(N)}, P_{k} = P_{k}^{(N)} .

the invention discloses an initial alignment method based on ISTSSRCKF, which adopts SSR rules to select volume points of CKF; introducing an evanescent factor in the strong tracking filtering into a time and measurement updating equation of the CKF; and introducing and improving corresponding innovation variance and covariance in an iterative process by using a Gauss-Newton iterative algorithm. For the initial alignment problem under the conditions of large misalignment angle and base shaking, the invention reduces excessive sampling points in high-order CKF by adopting SSR rule sampling, introduces a strong tracking algorithm to overcome the problem of performance reduction of the CKF when the model is inaccurate, and fully utilizes the latest measurement information by using an iterative process, thereby effectively reducing the state estimation error and obtaining the initial alignment precision superior to the standard CKF.

Has the advantages that: compared with the prior art, the invention has the following advantages:

1) the problem that CKF alignment precision is reduced under the conditions of shaking the base and large misalignment angle is solved, and high-precision initial attitude information is provided for accurate navigation and positioning of the strapdown inertial navigation system.

2) The sampling points are selected according to the SSR rules, so that the problem of excessive high-order CKF sampling points is solved, the operation complexity is reduced, and the calculation precision is improved.

3) In order to solve the problem of performance reduction of the CKF when the model is inaccurate, an evanescence factor in a strong tracking algorithm is introduced into a time and measurement updating equation of the CKF. A Gauss-Newton iterative algorithm is introduced, the latest measurement information is fully utilized in the iterative process, and the innovation variance and covariance generated in the iterative process are improved, so that the state estimation error can be effectively reduced, and the alignment precision superior to the standard CKF is obtained.

Drawings

FIG. 1 is an installation view of a three-axis gyroscope and three-axis accelerometer of the present invention.

FIG. 2 is a diagram of an alignment scheme of the SINS shaking base of the present invention.

Fig. 3 is a schematic diagram of an initial alignment method based on the istsssrckf according to the present invention.

FIG. 4 is a rocking simulation diagram of the pitch angle, roll angle and course angle of the rocking base according to the present invention.

Fig. 5 is a simulation diagram of the output of the SINS three-axis gyroscope of the present invention.

FIG. 6 is a simulation diagram of the output of the SINS triaxial accelerometer of the present invention.

FIG. 7 is a diagram of the initial alignment pitch angle error under the SINS shaking base of the present invention.

FIG. 8 is a diagram of the initial alignment roll angle error under the SINS shaking base of the present invention.

FIG. 9 is a diagram of the initial alignment course angle error under the SINS shaking base according to the present invention.

Detailed Description

The present invention will be further described with reference to the accompanying drawings.

v_{s}^{n} = {[\begin{matrix} v_{e}^{n} & v_{n}^{n} & v_{u}^{n} \end{matrix}]}^{T},

{\tilde{v}}_{s}^{n} = {[\begin{matrix} {\tilde{v}}_{e}^{n} & {\tilde{v}}_{n}^{n} & {\tilde{v}}_{u}^{n} \end{matrix}]}^{T},

The geographic coordinates are

Recording a coordinate system established by the geographical position solved by the SINS as a calculation navigation coordinate system n', and defining SINS attitude angle and speed error asPhi and vⁿThe differential equation of (a) is as follows:

wherein phi is [ phi ]_e φ_n φ_u]^TV is the pitch angle, roll angle and course angle errorⁿ＝[v_e v_n v_u]^TIs east and northAnd the error of the speed in the direction of the sky,

the nonlinear measurement equation is established as follows:

z(t)＝h(x,t)+v(t)

x_k＝f(x_k-1)+ω_k-1

the measurement equation z (t) is discretized into z (h (x, t) + v (t)_k＝h(x_k,t_k)+v(t_k) The measurement equation can be obtained:

z_k＝h(x_k)+v_k

step 2.1: the following discrete nonlinear system is composed of a state equation and a measurement equation:

in the formula, x_kIs the state vector of the system at the moment k; z is a radical of_kMeasured value of system state at k moment; random system noise omega_k～N(0,Q_k) (ii) a Random observation noise v_k～N(0,R_k)。

Step 2.2: setting system state initial valueInitial error covariance matrix:

P₀＝diag{(1°)²(1°)²(10°)²(0.1)²(0.1)²

(0.01°/h)²(0.01°/h)²(0.01°/h)²(100μg)²(100μg)²}*10

Step 2.3: sampling points are taken by adopting an SSR rule, and a group of vectors are taken as follows: a is_j＝[a_j,1 a_j,2 … a_j,n]^TJ is 1,2, …, n +1, where n is the number of state quantities, then

<math> <mrow> <msub> <mi>a</mi> <mrow> <mi>j</mi> <mo>,</mo> <mi>i</mi> </mrow> </msub> <mo>&equiv;</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mo>-</mo> <msqrt> <mfrac> <mrow> <mi>n</mi> <mo>+</mo> <mn>1</mn> </mrow> <mrow> <mi>n</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>i</mi> <mo>+</mo> <mn>2</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>i</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </mfrac> </msqrt> <mo>,</mo> </mtd> <mtd> <mi>i</mi> <mo><</mo> <mi>j</mi> </mtd> </mtr> <mtr> <mtd> <msqrt> <mfrac> <mrow> <mrow> <mo>(</mo> <mi>n</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>j</mi> <mo>+</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> <mrow> <mi>n</mi> <mrow> <mo>(</mo> <mi>n</mi> <mo>-</mo> <mi>j</mi> <mo>+</mo> <mn>2</mn> <mo>)</mo> </mrow> </mrow> </mfrac> </msqrt> <mo>,</mo> </mtd> <mtd> <mi>i</mi> <mo>=</mo> <mi>j</mi> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> <mo>,</mo> </mtd> <mtd> <mi>i</mi> <mo>></mo> <mi>j</mi> </mtd> </mtr> </mtable> </mfenced> <mo>,</mo> </mrow> </math>

Step 2.4: calculating a state one-step prediction valueCovariance matrix of sum-step prediction errorsDecomposing error covariance matrix P by Cholesky_k-1/k-1To obtain S_k-1/k-1：

P_k-1/k-1＝S_k-1/k-1S^T _k-1/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the equation of state:

X^* _i,k/k-1＝f(X_i,k-1/k-1)

estimating a state prediction value at the k moment:

estimating the state error covariance at time k:

and step 3: using current measurement value z_kSubtracting the measurement predicted value at the same timeThe residual error at the current moment is obtained as follows:

taking a covariance matrix of an actual output sequence:

calculating a matrix:

N_{k} = V_{k} - {P^{T}}_{xz, k / k - 1} P_{k / k - 1}^{- 1} Q_{k - 1} P_{k / k - 1}^{- 1} P_{xz, k / k - 1} - R_{k}

M_k＝P_zz,k/k-1+N_k-V_k

Calculating a suboptimal fading factor lambda_k：

Where tr [ □ ] represents the traces of the matrix.

And 4, step 4: time-updated predictive estimatesAnd the predicted variance P_k/k-1Performing an iterative process for the initial value, performing N_maxPerforming secondary iteration to complete measurement updating;

the iterative CKF algorithm process is as follows:

step 4.1.1: using the formulas in step 2.4 to calculate the one-step predicted value of the stateCovariance matrix P of sum-step prediction errors_k/k-1。

P_k/k-1＝S_k/k-1S^T _k/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the observation equation:

Z_i,k/k-1＝h(X_i,k/k-1)

estimating an observation predicted value at the k moment:

estimating an autocorrelation covariance matrix:

estimating cross-correlation covariance matrix:

calculating the fading factor lambda according to the step 3_k。

step 4.2.1: the measurement is updated byAnd P_k/k-1An iterative process of initial values. Let the estimate and variance of the ith iteration beAnd(i＝0,1,2,…,N_max)：

{\hat{S}}_{k}^{(i)} = chol (P_{k}^{(i)})

in which ξ_jRepresenting the jth volume point.

Calculate the state and variance estimates for the ith iteration:

W_{k}^{(i)} = P_{xz, k}^{(i)} {(P_{zz, k}^{(i)})}^{- 1}

{\hat{x}}_{k}^{(i + 1)} = {\hat{x}}_{k / k - 1} + W_{k}^{(i)} [z_{k} - h ({\hat{x}}_{k}^{(i)}) - {(P_{xz, k}^{(i)})}^{T} {(P_{k / k - 1})}^{- 1} ({\hat{x}}_{k / k - 1} - {\hat{x}}_{k}^{(i)})]

P_{k}^{(i + 1)} = P_{k / k - 1} - W_{k}^{(i)} (P_{zz, k}^{(i)}) {(W_{k}^{(i)})}^{T}

{\hat{x}}_{k} = {\hat{x}}_{k}^{(N)}, P_{k} = P_{k}^{(N)}

and 5: using currently acquired EulerPlatform angle estimateAnd velocity estimateCorrecting SINS resolved attitude matrixAnd velocityTaking the corrected value as the initial value of the next strapdown calculation, and utilizing the currently obtained gyro constant error estimation valueAnd a constant error estimate of the accelerometerCorrecting the gyro output at the next momentAnd output of the accelerometerThe specific correction formula is calculated according to the following formula:

and if the precision meets the requirement of initial alignment, ending the process, otherwise, continuously and circularly executing the steps 2-5 until the initial alignment is ended.

Fig. 1 shows that three gyroscopes and three accelerometers are orthogonally and fixedly installed inside an AUV, fig. 2 is a schematic diagram of an alignment scheme of an SINS shaking base according to the present invention, and fig. 3 is a schematic diagram of an initial alignment method based on istsssrckf according to the present invention.

The following description is directed to a generic underwater vehicle.

The following simulation examples are used to illustrate the practical effects of the present invention:

1) ship motion condition and parameter setting

The initial simulation moment is at 32 degrees north latitude and 118 degrees east longitude, and at 20m underwater, under the action of an underwater environment, the three-axis sine swinging motion is respectively carried out around a pitch axis, a roll axis and a heading axis, the swinging amplitudes of a pitch angle theta, a roll angle gamma and a heading angle psi are 10 degrees, 8 degrees and 6 degrees in sequence, the corresponding swinging periods are 6s, 8s and 10s, and curves simulated by the parameters are shown in FIG. 4.

2) Parameter setting for gyros and accelerometers

The strapdown inertial navigation usually adopts a fiber-optic gyroscope and a flexible accelerometer, the constant drift of the gyroscope is 0.02 degree/h, the random drift of the gyroscope is 0.01 degree/h, and the constant offset of the accelerometer is 100 multiplied by 10^-6g, random error of accelerometer is 50 × 10^-6g (g is gravity acceleration), and simulating the output omega of the three-axis gyroscope_x、ω_y、ω_zAs shown in FIG. 5, the triaxial accelerometer output f_x、f_y、f_zAs shown in fig. 6.

3) Analysis of simulation results

The initial misalignment angle is set to 10 degrees, 10 degrees and 10 degrees, the nonlinear filtering algorithm provided by the invention is used for carrying out the initial alignment of the SINS shaking base, and the pitch angle error phi after the alignment is finished_xRoll angle error phi_yAnd course angle error phi_zAs shown in fig. 7, 8 and 9, respectively. Simulation results show that the nonlinear filtering technology provided by the invention has higher alignment precision under the conditions of a complex water area under the conditions of shaking a base and existence of a large misalignment angle, and can meet the initial alignment requirement of navigation positioning.

The above description is only of the preferred embodiments of the present invention, and it should be noted that: it will be apparent to those skilled in the art that various modifications and adaptations can be made without departing from the principles of the invention and these are intended to be within the scope of the invention.

Claims

1. An inertial navigation initial alignment method based on ISTSRCKF is characterized by comprising the following steps:

step 1.1: taking a northeast geographic coordinate system as a navigation coordinate system n and a carrier right front upper direction vector right handA coordinate system is formed by the rule as a carrier coordinate system b, and the real attitude angle isTrue velocity ofThe real geographic coordinate is p ═ L lambda H]^T(ii) a The attitude actually solved by SINS isAt a speed ofThe geographic coordinates areRecording a coordinate system established by the geographical position solved by the SINS as a calculation navigation coordinate system n', and defining SINS attitude angle and speed error as Phi and vⁿThe differential equation of (a) is as follows:

wherein phi is [ phi ]_e φ_n φ_u]^TV is the pitch angle, roll angle and course angle errorⁿ＝[v_e v_n v_u]^TFor east, north and sky speed errors,b is the constant error of the lower triaxial gyro,is b is the random error of the lower three-axis gyroscope,b is the constant error of the lower triaxial accelerometer,to be b is the random error of the lower three-axis accelerometer,is the actual output of the accelerometer,for the speed of the solution of the SINS,for the calculated angular velocity of rotation in the navigation coordinate system,for the purpose of calculating the angular velocity of rotation of the earth,for the calculated angular velocity of the navigation coordinate system relative to the earth's rotation, to correspond toThe error in the calculation of (a) is,n is a rotation angle phi in sequence_e、φ_n、φ_uObtaining a directional cosine matrix formed by n',is a state transition matrix from b series to n' series, i.e. a calculated attitude matrix, C_wFor coefficient matrix of Euler angle differential equation, superscript T represents transposition, matrixAnd C_wThe method specifically comprises the following steps:

step 1.2: taking the Euler platform error angle phi of the SINS_e、φ_n、φ_uVelocity error v_e、v_nAnd b is the gyro constant errorConstant error of accelerometer under b systemConstructing state vectorsUnder the shaking baseAndapproximately zero, the state equation for the nonlinear filtering can be simplified as:

the nonlinear measurement equation is established as follows:

z(t)＝h(x,t)+v(t)

x_k＝f(x_k-1)+ω_k-1

z_k＝h(x_k)+v_k

And step 3: using current measurement value z_kSubtracting the measurement predicted value at the same timeObtaining the residual error of the current moment_kCalculating a suboptimal fading factor lambda according to the strong tracking principle_kThen using λ_kCorrection filterA wave time updating process;

2. The inertial navigation initial alignment method based on the ISTSRCKF of claim 1, wherein: in the step 2), the step of performing time update according to the obtained discretization model in the framework of the volume kalman filter specifically includes:

step 2.2: setting system state initial valueInitial error covariance matrix:

P₀＝diag{(1°)²(1°)²(10°)²(0.1)²(0.1)²

(0.01°/h)²(0.01°/h)²(0.01°/h)²(100μg)²(100μg)²}*10

a_j＝[a_j,1 a_j,2 … a_j,n]^Tj is 1, 2., n +1, where n is the number of state quantities, then

wherein n is the number of state quantities, and n is 10; shorthand non-linear multi-dimensional integral

Q (f), SSR rules

Step 2.4: calculating a state one-step prediction valueCovariance matrix P of sum-step prediction errors_k/k-1：

Decomposing error covariance matrix P by Cholesky_k-1/k-1To obtain S_k-1/k-1：

P_k-1/k-1＝S_k-1/k-1S^T _k-1/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the equation of state:

X^* _i,k/k-1＝f(X_i,k-1/k-1)

estimating a state prediction value at the k moment:

estimating the state error covariance at time k:

3. the inertial navigation initial alignment method based on the ISTSRCKF of claim 1, wherein: in the step 3:

said strong tracking of evidencePrinciple calculation of suboptimal evanescence factor lambda_kThe operation process is as follows:

taking a covariance matrix of an actual output sequence:

calculating a matrix:

Calculating a suboptimal fading factor lambda_k：

Where tr [ □ ] represents the traces of the matrix.

4. The inertial navigation initial alignment method based on the ISTSRCKF of claim 1, wherein: the iterative CKF algorithm in step 4:

step 4.1.1: using the formulas in step 2.4 to calculate the one-step predicted value of the stateAnd a stepPrediction error covariance matrix P_k/k-1；

P_k/k-1＝S_k/k-1S^T _k/k-1

Calculate the cubage point (i ═ 1,2, …, n + 1; m ═ 2(n + 1)):

propagating the Cufoundation point through the observation equation:

Z_i,k/k-1＝h(X_i,k/k-1)

estimating an observation predicted value at the k moment:

estimating an autocorrelation covariance matrix:

estimating cross-correlation covariance matrix:

calculating the fading factor lambda according to the step 3_k；

step 4.2.1: the measurement is updated byAnd P_k/k-1An iterative process of initial values. Let the estimate and variance of the ith iteration beAnd

in which ξ_jRepresents the jth volume point;

step 4.2.3: with P in step 4.1.3_k/k-1Recalculating the formulas in step 4.1.2 to obtain P_zz,k/k-1、P_xz,k/k-1；

Calculate the state and variance estimates for the ith iteration: