SINS / USBL integrated navigation method based on chi-square detection

Abstract

The invention provides an SINS (strapdown inertial navigation system) / USBL (universal serial bus) integrated navigation method based on chi-square detection, aiming at the problem that the precision of an integrated navigation system is reduced due to the fact that a large number of outliers are accompanied in measurement information of a USBL system in an underwater complex noise environment. The invention designs a navigation method for assisting an SINS / USBL (strapdown inertial navigation system / universal serial bus) combined system by utilizing chi-square detection and a least square support vector regression (LSSVR) algorithm. The method comprises the following steps: firstly, establishing a state equation and a measurement equation of an SINS / USBL tightly integrated navigation system; then, judging an outlier interference condition by utilizing chi-square detection, if relatively large outlier interference exists, outputting predicted USBL position information by adopting the trained LSSVR model, and participating in integrated navigation calculation; and if the larger outlier does not exist, carrying out integrated navigation calculation by adopting a robust filtering algorithm based on Huber M estimation. According to the method, the influence of the USBL outlier on the integrated navigation performance is effectively solved, and the positioning precision in the underwater noise environment is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of underwater combined navigation, and in particular relates to a SINS / USBL combined navigation method based on chi-square detection. Background Art

[0002] With the exploration and development of the ocean by humans, underwater vehicles are developing rapidly, so the demand for underwater high-precision navigation technology is becoming more and more urgent. Among the existing underwater positioning technologies, the strapdown inertial navigation system (SINS) is widely used in underwater vehicles because of its strong autonomy, good concealment, high accuracy in a short time, fast update frequency and comprehensive navigation information. However, its positioning error will accumulate over time and requires auxiliary correction by other navigation systems. Since sound waves have a long transmission distance and small attenuation loss underwater, hydroacoustic positioning technology has been widely used. Commonly used hydroacoustic baseline positioning methods include long baseline, short baseline, ultra-short baseline (USBL), etc. Among them, ultra-short baseline is widely used because of its small array size, flexible use and easy installation. Since the inertial navigation system and the ultra-short baseline positioning system can assist and complement each other in principle and application, the research on the combined navigation system with inertial navigation as the core and supplemented by ultra-short baseline positioning technology has also become an important development direction of underwater vehicle navigation.

[0003] The received signals of USBL hydrophones in the marine environment may contain various noises. Under the influence of these underwater noises, the measurement information of the USBL system will be accompanied by a large number of wild values, which will reduce the filtering performance of the integrated navigation system, and the navigation results will contain uncontrollable errors. In severe cases, it may even cause the SINS / USBL integrated navigation to fail.

[0004] Technical comparison with patent CN202210796815.X "A GPS / AOA / SINS integrated navigation method based on federated Kalman filtering"

[0005] Patent CN202210796815.X adds a fault detection and fault handling module between the federated Kalman subfilter and the main filter of the GPS / AOA / SINS combined navigation system. When an abnormality is detected, the gain matrix of the fault subfilter is adaptively adjusted by the ratio of the threshold value and the detection function in the residual chi-square detection, and the degree of trust in the measured value and the state value is reasonably allocated. The federated Kalman filter result after fault handling is further fused with the time update value of the fault subfilter to suppress the positioning divergence caused by abnormal values. The present invention uses chi-square detection to determine whether there are wild values ​​in the USBL measurement information in the SINS / USBL combined navigation system. If there are a large number of measured wild values, the USBL position information predicted by the LSSVR model output is used for combined navigation calculation to improve the positioning accuracy of the combined navigation system in an underwater noise environment.

[0006] Technical comparison with patent CN202410028635.6 "A SINS / DVL collaborative navigation method based on distance mode"

[0007] Patent CN202410028635.6 establishes a factor graph optimal estimation model to globally estimate the state of the AUV at the current moment and outputs the optimization result after multi-source information fusion. The present invention uses Kalman filtering as a framework to achieve data fusion, and uses chi-square detection to determine whether there are measurement outliers, combining huber M estimation and LSSVR model prediction to eliminate the impact of outliers on the performance of the integrated navigation system. Summary of the invention

[0008] Aiming at the problem that there are a large number of outliers in USBL measurement information in the combined navigation system, the present invention proposes a SINS / USBL combined navigation method based on chi-square detection, which utilizes chi-square detection and least squares support vector regression (LSSVR) algorithm to assist the navigation of the SINS / USBL combined system to improve the navigation accuracy.

[0009] To achieve the above object, the technical solution adopted by the present invention is:

[0010] A SINS / USBL combined navigation method based on chi-square detection is specifically as follows:

[0011] (1) Establish the state equation of the SINS / USBL integrated navigation system;

[0012] (2) Establish the measurement equations of the SINS / USBL integrated navigation system;

[0013] (3) Using normal data to train the LSSVR model: When the USBL data is normal, the position calculated by SINS and the position output by USBL are used to train the LSSVR model to obtain the LSSVR model parameters;

[0014] (4) Combined navigation filter calculation based on chi-square detection: Based on the system equations established in steps (1) to (3) and the trained LSSVR model, combined with chi-square detection, the combined navigation system filter calculation is performed.

[0015] As a further improvement of the present invention, in step (1), the state equation of the SINS / USBL integrated navigation system is established:

[0016] 1) Establish the SINS state equation;

[0017] Select the local geographic coordinate system East-North-Sky as the navigation coordinate system, and take the attitude error [φ E φ N φ U ], speed error [δV E δV N δV U ], position error [δL δλ δh], gyro constant drift [ε x ε y ε z ], Accelerometer bias As the state variable of SINS;

[0018] Then the state equation of SINS is expressed as:

[0019]

[0020] in, G SINS is the system noise allocation matrix, W SINS is the system noise matrix, F SINS is the state transfer matrix, which can be obtained according to the SINS error equation;

[0021] 2) Establish the USBL state equation;

[0022] Select the installation error angle [δθ x δθ y δθ z ], the constant slope range error δD of USBL is taken as the state variable of USBL;

[0023] Then the state equation of USBL is expressed as:

[0024]

[0025] Among them, X USBL =[δθ x δθ y δθz δD] T ;

[0026] Since the installation error angle and slope distance error of the system are small quantities and are considered constants, F USBL (t) = 0 4×4 ;

[0027] 3) Establish the state equation of SINS / USBL integrated navigation system;

[0028] Combining the SINS state equation and the USBL state equation, the state equation of the integrated navigation system is obtained as follows:

[0029]

[0030] As a further improvement of the present invention, in step (2), the measurement equation of the SINS / USBL integrated navigation system established is specifically:

[0031] In the tight combination mode, the azimuth and slant range measured by USBL are subtracted from the azimuth and slant range calculated from the SINS position and the transponder position, and the measurement equation is as follows:

[0032]

[0033] in,

[0034] H4=[0 0 1] T ;

[0035] α USBL , β USBL 、r USBL Expressed as the azimuth and slant range measured by USBL, α SINS , β SINS 、r SINS represents the azimuth and slant range of the transponder relative to the SINS in the acoustic array coordinate system calculated using the SINS position and the transponder position. Δα, Δβ, and Δt represent the azimuth error and delay error. c is the speed of sound. represents the installation error matrix between the carrier coordinate system and the acoustic array coordinate system, Represents the coordinate transformation matrix from the navigation coordinate system to the carrier coordinate system, Represents the coordinate transformation matrix from the earth coordinate system to the navigation coordinate system. Represents the arm between the center of the USBL sound head and the center of the SINS, is the relative position vector of the transponder relative to SINS in the earth system, expressed as:

[0036]

[0037] Among them, R N is the radius of curvature of the ellipse, e is the eccentricity of the ellipse, [L t λ t h t ] is the position of the transponder, [L s λ s h s ] is the position of the inertial navigation output;

[0038] D e It is expressed as:

[0039]

[0040] Where [L λ h] represents longitude, latitude and altitude respectively;

[0041] B s Expressed as

[0042]

[0043] A s It is expressed as:

[0044]

[0045] Among them, [x tu y tu z tu ] represents the relative position of the transponder in the acoustic array coordinate system, and r represents the slant range.

[0046] As a further improvement of the present invention, in step (3), the LSSVR model is trained using normal data, specifically:

[0047] Given a training sample set S = {(x i ,y i ),i=1,2,…,N}, where x i is the input quantity, y i is the output, N is the number of data points in the sample set, and support vector regression SVR transforms the nonlinear optimal estimation problem of the original sample space into a linear function estimation problem in the high-dimensional feature space through the high-dimensional mapping of the sample space by nonlinear functions. The regression equation is:

[0048]

[0049] Among them, w is the weight vector, b is the bias term, It is a nonlinear function that maps the input vector to a higher-dimensional feature space to ensure that the sample data can be linearly regressed;

[0050] The LSSVR algorithm controls the empirical risk of the model by taking the sum of squares of the approximation error, and its regression estimation problem is converted into an optimization problem:

[0051]

[0052] Among them, μ is the regularization parameter;

[0053] In order to solve the above optimization problem, the corresponding Lagrange equation is established to transform the constrained optimization problem into an unconstrained optimization problem;

[0054]

[0055] Among them, α and α i is the Lagrange multiplier;

[0056] According to the Karush-Kuhn-Tucker condition, the Lagrange equation is solved and the analytical equation of f(x) is obtained as follows:

[0057]

[0058] Among them, K(x i ,x) is the kernel function;

[0059] Selecting the radial basis function as the kernel function, we can get the prediction function of LSSVR for the model:

[0060]

[0061] Among them, σ is the width of the kernel function.

[0062] As a further improvement of the present invention, in step (4), based on the system equations established in steps (1)-(3) and the trained LSSVR model, combined with chi-square detection, the combined navigation system filtering calculation is performed, specifically:

[0063] The state equation and measurement equation of the SINS / USBL integrated navigation system established in step (1) and step (2) are discretized, and then the discretized state equation and measurement equation are calculated. The specific steps of the algorithm process are as follows:

[0064] 1) One-step prediction of Kalman filter state;

[0065]

[0066] 2) Kalman filter state one-step prediction mean square error matrix;

[0067]

[0068] 3) Calculate the chi-square test statistic λk ;

[0069] The measurement information is expressed as:

[0070]

[0071] When no fault occurs, r k is zero-mean Gaussian white noise, and its theoretical variance is:

[0072]

[0073] Then construct the chi-square test statistic λ k for:

[0074]

[0075] Among them, λ k is subject to the degree of freedom m, where m is the dimension of the quantity measured 2 distributed;

[0076] 4) Determine λ k and T D relation;

[0077] Given a preset wild value threshold T D , through λ k Compare it to determine whether outliers exist;

[0078] (a) When λ k >T D When , it is determined that there is a chi-square value exceeding the large wild value threshold, and the USBL position information predicted by the LSSVR model trained in step (3) is used to participate in the combined navigation calculation;

[0079] (b) When λ k ≤T D When , it is determined that there is no chi-square value exceeding the large outlier threshold, and the robust filtering algorithm based on Huber M estimation is used for integrated navigation calculation;

[0080] The specific steps are as follows:

[0081] First, the system state equation and measurement equation are written in the following form:

[0082]

[0083] in,

[0084]

[0085] Define the following variables:

[0086]

[0087] Then, the pseudo measurement equation is constructed as:

[0088] Y k =A k X k +ξ k

[0089] At this time, k The following conditions must be met:

[0090]

[0091] The transformed pseudo-measurement equation has normalized noise characteristics;

[0092] Using the M-estimation theory, the solution of the above pseudo-measurement equation is transformed into the solution of the Huber cost function;

[0093] From the definition of the cost function, we can know that:

[0094]

[0095] in,

[0096]

[0097] η i,k =Y i,k -A i,k X k

[0098] Y i,k represents the i-th element in the pseudo-measurement value; A i,k represents the i-th row in the matrix; γ is the adjustment factor, and its value is γ=1.345;

[0099] Taking the first-order partial derivative of the cost function and setting it equal to zero gives:

[0100]

[0101] in,

[0102]

[0103] make Then there is

[0104]

[0105] The weight matrix is ​​defined as Ψ = diag[ψ(η i,k )], and η i,k =Y i,k -A i,k X kSubstituting into the calculation formula of the first-order partial derivative of the cost function, we get the following matrix form equation

[0106]

[0107] From the above formula, the state of robust filtering estimation based on Huber M is:

[0108]

[0109] Beneficial effects: The present invention adopts the above technical solution and has the following beneficial effects:

[0110] Aiming at the problem that USBL measurement information in complex underwater noise environment is prone to outlier interference, the chi-square test is used to classify the outlier interference. In the case of large outlier interference, the trained LSSVR model is used to output the predicted USBL position information for correcting SINS. For the case with less outlier interference, the robust filtering algorithm based on Huber M estimation is used for integrated navigation calculation, which effectively solves the influence of USBL outliers on the integrated navigation performance and improves the positioning accuracy in underwater noise environment. BRIEF DESCRIPTION OF THE DRAWINGS

[0111] Figure 1 This is the flow chart of the SINS / USBL combined navigation algorithm based on chi-square detection. DETAILED DESCRIPTION

[0112] The present invention is further described in detail below in conjunction with the accompanying drawings and specific embodiments:

[0113] Aiming at the problem that USBL measurement information in a complex underwater noise environment is prone to wild value interference, thus affecting the accuracy of the integrated navigation system, the present invention designs a SINS / USBL integrated navigation method based on chi-square detection. The specific method is as follows:

[0114] Step 1: Establish the state equation of the SINS / USBL integrated navigation system, specifically:

[0115] (1) Establish the SINS state equation.

[0116] Select the local geographic coordinate system (east-north-sky) as the navigation coordinate system and take the attitude error [φ E φ N φ U ], speed error [δV E δV N δV U ], position error [δL δλ δh], gyro constant drift [ε x ε y ε z], Accelerometer bias As the state variable of SINS.

[0117] Then the state equation of SINS can be expressed as:

[0118]

[0119] in,

[0120] G SINS is the system noise allocation matrix, W SINS is the system noise matrix, F SINS is the state transfer matrix, which can be obtained according to the SINS error equation.

[0121] (2) Establish the USBL state equation.

[0122] Select the installation error angle [δθ x δθ y δθ z ] and the constant slope range error δD of USBL are taken as the state variables of USBL.

[0123] Then the state equation of USBL can be expressed as:

[0124]

[0125] Among them, X USBL =[δθ x δθ y δθ z δD] T .

[0126] Since the system installation error angle and slope distance error are small, they can be regarded as constants. USBL (t) = 0 4×4 .

[0127] (3) Establish the state equation of the SINS / USBL combined navigation system.

[0128] By combining the SINS state equation and the USBL state equation, the state equation of the integrated navigation system can be obtained as follows:

[0129]

[0130] Step 2: Establish the measurement equation of the SINS / USBL integrated navigation system, specifically:

[0131] The SINS / USBL integrated navigation system of the present invention adopts a tight combination mode, and the azimuth and slant range measured by USBL are subtracted from the azimuth and slant range calculated from the SINS position and the transponder position to obtain the following measurement equation:

[0132]

[0133] in,

[0134] H4=[0 0 1] T .

[0135] α USBL , γ USBL 、r USBL Expressed as the azimuth and slant range measured by USBL, α SINS , β SINS 、r SINS represents the azimuth and slant range of the transponder relative to the SINS in the acoustic array coordinate system calculated using the SINS position and the transponder position. Δα, Δβ, and Δt represent the azimuth error and delay error, and c is the speed of sound. represents the installation error matrix between the carrier coordinate system and the acoustic array coordinate system, Represents the coordinate transformation matrix from the navigation coordinate system to the carrier coordinate system, Represents the coordinate transformation matrix from the earth coordinate system to the navigation coordinate system. Represents the arm between the center of the USBL sound head and the center of the SINS, is the relative position vector of the transponder relative to SINS in the earth system, expressed as:

[0136]

[0137] Among them, R N is the radius of curvature of the ellipse, e is the eccentricity of the ellipse, [L t λ t h t ] is the position of the transponder, [L s λ s h s ] is the position of the inertial navigation output.

[0138] D e It is expressed as:

[0139]

[0140] Where [L λ h] represents longitude, latitude and altitude respectively.

[0141] B s Expressed as

[0142]

[0143] A s It is expressed as:

[0144]

[0145] Among them, [x tu y tu z tu ] represents the relative position of the transponder in the acoustic array coordinate system. r represents the slant range.

[0146] Step 3: Use normal data to train the LSSVR model, specifically:

[0147] Given a training sample set S = {(x i ,y i ),i=1,2,…,N}, where x i is the input quantity, y i is the output, and N is the number of data points in the sample set. Support vector regression (SVR) transforms the nonlinear optimal estimation problem of the original sample space into a linear function estimation problem of the high-dimensional feature space through the high-dimensional mapping of the sample space by nonlinear functions. Its regression equation is:

[0148]

[0149] Among them, w is the weight vector and b is the bias term. It is a nonlinear function that maps the input vector to a higher-dimensional feature space to ensure that the sample data can be linearly regressed.

[0150] The LSSVR algorithm controls the empirical risk of the model by taking the sum of squares of the approximation error. The regression estimation problem can be converted into an optimization problem:

[0151]

[0152] Among them, μ is the regularization parameter.

[0153] In order to solve the above optimization problem, the corresponding Lagrange equation is established to transform the constrained optimization problem into an unconstrained optimization problem.

[0154]

[0155] Among them, α and α i is the Lagrange multiplier.

[0156] According to the Karush-Kuhn-Tucker condition, the Lagrange equation is solved and the analytical equation of f(x) is obtained as follows:

[0157]

[0158] Among them, K(x i ,x) is the kernel function.

[0159] The present invention selects radial basis function as kernel function, and obtains the prediction function of LSSVR for the model:

[0160]

[0161] Among them, σ is the width of the kernel function.

[0162] Step 4: Combined navigation filter calculation based on chi-square detection, specifically:

[0163] The state equation and measurement equation of the SINS / USBL integrated navigation system established in steps 1 and 2 are discretized, and then the discretized state equation and measurement equation are calculated using the algorithm proposed by the present invention. The algorithm process is as follows: Figure 1 The specific steps of the process shown are:

[0164] (1) One-step prediction of Kalman filter state.

[0165]

[0166] (2) Kalman filter state one-step prediction mean square error matrix.

[0167]

[0168] (3) Calculate the chi-square test statistic λ k .

[0169] The measurement innovation can be expressed as:

[0170]

[0171] When no fault occurs, r k is zero-mean Gaussian white noise, and its theoretical variance is:

[0172]

[0173] Then we can construct the chi-square test statistic λ k for:

[0174]

[0175] Among them, λ kis a χ with m degrees of freedom (m is the dimension of the quantity measured) 2 distributed.

[0176] (4) Determine λ k and T D relation.

[0177] Given a preset wild value threshold T D , through λ k Compare it to determine whether an outlier exists.

[0178] (a) When λ k >T D When , it is determined that there is a chi-square value exceeding the large wild value threshold, and the USBL position information predicted by the LSSVR model trained in step (3) is used to participate in the combined navigation calculation;

[0179] (b) When λ k ≤T D When , it is determined that there is no chi-square value exceeding the large outlier threshold, and the robust filtering algorithm based on Huber M estimation is used for integrated navigation calculation;

[0180] First, the system state equation and measurement equation are written in the following form:

[0181]

[0182] in,

[0183]

[0184] Define the following variables:

[0185]

[0186] Then, the pseudo measurement equation can be constructed as:

[0187] Y k =A k X k +ξ k

[0188] At this time, k The following conditions must be met:

[0189]

[0190] It can be seen from this that the transformed pseudo-measurement equation has normalized noise characteristics.

[0191] Using the M-estimation theory, the solution to the above pseudo-measurement equation can be converted into the solution to the Huber cost function.

[0192] From the definition of the cost function, we can know that:

[0193]

[0194] in,

[0195]

[0196] η i,k =Y i,k -A i,k X k

[0197] Y i,k represents the i-th element in the pseudo-measurement value; A i,k represents the i-th row in the matrix; γ is the adjustment factor, which is generally taken as γ=1.345.

[0198] Taking the first-order partial derivative of the cost function and setting it equal to zero gives:

[0199]

[0200] in,

[0201]

[0202] make Then there is

[0203]

[0204] The weight matrix is ​​defined as Ψ = diag[ψ(η i,k )], and η i,k =Y i,k -A i,k X k Substituting into the calculation formula of the first-order partial derivative of the cost function, we can get the following matrix form equation

[0205]

[0206] From the above formula, the state of robust filtering estimation based on Huber M is:

[0207]

[0208] The above description is only a preferred embodiment of the present invention and does not constitute any other form of limitation to the present invention. Any modification or equivalent change made based on the technical essence of the present invention still falls within the scope of protection required by the present invention.

Claims

1. A S INS / USBL combined navigation method based on chi-square detection, characterized in that: Specifically: (1) Establish the state equation of the SINS / USBL integrated navigation system; (2) Establish the measurement equations of the SINS / USBL integrated navigation system; (3) Using normal data to train the LSSVR model: When the USBL data is normal, the position calculated by SINS and the position output by USBL are used to train the LSSVR model to obtain the LSSVR model parameters; (4) Combined navigation filter calculation based on chi-square detection: Based on the system equation established in step (1)-step (1) and the trained LSSVR model, combined with chi-square detection, the combined navigation system filter calculation is performed.

2. A S INS / USBL integrated navigation method based on chi-square detection, characterized in that: In the step (1), the state equation of the SINS / USBL integrated navigation system is established: 1) Establish the SINS state equation; Select the local geographic coordinate system East-North-Sky as the navigation coordinate system, and take the attitude error [φ E φ N φ U ], speed error [δV E δV N δV U ], position error [δL δλ δh], gyro constant drift [ε x ε y ε z ], Accelerometer bias As the state variable of SINS; Then the state equation of SINS is expressed as: in, G SINs is the system noise allocation matrix, W SINs is the system noise matrix, F SINS is the state transfer matrix, which can be obtained according to the SINS error equation; 2) Establish the USBL state equation; Select the installation error angle [δθ x δθ y δθ z ], the constant slope range error δD of USBL is taken as the state variable of USBL; Then the state equation of USBL is expressed as: Among them, X USBL =[δθ x dth y dth z [D] T ; Since the installation error angle and slope distance error of the system are small quantities and are considered constants, F USBL (t) = 0 4×4 ; 3) Establish the state equation of SINS / USBL integrated navigation system. Combining the SINS state equation and the USBL state equation, the state equation of the integrated navigation system is obtained as follows:

3. The S INS / USBL integrated navigation method based on chi-square detection according to claim 1, characterized in that: In step (2), the measurement equation of the SINS / USBL integrated navigation system is established as follows: In the tight combination mode, the azimuth and slant range measured by USBL are subtracted from the azimuth and slant range calculated from the SINS position and the transponder position, and the measurement equation is as follows: in, H4＝[0 0 1] T ； α USBL , β USBL 、r USBL Expressed as the azimuth and slant range measured by USBL, α SINS , β SINS 、r SINS represents the azimuth and slant range of the transponder relative to the SINS in the acoustic array coordinate system calculated using the SINS position and the transponder position. Δα, Δβ, and Δt represent the azimuth error and delay error. c is the speed of sound. represents the installation error matrix between the carrier coordinate system and the acoustic array coordinate system, Represents the coordinate transformation matrix from the navigation coordinate system to the carrier coordinate system, Represents the coordinate transformation matrix from the earth coordinate system to the navigation coordinate system. Represents the arm between the center of the USBL sound head and the center of the SINS, is the relative position vector of the transponder relative to SINS in the earth system, expressed as: Among them, R N is the radius of curvature of the ellipse, e is the eccentricity of the ellipse, [L t λ t h t ] is the position of the transponder, [L s λ s h s ] is the position of the inertial navigation output; D e It is expressed as: Where [L λ h] represents longitude, latitude and altitude respectively; B s Expressed as A s It is expressed as: Among them, [x tu y tu z tu ] represents the relative position of the transponder in the acoustic array coordinate system, and r represents the slant range.

4. The S INS / USBL integrated navigation method based on chi-square detection according to claim 1, characterized in that: In step (3), the LSSVR model is trained using normal data, specifically: Given a training sample set S = {(x i ,y i ), i = 1, 2, ..., N}, where x i is the input quantity, y i is the output, N is the number of data points in the sample set, and support vector regression SVR transforms the nonlinear optimal estimation problem of the original sample space into a linear function estimation problem in the high-dimensional feature space through the high-dimensional mapping of the sample space by nonlinear functions. The regression equation is: Among them, w is the weight vector, b is the bias term, It is a nonlinear function that maps the input vector to a higher-dimensional feature space to ensure that the sample data can be linearly regressed; The LSSVR algorithm controls the empirical risk of the model by taking the sum of squares of the approximation error, and its regression estimation problem is converted into an optimization problem: Among them, μ is the regularization parameter; In order to solve the above optimization problem, the corresponding Lagrange equation is established to transform the constrained optimization problem into an unconstrained optimization problem; Among them, α and α i is the Lagrange multiplier; According to the Karush-Kuhn-Tucker condition, the Lagrange equation is solved and the analytical equation of T(x) is obtained as follows: Among them, K(x i , x) is the kernel function; Selecting the radial basis function as the kernel function, we can get the prediction function of LSSVR for the model: Among them, σ is the width of the kernel function.

5. The S INS / USBL integrated navigation method based on Chi-square detection according to claim 1, characterized in that: In step (4), based on the system equations established in steps (1)-(3) and the trained LSSVR model, combined with the chi-square test, the combined navigation system filtering calculation is performed, specifically: The state equation and measurement equation of the SINS / USBL integrated navigation system established in step (1) and step (2) are discretized, and then the discretized state equation and measurement equation are calculated. The specific steps of the algorithm process are as follows: 1) One-step prediction of Kalman filter state; 2) Kalman filter state one-step prediction mean square error matrix; 3) Calculate the chi-square test statistic λ k ; The measurement information is expressed as: When no fault occurs, r k is zero-mean Gaussian white noise, and its theoretical variance is: Then construct the chi-square test statistic λ k for: Among them, λ k is subject to the degree of freedom m, where m is the dimension of the quantity measured 2 distributed; 4) Determine λ k and T D relation; Given a preset wild value threshold T D , through λ k Compare it to determine whether outliers exist; First, the system state equation and measurement equation are written in the following form: in, Define the following variables: Then, the pseudo measurement equation is constructed as: Y k =A k X k +ξ k At this time, k The following conditions must be met: The transformed pseudo-measurement equation has normalized noise characteristics; Using the M-estimation theory, the solution of the above pseudo-measurement equation is transformed into the solution of the Huber cost function; From the definition of the cost function, we can know that: in, or i，k =Y i，k -A i，k X k Y i，k represents the i-th element in the pseudo-measurement value; A i，k represents the i-th row in the matrix; γ is the adjustment factor, and its value is γ=1.345; Taking the first-order partial derivative of the cost function and setting it equal to zero gives: in, make Then there is The weight matrix is ​​defined as Ψ = diag[ψ(η i，k )], and η i，k =Y i，k -A i，k X k Substituting into the calculation formula of the first-order partial derivative of the cost function, we get the following matrix form equation From the above formula, the state of robust filtering estimation based on Huber M is:

Images