Improved unscented Kalman filter long baseline positioning method based on variable forgetting factor

Abstract

The invention discloses an improved unscented Kalman filter long baseline positioning method based on a variable forgetting factor, belongs to the technical field of aircraft measurement, and particularly relates to the improved unscented Kalman filter long baseline positioning method based on the variable forgetting factor. The objective of the invention is to solve the problem of low positioning accuracy of an existing long-baseline underwater acoustic positioning method. According to the invention, a forgetting factor valuing method based on recursive least squares is deduced, so that the forgetting factor can realize self-adaptive valuing in unscented Kalman filtering prediction updating iteration, and the adaptability and updating efficiency of the algorithm for an actual positioning problem are improved; and compared with an original adaptive unscented Kalman filter long baseline positioning algorithm, the positioning accuracy is improved. Meanwhile, the problem of reference value of the combined positioning system for current data and historical data is solved by adding the self-adaptive forgetting factor, the memory length of the unscented Kalman filter is limited in a self-adaptive mode, accumulative errors occurring in the calculation process are reduced, and the positioning accuracy is improved.

Description

Technical Field

[0001] The invention belongs to the technical field of aircraft measurement, and in particular relates to an unscented Kalman filter long baseline positioning method based on a variable forgetting factor improvement. Background Art

[0002] In recent years, underwater vehicle measurement technology has gradually become a hot topic. The research on this technology is crucial to the development of marine military development, marine resource development, marine environmental monitoring and other fields. Autonomous underwater vehicle (AUV), a mission controller that integrates artificial intelligence and other advanced computing technologies, is an important tool in the current field of vehicle measurement technology. With its high-precision navigation and positioning system, it not only successfully obtains effective operation information, but also improves the safety factor of operation and the success rate of return. At present, the high-precision navigation and positioning system of AUV mainly relies on the integrated hydroacoustic / inertial navigation and positioning technology. Among them, hydroacoustic positioning technology can be divided into three types according to the baseline length: long baseline positioning (LBL), short baseline positioning (SBL) and ultra-short baseline positioning (USBL).

[0003] The long baseline positioning system generally deploys three to four primitives (hydroacoustic transponders) with a spacing of hundreds of meters or thousands of meters in the ocean. When the target moves within the range of the array, the propagation distance can be calculated by the propagation delay between the acoustic signal emitted by the AUV and each primitive, and then combined with the spherical intersection model solution, the least squares algorithm is used to estimate the target position with high accuracy. However, the biggest problem with this method is that the position and time information when the target emits the acoustic signal are often inconsistent with the position and time when each primitive receives the acoustic signal, which greatly reduces the positioning timeliness and accuracy of the positioning system. In response to this, people propose to use Kalman filtering (KF) combined with the long baseline positioning system to update the AUV position information in real time, estimate the system error, and improve the positioning accuracy of the linear system. For nonlinear models, the extended Kalman filter (EKF) is used. However, the extended Kalman filter uses the first-order Taylor expansion to discard high-order terms and approximate nonlinear functions as linear functions. Long-term prediction update iterations will produce huge cumulative errors, which seriously affect the accuracy of the positioning system. At the same time, the Jacobian matrix calculation in the algorithm requires the equation to be differentiable, which increases the complexity of the algorithm to a certain extent.

[0004] In response to the above problems, people proposed a combined positioning and navigation system of unscented Kalman filter (UKF) combined with long baseline positioning (LBL). The core idea of ​​unscented Kalman filter is to obtain a set of discrete sigma sampling points that approximate the distribution of system state through UT transformation, perform nonlinear transformation on these sampling points, solve the mean and covariance after transformation, and then estimate the system state. This combined system not only avoids the cumulative error caused by discarding high-order quantities, but also reduces the amount of algorithm calculation and realizes high-precision and effective nonlinear filtering. On this basis, people proposed an adaptive unscented Kalman filter positioning algorithm (AUKF). On the basis of the original unscented Kalman filter, a forgetting factor is introduced to limit the memory length of the unscented Kalman filter, and the process noise covariance matrix and the measurement noise covariance matrix are updated in real time to further reduce the errors in the calculation process and make the positioning results closer to the actual situation.

[0005] In summary, although the combined positioning and navigation system of unscented Kalman filter (UKF) and long baseline positioning (LBL) has achieved certain results, the introduction of forgetting factor technology still needs to be further strengthened and improved. Summary of the invention

[0006] The purpose of the present invention is to solve the problem of low positioning accuracy of the existing long baseline hydroacoustic positioning method, and to propose an unscented Kalman filter long baseline positioning method based on a variable forgetting factor improvement.

[0007] An improved unscented Kalman filter long baseline positioning method based on variable forgetting factor has the following specific process:

[0008] Step 1: Initialize the nonlinear algorithm model;

[0009] Step 2: Obtain the position information in the water depth direction through the pressure sensor installed on the AUV;

[0010] Step 3: Calculate the initial position of the AUV based on the initial measurement data of the long baseline positioning (LBL) system;

[0011] Step 4: According to the initial position of the AUV, sigma point sampling is performed through UT transformation to obtain the prior distribution of the AUV position information;

[0012] Step 5: Calculate the weight of the sigma point;

[0013] Step 6: Predict the state mean of the sigma point at the next moment and covariance;

[0014] Step 7: The speed and acceleration of the AUV at time k are measured by the Doppler velocimeter carried by the AUV, thereby constructing the observation matrix H;

[0015] Step 8: Calculate the observation value based on the observation matrix H and discrete sigma sampling points Based on the observed values ​​and the weights calculated in step 5, solve for the weighted observed mean

[0016] Step 9: Based on Observations and the observed mean Calculate the process noise covariance P Q,k|k -1 ;

[0017] The state mean based on the sigma sampling point at the next moment k and the motion state of the sigma point at the next moment Calculate the observation noise covariance P R,k|k-1 ;

[0018] Step 10: Based on process noise covariance P Q,k|k-1 and the observation noise covariance P R,k|k-1 , calculate the Kalman gain;

[0019] Step 11: Kalman gain K obtained based on step 10 k , the state mean of the sigma sampling point at the next moment k Weighted observed mean Update system status X;

[0020] Based on the Kalman gain K obtained in step 10 k , observation noise covariance P R,k|k-1 , update the covariance P;

[0021] Step 12: Based on the actual observation data Z directly obtained from the sensor or other equipment at the current time k k , the weighted observed mean Calculate the residual E k ;

[0022] Step 13: Introduce the forgetting factor λ and make - adaptively change by recursive least squares method;

[0023] Step 14: Calculate the adjustment parameter Δd according to the forgetting factor λ, and adjust the mathematical parameter Δr based on the adjustment parameter Δd;

[0024] Step 15: Dynamically adjust the process noise covariance matrix Q and the observation noise covariance matrix R according to Δd, and return to step 4 until the required positioning time is reached.

[0025] The beneficial effects of the present invention are:

[0026] The purpose of the present invention is to further improve the positioning performance of a combined positioning and navigation system of an unscented Kalman filter (UKF) combined with a long baseline positioning (LBL) after introducing a forgetting factor, and to propose an improved unscented Kalman filter long baseline positioning method based on a variable forgetting factor.

[0027] The present invention derives a forgetting factor value method based on recursive least squares, so that the forgetting factor can also be adaptively valued in the unscented Kalman filter prediction update iteration, improving the adaptability and update efficiency of the algorithm for actual positioning problems, and improving the positioning accuracy compared to the original adaptive unscented Kalman filter long baseline positioning algorithm. At the same time, the addition of the adaptive forgetting factor further solves the reference value problem of the combined positioning system for current data and historical data. Compared with the constant forgetting factor method, the method of the present invention adaptively limits the memory length of the unscented Kalman filter, reduces the cumulative error in the calculation process, makes the positioning result closer to the actual situation, and improves the positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 is a flow chart of the method of the present invention;

[0029] Figure 2 Schematic diagram of the layout of the long baseline positioning system based on buoys. DETAILED DESCRIPTION

[0030] Specific implementation method 1: This implementation method is based on a variable forgetting factor to improve the unscented Kalman filter long baseline positioning method. The specific process is:

[0031] Step 1: Initialize the nonlinear algorithm model;

[0032] Step 2: Obtain the position information in the water depth direction (z direction information) through the pressure sensor installed on the AUV;

[0033] Convert the AUV position information from three dimensions to two dimensions;

[0034] Step 3: Use the initial measurement data of the LBL system based on the long baseline positioning (x, y, z coordinate values, where the x-axis is parallel to the water surface, the y-axis is perpendicular to the x-axis and parallel to the water surface, and the z-axis is perpendicular to the xy plane and points to the seabed, such as Figure 2 ;), calculate the initial position of AUV;

[0035] Step 4: According to the initial position of the AUV, sigma point sampling is performed through UT transformation to obtain the prior distribution of the AUV position information;

[0036] Step 5: Calculate the weight of the sigma point;

[0037] Step 6: Predict the state mean of the sigma point at the next moment and covariance;

[0038] Step 7: The speed and acceleration of the AUV at time k are measured by the Doppler velocimeter carried by the AUV, thereby constructing the observation matrix H;

[0039] Step 8: Calculate the observation value based on the observation matrix H and discrete sigma sampling points Based on the observed values ​​and the weights calculated in step 5, solve for the weighted observed mean

[0040] Step 9: Based on Observations and the observed mean Calculate the process noise covariance P Q,k|k-1 ;

[0041] The state mean based on the sigma sampling point at the next moment k and the motion state of the sigma point at the next moment Calculate the observation noise covariance P R,k|k-1 ;

[0042] Step 10: Based on process noise covariance P Q,k|k-1 and the observation noise covariance P R,k|k-1 , calculate the Kalman gain;

[0043] Step 11: Kalman gain K obtained based on step 10 k , the state mean of the sigma sampling point at the next moment k Weighted observed mean Update system status X (the most important part of the present invention);

[0044] Based on the Kalman gain K obtained in step 10 k , observation noise covariance P R,k|k-1 , update the covariance P;

[0045] Step 12: Based on the actual observation data Z directly obtained from the sensor or other equipment at the current time k k , the weighted observed mean Calculate the residual E k ;

[0046] Step 13: Introduce the forgetting factor λ, and make λ adaptively change through the recursive least square method;

[0047] Step 14: Calculate the adjustment parameter Δd according to the forgetting factor λ, and adjust the mathematical parameter Δr based on the adjustment parameter Δd;

[0048] Step 15: Dynamically adjust the process noise covariance matrix Q and the observation noise covariance matrix R according to Δd, and return to step 4 until the required positioning time is reached.

[0049] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the nonlinear algorithm model is initialized in step 1; the specific process is:

[0050] χ k =f(χ k-1 )+w k-1 (1)

[0051] Z k =h(χ k )+v k (2)

[0052] Among them, χ k is the motion state of k at the current moment, Z k is the measured data (predicted observation data) at the current time k, f() is the system state model, h() is the observation model, and w k-1 is the process noise at time k-1, v k is the observation noise at the current moment k; the expectation of w is 0, the covariance is Q, the expectation of v is 0, the covariance is R, and there is no correlation between the two.

[0053] The other steps and parameters are the same as those in the first embodiment.

[0054] Specific implementation method three: This implementation method is different from specific implementation method one or two in that in step three, the initial position of the AUV is calculated based on the initial measurement data of the long baseline positioning LBL system; the specific process is:

[0055] Step 3.1: Measure the propagation delay τ from AUV to element α α ;

[0056] The propagation delay τ from AUV to element β is measured β ;

[0057] The propagation delay τ from AUV to element γ is measured γ ;

[0058] Step 3.2: Long baseline positioning (LBL) system obtains the distance between the target and each primitive through propagation delay; it is expressed as:

[0059] R α =τ α ×c

[0060] R β =τ β ×c

[0061] Rγ =τ γ ×c

[0062] Among them, R α is the distance between the target and the primitive α; R β is the distance between the target and primitive β; R γ is the distance between the target and the primitive γ; c is the speed of sound;

[0063] Step 3. Use the spherical intersection method and combine it with L α ,L β ,L γ The location information of the three transponders (x α ,y α ,z α )、(x β ,y β ,z β )、(x γ ,y γ ,z γ ), calculate the position of AUV (x s ,y s ,z s );

[0064] In the coordinate system, the x-axis is parallel to the water surface, the y-axis is perpendicular to the x-axis and parallel to the water surface, and the z-axis is perpendicular to the xy plane and points to the seabed. Figure 2 ;

[0065] The specific solution process is:

[0066]

[0067] The solution is:

[0068]

[0069]

[0070] zs step 2 is known;

[0071] in,

[0072] δ α is the intermediate variable,

[0073] δ β is the intermediate variable,

[0074] δ γ is the intermediate variable,

[0075] The other steps and parameters are the same as those in the first or second embodiment.

[0076] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that in step 4, according to the initial position of the AUV, sigma point sampling is performed through UT transformation to obtain the prior distribution χ of the AUV position information. i ; The specific process is:

[0077] According to the initial position information of AUV Through UT transformation, 2n+1 sigma sampling points are generated; specifically:

[0078]

[0079] in, is the initial position information of AUV;

[0080] n is the dimension of the state vector; i is the i-th;

[0081] χ is the sigma sampling point, a total of 2n+1, χ 0 is the central value of the sigma sampling point, which is generally the state of the system after the last time step k-1. The remaining 2n sampling points are evenly distributed in χ by the central symmetric sampling method. 0 Nearby;

[0082] λ is a transition parameter used to scale the system state. and κ are parameters used to adjust the distribution of sigma sampling points. The discreteness of the sigma sampling point can be controlled. κ is a small quantity. When the state is a single variable, κ=0, and when the state is a multivariate variable, k=3-n.

[0083] The value of k will also affect the distribution of sampling points. When n=1 univariate state, that is, the state variable has only one dimension, k=0, and the distribution of sigma points is mainly determined by the mean and covariance and will not be subject to additional adjustments; when n>1 multivariate state, that is, the state variable has multiple dimensions, κ=3-n. This value is to better balance the distribution of sigma points in multidimensional space and avoid sigma points being too dispersed or too concentrated in high-dimensional space.

[0084] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0085] Specific implementation method 5: This implementation method is different from any one of specific implementation methods 1 to 4 in that the weight of the sigma point is calculated in step 5; it is expressed as:

[0086]

[0087] in, is the mean weight of the sigma sampling points, is the covariance weight, It is a parameter to adjust the distribution of sigma sampling points.

[0088] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0089] Specific implementation method 6: This implementation method is different from any one of the specific implementation methods 1 to 5 in that the state mean and covariance of the sigma point at the next moment are predicted in step 6; the specific method is as follows:

[0090] Step 61: Use the system state model f() to predict the motion state of the sigma point at the next moment; the expression is:

[0091]

[0092] in, Predict the motion state of the i-th sigma point at time k if the motion state of the i-th sigma point at time k-1 is known; i,k|k-1 is the motion state of the i-th sigma point at time k-1 (corresponding to formula 8);

[0093] F is the state transfer matrix;

[0094] Step 62: According to Predict the state mean and covariance of the sigma sampling point at the next moment k:

[0095]

[0096] in,

[0097] Q k-1 is the k-1 time prediction noise covariance matrix;

[0098] is the state mean of the sigma sampling point at the next moment k;

[0099] P x,k|k-1 is the covariance of the sigma sampling point at the next moment k;

[0100] The superscript T means transpose.

[0101] The other steps and parameters are the same as those in Specific Implementations 1 to 5.

[0102] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that in step 7, the speed and acceleration of the AUV at time k are measured by the Doppler velocimeter carried by the AUV, thereby constructing the observation matrix H; the specific process is:

[0103] Step 2: The motion state x at time k k For X = [x x v x a x y y v y a y z z v z a z ], so the observed value is expressed as Z = [v x a x v y a y v z a z ];

[0104] Among them, x x ,y y ,z z represents the three-axis position of the AUV at time k; (in the coordinate system, the x-axis is parallel to the water surface, the y-axis is perpendicular to the x-axis and parallel to the water surface, and the z-axis is perpendicular to the xy plane and points to the seabed. Figure 2 ;)

[0105] v x ,v y ,v z represents the three-axis velocity of the AUV at time k; (in the coordinate system, the x-axis is parallel to the water surface, the y-axis is perpendicular to the x-axis and parallel to the water surface, and the z-axis is perpendicular to the xy plane and points to the seabed. Figure 2 ;)

[0106] a x ,a y ,a z represents the triaxial acceleration of the AUV at time k; (in the coordinate system, the x-axis is parallel to the water surface, the y-axis is perpendicular to the x-axis and parallel to the water surface, and the z-axis is perpendicular to the xy plane and points to the seabed. Figure 2 ;)

[0107] Since the specific model of the target motion state is not given, that is, the relationship between x, v, a and time step k; the observation matrix H is constructed as:

[0108]

[0109] The rows of the observation matrix H correspond to X = [x x v x a x y y v y a y z z v z a z];

[0110] The columns of the observation matrix H correspond to Z = [v x a x v y a y v z a z ];

[0111] Since the water depth position information obtained by the pressure sensor in step 2 converts the AUV position information from three-dimensional to two-dimensional model, at this time X = [x x v x a x y y v y a y ],Z=[v x a x v y a y ], then the observation matrix H is simplified to:

[0112]

[0113] The rows of the simplified observation matrix H correspond to The value in ;

[0114] The columns of the simplified observation matrix H correspond to Z = [v x a x v y a y ] in the .

[0115] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0116] Specific implementation eight: This implementation differs from any one of specific implementations one to seven in that in step eight, the observation values ​​are calculated according to the observation matrix H and the discrete sigma sampling points, and the weighted observation mean is solved based on the observation values ​​and the weights calculated in step five; the specific process is:

[0117] Step 81: Calculate the observed values ​​of discrete sigma points

[0118]

[0119] Among them, H() is the observation matrix; h() is the observation model; is the observed value; i,k|k-1 is the prior distribution of step 4;

[0120] Step 82: Based on the observed values and the weights calculated in step 5 Solve for the weighted observation mean

[0121]

[0122] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0123] Specific implementation method 9: This implementation method is different from any one of the specific implementation methods 1 to 8 in that the step 9 is based on the observed value and the observed mean Calculate the process noise covariance P Q,k|k-1 ;

[0124] The state mean at the next moment k based on the sigma sampling point and For the motion state of the sigma point at the next moment, calculate the observation noise covariance P R,k|k-1 ;

[0125] The specific process is:

[0126]

[0127] Among them, R k-1 is the observation noise covariance matrix at time k-1;

[0128] The step 10 is based on the process noise covariance P Q,k|k-1 and the observation noise covariance P R,k|k-1 , calculate the Kalman gain; the specific process is:

[0129] Calculate the Kalman gain K of the infinite Kalman filter at the current time step k The specific process is:

[0130]

[0131] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0132] Specific implementation method 10: This implementation method is different from any one of specific implementation methods 1 to 9 in that the Kalman gain K obtained in step 10 in step 11 is k , the state mean of the sigma sampling point at the next moment k Weighted observed mean Update system status X;

[0133] Based on the Kalman gain K obtained in step 10 k , observation noise covariance P R,k|k-1 , update the covariance P;

[0134] The specific process is:

[0135]

[0136] Among them, Z k It is the actual observation data directly obtained from sensors or other devices at the current k moment;

[0137] In step 12, the actual observation data Z directly obtained from the sensor or other equipment at the current k moment is used. k , the weighted observed mean Calculate the residual E k ; expressed as:

[0138]

[0139] Among them, Δr is an adjustment parameter that can change over time and can dynamically adjust the size of the residual; E k is the residual at time k;

[0140] In step thirteen, based on the residual E k , introduce the forgetting factor λ, and make λ adaptively change through the recursive least squares method; the specific process is:

[0141]

[0142] Among them, E i is the residual at time i, i = 1, 2, …, k;

[0143] E need is the residual allowed by the algorithm model, i.e., the threshold;

[0144] round() is the integer function;

[0145] is the sensitivity index calculated based on the residual at the current k moment;

[0146] λ min is the minimum value of the forgetting factor, λ max is the maximum value of the forgetting factor, and the value range of the forgetting factor λ is [0.95, 0.995];

[0147] λ k+1 is the forgetting factor at time k+1;

[0148] h is the sensitivity coefficient that controls the degree of change of the forgetting factor λ, which is generally taken as 0.99.

[0149] The adaptive change rule of the forgetting factor is:

[0150] like A larger value indicates a larger residual, that is, a larger difference between the actual observation data directly obtained from the sensor or other device and the updated observation mean calculated by the model, and the forgetting factor λ will be closer to λ min , the model depends more on the current data; on the contrary Smaller means smaller residual, and the forgetting factor λ will be closer to λ max , the model relies more on historical data.

[0151] In step 14, the adjustment parameter Δd is calculated according to the forgetting factor λ, and the mathematical parameter Δr is adjusted based on the adjustment parameter Δd. The specific process is:

[0152]

[0153] Among them, λ k is the forgetting factor at time k;

[0154] Δd is an adjustment parameter that can dynamically adjust the prediction noise covariance Q and the observation noise covariance R;

[0155] In step 15, the process noise covariance matrix Q and the observation noise covariance matrix R are dynamically adjusted according to Δd; the specific process is:

[0156]

[0157] Among them, Q k is the process noise covariance matrix at time k; Q k-1 is the process noise covariance matrix at time k-1; E k is the residual at time k; K k is the Kalman gain at time k; P is the covariance (formula 19); R k is the observation noise coordination difference matrix at time k; R k-1 is the observation noise coordination difference matrix at time k-1; P R,k|k-1 is the observation noise covariance.

[0158] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0159] The present invention may also have many other embodiments. Without departing from the spirit and essence of the present invention, those skilled in the art may make various corresponding changes and modifications based on the present invention, but these corresponding changes and modifications should all fall within the scope of protection of the claims attached to the present invention.

Claims

1. An improved unscented Kalman filter long baseline positioning method based on variable forgetting factor, characterized in that: The specific process of the method is: Step 1: Initialize the nonlinear algorithm model; Step 2: Obtain the position information in the water depth direction through the pressure sensor installed on the AUV; Step 3: Calculate the initial position of the AUV based on the initial measurement data of the long baseline positioning (LBL) system; Step 4: According to the initial position of the AUV, sigma point sampling is performed through UT transformation to obtain the prior distribution of the AUV position information; Step 5: Calculate the weight of the sigma point; Step 6: Predict the state mean of the sigma point at the next moment and covariance; Step 7: The speed and acceleration of the AUV at time k are measured by the Doppler velocimeter carried by the AUV, thereby constructing the observation matrix H; Step 8: Calculate the observation value based on the observation matrix H and discrete sigma sampling points Based on the observed values ​​and the weights calculated in step 5, solve for the weighted observed mean Step 9: Based on Observations and the observed mean Calculate the process noise covariance P Q,k|k-1 ; The state mean at the next moment k based on the sigma sampling point and the motion state of the sigma point at the next moment Calculate the observation noise covariance P R,k|k-1 ; Step 10: Based on process noise covariance P Q,k|k-1 and the observation noise covariance P R,k|k-1 , calculate the Kalman gain; Step 11: Kalman gain K obtained based on step 10 k , the state mean of the sigma sampling point at the next moment k Weighted observed mean Update system status X; Based on the Kalman gain K obtained in step 10 k , observation noise covariance P R,k|k-1 , update the covariance P; Step 12: Based on the actual observation data Z directly obtained from the sensor or other equipment at the current time k k , the weighted observed mean Calculate the residual E k ; Step 13: Introduce the forgetting factor λ, and make λ adaptively change through the recursive least square method; Step 14: Calculate the adjustment parameter Δd according to the forgetting factor λ, and adjust the mathematical parameter Δr based on the adjustment parameter Δd; Step 15: Dynamically adjust the process noise covariance matrix Q and the observation noise covariance matrix R according to Δd, and return to step 4 until the required positioning time is reached.

2. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor according to claim 1, characterized in that: In the step 1, the nonlinear algorithm model is initialized; the specific process is: X k =f(χ k-1 )+w k-1 (1)Z k =h(X k )+v k (2) Among them, X k is the motion state of k at the current moment, Z k is the measurement data at the current time k, f() is the system state model, h() is the observation model, and w k-1 is the process noise at time k-1, v k is the observation noise at the current time k.

3. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor improvement according to claim 2 is characterized in that: In step 3, the initial position of the AUV is calculated based on the initial measurement data of the long baseline positioning LBL system; the specific process is: Step 3.1: Measure the propagation delay τ from AUV to element α α ; The propagation delay τ from AUV to element β is measured β ; The propagation delay τ from AUV to element γ is measured γ ; Step 3.2: Long baseline positioning (LBL) system obtains the distance between the target and each primitive through propagation delay; it is expressed as: R α =t α ×c R β =t β ×c R γ =t γ ×c Among them, R α is the distance between the target and the primitive α; R β is the distance between the target and primitive β; R γ is the distance between the target and the primitive γ; c is the speed of sound; Step 33: Combine L α ,L β ,L γ The location information of the three transponders (x α ,y α ,z α )、(x β ,y β ,z β )、(x γ ,y γ ,z γ ), calculate the position of AUV (x s ,y s ,z s ); The specific solution process is: The solution is: in, δ α is the intermediate variable, δ β is the intermediate variable, δ γ is the intermediate variable, 4. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor improvement according to claim 3 is characterized in that: In step 4, according to the initial position of the AUV, UT transformation is performed to perform sigma point sampling to obtain the prior distribution of the AUV position information; the specific process is: According to the initial position information of AUV Through UT transformation, 2n+1 sigma sampling points are generated; specifically: in, is the initial position information of AUV; n is the dimension of the state vector; i is the i-th; χ is the sigma sampling point, a total of 2n+1, χ0 is the central value of the sigma sampling point, which is the state of the system after update at the previous time step k-1, and the remaining 2n sampling points are evenly distributed around χ0 through the central symmetric sampling method; λ is a transition parameter. and κ are parameters used to adjust the distribution of sigma sampling points. When the state is a single variable, κ=0, and when the state is a multivariate variable, κ=3-n.

5. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor according to claim 4 is characterized in that: The weight of the sigma point is calculated in step 5; it is expressed as: in, is the mean weight of the sigma sampling points, is the covariance weight, It is a parameter to adjust the distribution of sigma sampling points.

6. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor improvement according to claim 5, characterized in that: In step 6, the state mean and covariance of the sigma point at the next moment are predicted; the specific method is as follows: Step 61: Use the system state model f() to predict the motion state of the sigma point at the next moment; the expression is: in, Predict the motion state of the i-th sigma point at time k if the motion state of the i-th sigma point at time k-1 is known; i,k|k-1 is the motion state of the i-th sigma point at time k-1; F is the state transfer matrix; Step 62: According to Predict the state mean and covariance of the sigma sampling point at the next moment k: in, Q k-1 is the k-1 time prediction noise covariance matrix; is the state mean of the sigma sampling point at the next moment k; P x,k|k-1 is the covariance of the sigma sampling point at the next moment k; The superscript T means transpose.

7. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor according to claim 6, characterized in that: In step 7, the speed and acceleration of the AUV at time k are measured by the Doppler velocimeter carried by the AUV, thereby constructing the observation matrix H; the specific process is: Step 2: The motion state X at time k k For X = [x x v x a x y y v y a y z z v z a z ], so the observed value is expressed as Z = [v x a x v y a y v z a z ]; Among them, x x ,y y ,z z represents the three-axis position of the AUV at time k; v x ,v y ,v z represents the three-axis velocity of the AUV at time k; a x ,a y ,a z represents the three-axis acceleration of the AUV at time k; Therefore, the observation matrix H is constructed as: Since the water depth position information obtained by the pressure sensor in step 2 converts the AUV position information from three-dimensional to two-dimensional model, at this time X = [x x v x a x y y v y a y ],Z=[v x a x v y a y ], then the observation matrix H is simplified to:

8. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor improvement according to claim 7 is characterized in that: In the step eight, the observation values ​​are calculated according to the observation matrix H and the discrete sigma sampling points, and the weighted observation mean is solved based on the observation values ​​and the weights calculated in step five; The specific process is: Step 81: Calculate the observed values ​​of discrete sigma points Among them, H() is the observation matrix; h() is the observation model; is the observed value; Step 82: Based on the observed values and the weights calculated in step 5 Solve for the weighted observation mean 9. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor improvement according to claim 8, characterized in that: In step nine, based on the observed value and the observed mean Calculate the process noise covariance P Q,k|k-1 ; The state mean at the next moment k based on the sigma sampling point and For the motion state of the sigma point at the next moment, calculate the observation noise covariance P R,k|k-1 ; The specific process is: Among them, R k-1 is the k-1 time observation noise covariance matrix; The step 10 is based on the process noise covariance P Q,k|k-1 and the observation noise covariance P R,k|k-1 , calculate the Kalman gain; the specific process is: Calculate the Kalman gain K of the infinite Kalman filter at the current time step k The specific process is:

10. The method for long baseline positioning based on unscented Kalman filtering with variable forgetting factor improvement according to claim 9, characterized in that: In step 11, the Kalman gain K obtained in step 10 is used as the basis. k , the state mean of the sigma sampling point at the next moment k Weighted observed mean Update system status X; Based on the Kalman gain K obtained in step 10 k , observation noise covariance P R,k|k-1 , update the covariance P; The specific process is: Among them, Z k It is the actual observation data directly obtained from sensors or other devices at the current k moment; In step 12, the actual observation data Z directly obtained from the sensor or other equipment at the current time k is used. k , the weighted observed mean Calculate the residual E k ; expressed as: Among them, Δr is a regulation parameter that can change over time; E k is the residual at time k; In step thirteen, based on the residual E k , introduce the forgetting factor λ, and make λ adaptively change through the recursive least squares method; the specific process is: Among them, E i is the residual at time i, i = 1, 2, …, k; E need is the threshold; round() is the integer function; is the sensitivity index calculated based on the residual at the current k moment; λ min is the minimum value of the forgetting factor, λ max is the maximum value of the forgetting factor, and the value range of the forgetting factor λ is [0.95, 0.995]; λ k+1 is the forgetting factor at time k+1; h is the sensitivity coefficient; In step 14, the adjustment parameter Δd is calculated according to the forgetting factor λ, and the mathematical parameter Δr is adjusted based on the adjustment parameter Δd. The specific process is: Among them, λ k is the forgetting factor at time k; In the step 15, the process noise covariance matrix Q and the observation noise covariance matrix R are dynamically adjusted according to Δd; The specific process is: Among them, Q k is the process noise covariance matrix at time k; Q k-1 is the process noise covariance matrix at time k-1; E k is the residual at time k; K k is the Kalman gain at time k; P is the covariance (formula 19); R k is the observation noise coordination difference matrix at time k; R k-1 is the observation noise coordination difference matrix at time k-1; P R,kk-1 is the observation noise covariance.

Images