CN105806363A - Alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter) - Google Patents

Abstract

The invention discloses an alignment method of an underwater large misalignment angle based on SINS (Strapdown Inertial Navigation System)/DVL (Doppler Velocity Log) of SRQKF (Square-root Quadrature Kalman Filter). The alignment method comprises the following steps: step 1: establishing a nonlinear error model and a nonlinear filtering equation of the SINS under the large misalignment angle; step 2: constructing the SRQKF by utilizing a multivariate Gauss point and coefficient configuration method and a square-root filtering method in Gauss-Hermite quadrature; and step 3: estimating the misalignment angle by utilizing the SRQKF, and correcting a strapdown attitude matrix, thus obtaining accurate strapdown attitude matrix and attitude angle. The alignment method disclosed by the invention has the advantage that the underwater alignment accuracy and alignment speed of the carrier strapdown system are improved.

Description

SINS/DVL underwater large misalignment angle alignment method based on SRQKF

Technical Field

The invention relates to the technical field of strapdown inertial navigation and integrated navigation, in particular to an SINS/DVL underwater large misalignment angle alignment method based on SRQKF.

Background

The initial alignment is related to the working accuracy of the inertial system, and is one of the key technologies in the Strapdown Inertial Navigation System (SINS). Due to the fact that sea conditions are complex, a ship body of the underwater vehicle is generally in a large-amplitude shaking state, and therefore the strapdown inertial navigation system cannot rapidly complete self-alignment and is in a large misalignment angle state for a long time. In this case, the initial alignment at a large misalignment angle with the moving base needs to be completed with the assistance of external information. Underwater vehicles typically accomplish this task with the aid of doppler velocity meters (DVLs).

However, because the hull is always in a large misalignment angle and in a shaking state, the conventional linear error model cannot accurately describe the error propagation characteristics of the SINS, and a more accurate nonlinear error model needs to be established, and the initial alignment of the underwater vehicle is completed by adopting a nonlinear filtering method.

In recent years, various nonlinear filtering methods, namely Extended Kalman Filtering (EKF), Unscented Kalman Filtering (UKF) and volumetric kalman filtering (CKF), are continuously applied, but all of them have some limitations, and the EKF has low precision when the nonlinear characteristic is strong; although the precision of the UKF and the CKF is improved, the higher precision can not be obtained under the complex conditions of a large misalignment angle and the like, and the convergence speed is slower. Therefore, it is significant to provide an SINS/DVL alignment method that has high accuracy and can effectively cope with a large misalignment angle, a moving base, and other complex environments.

Disclosure of Invention

The purpose of the invention is as follows: in order to overcome the defects in the prior art, the invention provides an SINS/DVL underwater large misalignment angle alignment method based on SRQKF, which is characterized by comprising the following steps:

step 1: establishing a nonlinear error model and a nonlinear filtering equation of a Strapdown Inertial Navigation System (SINS) under a large misalignment angle;

step 2: constructing a square root integral Kalman filter SRQKF by utilizing a multivariable Gauss point in Gauss-Hermite integral calculation, a coefficient configuration method and a square root filtering method;

and step 3: and estimating a misalignment angle by utilizing square root integral Kalman filtering SRQKF, and correcting the strapdown attitude matrix to obtain an accurate strapdown attitude matrix and an accurate attitude angle.

Further, the step 1: establishing a nonlinear error model and a nonlinear filtering equation of a Strapdown Inertial Navigation System (SINS) under a large misalignment angle, specifically:

step 1.1: selecting a geographical coordinate system of northeast as a navigation coordinate system n; selecting a carrier coordinate system b with a front right upper coordinate system; n is rotated to b through 3 times of Euler angles, and the three Euler angles are marked as a course angle psi, a longitudinal rocking angle theta and a transverse rocking angle gamma; the attitude matrix between n and b is recorded asTrue attitude angle is notedTrue velocity is notedThe attitude angle solved by the SINS is recorded as:the SINS resolving speed is recorded as:the mathematical platform calculated by SINS is recorded as n 'system, and the attitude matrix between n' system and b system is recorded asn series are successively rotated for 3 times to n' series, and the three rotation angles are respectively phi_u、φ_e、φ_nThey are called Euler platform error angle, memory vectorThe speed error is defined as:the nonlinear error model is then as follows:

wherein,b is the constant error of the lower triaxial gyro,is b is the random error of the lower three-axis gyroscope,is a constant error of the acceleration of the lower three axes,is b is the random error of the lower three-axis acceleration,is the actual output of the accelerometer,to solve for the calculated mathematical platform rotational angular velocity,is the angular velocity of the rotation of the earth,to solve for the calculated rotational angular velocity of the mathematical platform relative to the earth,to correspond toThe calculation error of (2).The inverse matrix of the Euler angle differential equation coefficient matrix is used;is an attitude matrix between n' and n, C_ωAndthe method specifically comprises the following steps:

step 1.2: the nonlinear filtering equation establishment process is as follows:

error in the selected velocityEuler angle platformError angle phi_u、φ_e、φ_n(ii) a Gyro constant error under b systemAnd constant error of accelerometer under bForm a 10-dimensional state vector:

under the shaking baseTo approximate zero, the nonlinear filter state equation can be established as:

andonly the first two-dimensional state is taken,andis a zero mean white noise process. This nonlinear equation of state can be simplified as:taking a sampling period T as a filtering period, a fourth-order Runge-Kutta integration method can be used, and T is discretized by taking the step length as follows:

assume the value x of the selected state vector at the initial time instant₀Is known, and let t₁T + T/2, then the following iterative formula will applyDispersing:

Δx₁＝[f(x,t)+ω(t)]T

Δx₂＝[f(x,t₁)+Δx₁/2+ω(t₁)]T

Δx₃＝[f(x,t₁)+Δx₂/2+ω(t₁)]T

Δx₄＝[f(x,t+T)+Δx₃+ω(t+T)]T

x_k+1＝x_k+(Δx₁+2Δx₂+2Δx₃+Δx₄)/6

the state filtering equation after dispersion is recorded as: x is the number of_k＝f(x_k-1)+ω_k-1

The method comprises the following steps of selecting speed as observed quantity to establish a nonlinear measurement equation:

the east-direction velocity and the north-direction velocity components output by the carrier SINS and the DVL are used as matching information sources, and the established nonlinear measurement equation is as follows:

wherein, v^bThe real speed under b series;the actual speed under b is the speed of DVL output; taking the two dimensions before z as an observed value; v is a zero-mean gaussian white noise process, and the nonlinear filtering measurement equation can be simplified as: z ═ h (x, t) + ν (t);

also uses T asFor the filtering period, and simple discretization is performed by taking T as a step length, and z ═ h (x, T) + ν (T) is discretized into: z is a radical of_k＝h(x_k,t_k)+ν(t_k) And obtaining a discretized measurement equation as follows: z is a radical of_k＝h(x_k)+ν_k。

The following nonlinear filter equation is composed of a state equation and a measurement equation:

further, the step 2: the method comprises the following steps of constructing a square root integral Kalman filter SRQKF by using a multivariable Gauss point in Gauss-Hermite integral calculation, a coefficient configuration method and a square root filtering method, and specifically comprises the following steps:

step 2.1: the multivariate Gauss point and the coefficient configuration method in Gauss-Hermite integration are as follows:

the Gauss-Hermite integration formula is as follows:

wherein x is a random variable, g (x) is a function with respect to x, x_iIs called Gauss point, A_iReferred to as Gauss coefficient, m is the set number of integration points.

Using the formula:

m Gauss points x can be obtained_i(H_m(x) Zero point of) and m Gauss coefficients a_i(ii) a Using the Gauss points and Gauss coefficients, the expected value E (g (x)) for g (x) can be expressed as:

wherein,

let x be n_xThe components of the dimension random vector are independent from each other and obey N (0,1) Gaussian distribution, and the ith Gauss coefficient a of the vector x can be obtained by solving Gauss points and m Gauss coefficients of each component_iAnd Gauss point vector ξ_i：

Wherein,gauss points corresponding to components of the vector x;representing Gauss point vectors consisting of single component Gauss points, in totalA plurality of; a is_ijIs Gauss point ξ_ijA corresponding Gauss coefficient;

step 2.2: the square root integral Kalman filter SRQKF is constructed as follows:

for the established nonlinear filter equation:

in the formula x_kIs a 10-dimensional state vector, and each component is independent and follows Gaussian distribution; z is a radical of_kIs a 2-dimensional observation vector; omega_k-1V and v_kRespectively, process noise and measurement noise satisfy:

in the formula,_ijis a function; q is a variance matrix of the system noise; r is the variance matrix of the measurement noise.

The SRQKF algorithm is as follows:

step 2.2.1: initializing state variablesAnd its mean square error P₀：

Step 2.2.2: the time updating method of the SRQKF comprises the following steps:

when the filtering is performed for the first time (k ═ 1) on the initial variance matrix P₀Cholesky decomposition is carried out to obtain the characteristic square root S of the initial error variance matrix₀：

P₀＝S₀S₀ ^T；

Using S at time k-1_k-1|k-1Calculating the Gaussian integral point X_l,k-1|k-1If filtering is first time, S is used₀Calculating a Gaussian integral point:

calculating Gaussian integral points of nonlinear state function propagation

Computing state one-step prediction

Calculating the characteristic square root S of the one-step prediction variance matrix_k|k-1：

In the formula S_Q,kCholesky decomposition for the system noise variance matrix, namely: expressed as:QR () denotes the QR decomposition of the matrix, S_k-1|k-1And taking an upper triangular array of the R array in QR decomposition.

Step 2.2.3: the measurement updating method of the SRQKF comprises the following steps:

calculating a one-step predicted state Gaussian integral point X_l,k|k-1：

Calculating the Gaussian integral point Z of the propagation of the nonlinear measurement function_l,k|k-1：

Z_l,k|k-1＝h(X_l,k-1|k-1)l＝1,2,...,M.

Calculating one-step predicted measurement values

Calculating the characteristic square root S of the one-step prediction measurement variance matrix_zz,k|k-1：

S_zz,k|k-1＝qr([Z_k|k-1S_R,k])

In the formula S_R,kCholesky decomposition for the observation noise variance matrix, i.e.:Z_k|k-1expressed as:

computing one-step predictive covariance matrix P_xz,k|k-1：

In the formula X_k|k-1Expressed as:

calculating a gain matrix K:

calculating a state update value

Calculating the characteristic square root S of the state variance matrix_k|k：

S_k|k＝qr([X_k|k-1-KZ_k|k-1KS_R,k])。

Further, step 3) estimates a misalignment angle by using square root integral kalman filter SRQKF, and corrects the strapdown attitude matrix to obtain an accurate strapdown attitude matrix and an accurate attitude angle, specifically:

step 3.1: after the SRQKF filter finishes one-time filtering, according to the 10-dimensional state quantity estimated value output by the filter:

computing an attitude matrix between the n' and n systemsThe calculation method is as follows:

step 3.2: based on the attitude matrix between the n' system and the b systemAnd the attitude matrix between the n' system and the n systemCalculating accurate strapdown attitude matrixThe calculation method is as follows:

$C_b^n=C_b^{n^{\′}}{\left(C_n^{n^{\′}}\right)}^T$

step 3.3: according to the strapdown attitude matrixAnd (3) calculating the Euler angle by the following method:

3.4: and if the attitude precision meets the requirement or the alignment time reaches a set value, finishing alignment, otherwise, returning to the operation step 2) to construct a square root integral Kalman filter SRQKF by utilizing a multivariable Gauss point in Gauss-Hermite integral calculation, a coefficient configuration method and a square root filtering method.

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

1. the invention establishes a nonlinear filtering equation aiming at the characteristics of an underwater carrier under the conditions of a large misalignment angle and a movable base, effectively improves the alignment precision of the strap-down inertial navigation system under the underwater complex condition of the carrier by adopting the SRQKF filtering method, reduces the alignment time and ensures that the strap-down inertial navigation system of the carrier provides reliable attitude information.

2. The invention introduces the square root filtering method into the QKF filtering method to form the SRQKF filtering method, thereby avoiding the state variance matrix P during each filtering_kAnd the prediction variance matrix P_k|k-1Cholesky decomposition is carried out, so that the filtering precision and the stability of numerical calculation are effectively improved.

3. The invention provides speed matching information with higher precision by using DVL, which is beneficial to improving the filtering precision and convergence speed of SRQKF, thereby improving the precision and speed of alignment.

Drawings

Fig. 1 is a system scheme diagram of the present invention.

FIG. 2 is a schematic diagram of the alignment of the SINS/DVL underwater large misalignment angle based on SRQKF of the present invention

FIG. 3 is a flow chart of the SRQKF filter of the present invention

FIG. 4 is a diagram of the SINS/DVL underwater large misalignment angle alignment pitch angle error of the present invention.

FIG. 5 is an error diagram of the SINS/DVL underwater large misalignment angle alignment roll angle of the present invention

FIG. 6 is a diagram of the SINS/DVL underwater large misalignment angle alignment course angle error of the present invention.

Detailed Description

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

As shown in fig. 1 and 2, are a system scheme and an alignment schematic of the present invention.

The SINS strapdown settlement module acquires a gyro output value and an accelerometer output value output by an Inertial Measurement Unit (IMU) module to carry out strapdown calculation to obtain information such as an attitude angle, an attitude matrix, speed and position; DVL equipment works underwater and outputs the speed information of a carrier; the information output by the SINS and the DVL is simultaneously input into an SRQKF filter, and the filtering processing of the information is carried out, wherein the filtering process is as follows:

1. selecting a 10-bit state vector according to the characteristics of the SINS/DVL under the condition of a large underwater misalignment angle:

establishing a nonlinear state filtering equation:

after discretization, it is noted as: x is the number of_k＝f(x_k-1)+ω_k-1.

2. Selecting the speed as a measurement value, and establishing a nonlinear measurement filtering equation:

after discretization, it is noted as: z is a radical of_k＝h(x_k)+v_k.

3. The configuration of the multivariable Gauss points and the coefficients thereof is specifically configured as follows:

using the formula:

obtaining Gauss point a corresponding to each component of vector x_ijAnd Gauss coefficient ξ_ij。

Reuse formula:

gauss point a of vector x_iAnd Gauss coefficient ξ_i。

SRQKF state variablesAnd its mean square error P₀The initialization process is as follows:

the time updating method of the SRQKF comprises the following steps:

when first filtered (k 1) is applied to the variance matrix P₀Cholesky decomposition is carried out to obtain the characteristic square root S of the initial error variance matrix₀：

P₀＝S₀S₀ ^T

Using S at time k-1_k-1|k-1Calculating the Gaussian integral point X_l,k-1|k-1If filtering is first time, S is used₀Calculating the Gaussian integral point X_l,k-1|k-1：

Calculating Gaussian integral points of nonlinear state function propagation

Computing state one-step prediction

Calculating the characteristic square root S of the one-step prediction variance matrix_k|k-1：

The measurement updating method of the SRQKF comprises the following steps:

calculating a one-step predicted state Gaussian integral point X_l,k|k-1：

Calculating the Gaussian integral point Z of the propagation of the nonlinear measurement function_l,k|k-1：

Z_l,k|k-1＝h(X_l,k-1|k-1)l＝1,2,...，M.

Calculating one-step predicted measurement values

Calculating the characteristic square root S of the one-step prediction measurement variance matrix_zz,k|k-1：

S_zz,k|k-1＝qr([Z_k|k-1S_R,k])

Computing one-step predictive covariance matrix P_xz,k|k-1：

In the formula X_k|k-1Expressed as:

calculating a gain matrix K:

calculating a state update value

Calculating the characteristic square root S of the state variance matrix_k|k：

S_k|k＝qr([X_k|k-1-KZ_k|k-1KS_R,k])

The flow chart of the SRQKF filter is shown in fig. 3.

7. And performing closed-loop correction of the state to obtain an accurate strapdown attitude matrix and an accurate attitude angle, wherein the specific method comprises the following steps:

according to the 10-dimensional state quantity estimated value output by the SRQKF filter:

computing an attitude matrix between the n' and n systems

Based on the attitude matrix between the n' system and the b systemAnd the attitude matrix between the n' system and the n systemCalculating accurate strapdown attitude matrix

According to the strapdown attitude matrixAnd (3) calculating the Euler angle by the following method:

and if the attitude precision meets the requirement or the alignment time reaches a set value, finishing the alignment, or else, continuing to recur and execute the step 2 and the step 3 until the alignment is finished.

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

1) setting the motion parameters of the underwater vehicle:

the simulation initial moment is that the underwater vehicle is at the position 15m underwater with north latitude of 32 degrees and east longitude of 118 degrees; and the three-axis swinging motion is carried out around the vertical rocking shaft, the horizontal rocking shaft and the course shaft by a sine function, and the swinging amplitude values of the vertical rocking angle theta, the horizontal rocking angle gamma and the course angle psi are respectively: 9 degrees, 12 degrees and 14 degrees, and the swing period is respectively as follows: 16s, 20s, 12s, the initial heading angle is 30 °, and the initial misalignment angle is: Δ θ equals 10 °, Δ γ equals 10 °, Δ ψ equals 20 °; the underwater vehicle makes uniform linear motion at the speed of 5m/s, and the navigation time is 600 s.

2) Parameter setting of the sensor:

the gyro constant drift of the strapdown system of the underwater vehicle is 0.01 degree/h: the random drift of the gyroscope is 0.01 degrees/h: the accelerometer is biased as: 50mg, accelerometer random error: 50mg, DVL output speed error: 0.1 m/s.

3) Parameter setting of SRQKF

Initial conditions for setting the filter are:

P₀＝diag{(0.1m/s)²,(0.1m/s)²,(10°)²,(10°)²,(20°)²,(50mg)²,(50mg)²,(0.01°/h)²,(0.01°/h)²,(0.01°/h)²}

Q＝diag{(50mg)²,(50mg)²,(0.01°/h)²,(0.01°/h)²,(0.01°/h)²,0,0,0,0,0}

R＝diag{(0.1m/s)²,(0.1m/s)²}

the Gauss points of the single component of the QKF state vector are 3, i.e., m is 3, and the corresponding Gauss points and Gauss coefficients are:

ξ₂＝0,

4) and (3) simulation result analysis:

the SINS/DVL underwater large misalignment angle alignment method based on SRQKF provided by the invention is used for carrying out DVL-assisted SINS underwater alignment, and FIGS. 4, 5 and 6 are curves of a pitch angle error delta theta, a roll angle error delta gamma and a course angle error delta psi in the alignment process of the invention, and can find that: after the underwater vehicle sails for 100s, the SRQKF basically converges, and at the moment, the alignment error of the longitudinal and transverse rocking angles is about 0.002 degrees, the alignment error of the transverse and transverse rocking angles is about 0.003 degrees, and the alignment error of the course angles is about 0.5 degrees; analysis shows that after sailing for 500s, the alignment error of the longitudinal rocking angle is within 0.004 degree, the alignment error of the transverse rocking angle is within 0.002 degree, and the alignment error of the heading angle is within 0.02 degree.

The results show that the SINS/DVL underwater large misalignment angle alignment method of the SRQKF can still ensure high alignment precision and high alignment speed under the conditions of large misalignment angle, shaking base and error in DVL speed output.

Although the preferred embodiments of the present invention have been described in detail, the present invention is not limited to the details of the embodiments, and various equivalent modifications can be made within the technical spirit of the present invention, and the scope of the present invention is also within the scope of the present invention.

Claims (4)

1. An SINS/DVL underwater large misalignment angle alignment method based on SRQKF is characterized by comprising the following steps:

step 1: establishing a nonlinear error model and a nonlinear filtering equation of a Strapdown Inertial Navigation System (SINS) under a large misalignment angle;

step 2: constructing a square root integral Kalman filter SRQKF by utilizing a multivariable Gauss point in Gauss-Hermite integral calculation, a coefficient configuration method and a square root filtering method;

and step 3: and estimating a misalignment angle by utilizing square root integral Kalman filtering SRQKF, and correcting the strapdown attitude matrix to obtain an accurate strapdown attitude matrix and an accurate attitude angle.

2. The SRQKF-based SINS/DVL underwater large misalignment angle alignment method of claim 1, wherein the step 1: establishing a nonlinear error model and a nonlinear filtering equation of a Strapdown Inertial Navigation System (SINS) under a large misalignment angle, specifically:

step 1.1: selecting a geographical coordinate system of northeast as a navigation coordinate system n; selecting a carrier coordinate system b with a front right upper coordinate system; n is rotated to b through 3 times of Euler angles, and the three Euler angles are marked as a course angle psi, a longitudinal rocking angle theta and a transverse rocking angle gamma; the attitude matrix between n and b is recorded asTrue attitude angle is notedTrue velocity is notedThe attitude angle solved by the SINS is recorded as:the SINS resolving speed is recorded as:the mathematical platform calculated by SINS is recorded as n 'system, and the attitude matrix between n' system and b system is recorded asn series are successively rotated for 3 times to n' series, and the three rotation angles are respectively phi_u、φ_e、φ_nThey are called Euler platform error angle, memory vectorThe speed error is defined as:the nonlinear error model is then as follows:

wherein,b is the constant error of the lower triaxial gyro,is b is the random error of the lower three-axis gyroscope,is a constant error of the acceleration of the lower three axes,is b is the random error of the lower three-axis acceleration,is the actual output of the accelerometer,to solve for the calculated mathematical platform rotational angular velocity,is the angular velocity of the rotation of the earth,to solve for the rotational angular velocity of the mathematical platform relative to the earth,to correspond toThe calculation error of (2).The inverse matrix of the Euler angle differential equation coefficient matrix is used;is an attitude matrix between n' and n, C_ωAndthe method specifically comprises the following steps:

step 1.2: the nonlinear filtering equation establishment process is as follows:

error in the selected velocityEuler angle platform error angle phi_u、φ_e、φ_n(ii) a Gyro constant error under b systemAnd constant error of accelerometer under bForm a 10-dimensional state vector:

under the shaking baseTo approximate zero, the nonlinear filter state equation can be established as:

andonly the first two-dimensional state is taken,andis a zero mean white noise process. This nonlinear equation of state can be simplified as:taking a sampling period T as a filtering period, a fourth-order Runge-Kutta integration method can be used, and T is discretized by taking the step length as follows:

assume the value x of the selected state vector at the initial time instant₀Is known, and let t₁T + T/2, then the following iterative formula will applyDispersing:

Δx₁＝[f(x,t)+ω(t)]T

Δx₂＝[f(x,t₁)+Δx₁/2+ω(t₁)]T

Δx₃＝[f(x,t₁)+Δx₂/2+ω(t₁)]T

Δx₄＝[f(x,t+T)+Δx₃+ω(t+T)]T

x_k+1＝x_k+(Δx₁+2Δx₂+2Δx₃+Δx₄)/6

the state filtering equation after dispersion is recorded as: x is the number of_k＝f(x_k-1)+ω_k-1

The method comprises the following steps of selecting speed as observed quantity to establish a nonlinear measurement equation:

the east-direction velocity and the north-direction velocity components output by the carrier SINS and the DVL are used as matching information sources, and the established nonlinear measurement equation is as follows:

wherein, v^bThe real speed under b series;the actual speed under b is the speed of DVL output; taking the two dimensions before z as an observed value; v is a zero-mean gaussian white noise process, and the nonlinear filtering measurement equation can be simplified as: z ═ h (x, t) + ν (t);

similarly, T is used as a filtering period, and T is used as a step size to perform simple discretization, and z ═ h (x, T) + ν (T) is discretized into: z is a radical of_k＝h(x_k,t_k)+ν(t_k) And obtaining a discretized measurement equation as follows: z is a radical of_k＝h(x_k)+ν_k。

The following nonlinear filter equation is composed of a state equation and a measurement equation:

3. the SRQKF-based SINS/DVL underwater large misalignment angle alignment method of claim 1, wherein the step 2: the method comprises the following steps of constructing a square root integral Kalman filter SRQKF by using a multivariable Gauss point in Gauss-Hermite integral calculation, a coefficient configuration method and a square root filtering method, and specifically comprises the following steps:

step 2.1: the multivariate Gauss point and the coefficient configuration method in Gauss-Hermite integration are as follows:

the Gauss-Hermite integration formula is as follows:

wherein x is a random variable, g (x) is a function with respect to x, x_iIs called Gauss point, A_iReferred to as Gauss coefficient, m is the set number of integration points.

Using the formula:

m Gauss points x can be obtained_i(H_m(x) Zero point of) and m Gauss coefficients a_i(ii) a Using the Gauss points and Gauss coefficients, the expected value E (g (x)) for g (x) can be expressed as:

wherein,

let x be n_xThe components of the dimension random vector are independent from each other and obey N (0,1) Gaussian distribution, and the ith Gauss coefficient a of the vector x can be obtained by solving Gauss points and m Gauss coefficients of each component_iAnd Gauss point vector ξ_i：

Wherein,gauss points corresponding to components of the vector x;representing Gauss point vectors consisting of single component Gauss points, in totalA plurality of; a is_ijIs Gauss point ξ_ijA corresponding Gauss coefficient;

step 2.2: the square root integral Kalman filter SRQKF is constructed as follows:

for the established nonlinear filter equation:

in the formula x_kIs a 10-dimensional state vector, and each component is independent and follows Gaussian distribution; z is a radical of_kIs a 2-dimensional observation vector; omega_k-1V and v_kRespectively, process noise and measurement noise satisfy:

in the formula,_ijis a function; q is a variance matrix of the system noise; r is the variance matrix of the measurement noise.

The SRQKF algorithm is as follows:

step 2.2.1: initializing state variablesAnd its mean square error P₀：

Step 2.2.2: the time updating method of the SRQKF comprises the following steps:

when the filtering is performed for the first time (k ═ 1) on the initial variance matrix P₀Cholesky decomposition is carried out to obtain the characteristic square root S of the initial error variance matrix₀：

P₀＝S₀S₀ ^T；

Using S at time k-1_k-1|k-1Calculating the Gaussian integral point X_l,k-1|k-1If filtering is first time, S is used₀Calculating a Gaussian integral point:

calculating Gaussian integral points of nonlinear state function propagation

Computing state one-step prediction

Calculating the characteristic square root S of the one-step prediction variance matrix_k|k-1：

In the formula S_Q,kCholesky decomposition for the system noise variance matrix, namely:expressed as:

QR () denotes the QR decomposition of the matrix, S_k-1|k-1And taking an upper triangular array of the R array in QR decomposition.

Step 2.2.3: the measurement updating method of the SRQKF comprises the following steps:

calculating a one-step predicted state Gaussian integral point X_l,k|k-1：

Calculating the Gaussian integral point Z of the propagation of the nonlinear measurement function_l,k|k-1：

Z_l,k|k-1＝h(X_l,k-1|k-1)l＝1,2,...,M.

Calculating one-step predicted measurement values

Calculating the characteristic square root S of the one-step prediction measurement variance matrix_zz,k|k-1：

S_zz,k|k-1＝qr([Z_k|k-1S_R,k])

In the formula S_R,kCholesky decomposition for the observation noise variance matrix, i.e.:Z_k|k-1expressed as:

computing one-step predictive covariance matrix P_xz,k|k-1：

In the formula X_k|k-1Expressed as:

calculating a gain matrix K:

calculating a state update value

Calculating the characteristic square root S of the state variance matrix_k|k：

S_k|k＝qr([X_k|k-1-KZ_k|k-1KS_R,k])。

4. The method for aligning the SINS/DVL underwater large misalignment angle based on the SRQKF as claimed in claim 1, wherein the step 3) estimates the misalignment angle by using the square root integral Kalman filter SRQKF, and corrects the strapdown attitude matrix to obtain an accurate strapdown attitude matrix and attitude angle, specifically:

step 3.1: after the SRQKF filter finishes one-time filtering, according to the 10-dimensional state quantity estimated value output by the filter:

computing an attitude matrix between the n' and n systemsThe calculation method is as follows:

step 3.2: based on the attitude matrix between the n' system and the b systemAnd the attitude matrix between the n' system and the n systemCalculating accurate strapdown attitude matrixThe calculation method is as follows:

step 3.3: according to the strapdown attitude matrixAnd (3) calculating the Euler angle by the following method:

3.4: and if the attitude precision meets the requirement or the alignment time reaches a set value, finishing alignment, otherwise, returning to the operation step 2) to construct a square root integral Kalman filter SRQKF by utilizing a multivariable Gauss point in Gauss-Hermite integral calculation, a coefficient configuration method and a square root filtering method.