Method for estimating parameters of extended kalman filter for offshore engineering structures based on virtual observation of attitude angles

Abstract

The application discloses a kind of marine engineering structure extension Kalman filter parameter estimation methods based on attitude angle virtual observation, it is related to marine engineering technical field, including the following steps: S1, initialization system process noise covariance matrix and observation noise covariance matrix, obtain the extension Kalman filtering attitude angle estimation value;S2, establish high-precision angular velocity-attitude angle dynamic conversion model, generate attitude angle virtual observation and be used for online evaluation Kalman filtering result;S3, set multidimensional performance evaluation index system, and the amplitude error of the extension Kalman filtering estimated attitude angle and trend stability are quantitatively evaluated;S4, based on the error measurement of attitude angle virtual observation and multidimensional performance evaluation index, the proportional factor of system process noise covariance matrix and observation noise covariance matrix is adjusted using gradient descent algorithm, and it is real-time iteration;To solve the problem that prior art cannot accurately measure and real-time predict attitude parameter.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of ocean engineering, and more particularly, it relates to a method for estimating parameters of an extended Kalman filter for an ocean engineering structure based on virtual observation of attitude angles. BACKGROUND

[0002] Ocean engineering structures such as semi-submersible platforms, floating wind turbines, and tension leg platforms are long-term served in the random external load environment of wind, wave, and current coupling. Small changes in the attitude angles (pitch angle, roll angle, and heading angle) of the ocean engineering structures will have a significant impact on deck operation safety, mooring line tension distribution, and structural fatigue life. Therefore, accurate measurement and real-time prediction of these attitude parameters are crucial to ensuring the safe operation of ocean engineering structures.

[0003] To obtain the attitude information of the ocean engineering structure, a lightweight inertial measurement unit (IMU) composed of an accelerometer, a gyroscope, and a magnetometer is usually used, and multi-source sensor data is processed by information fusion. Specifically, under static or low dynamic conditions, the projection of the gravity vector measured by the accelerometer in the carrier coordinate system can be used to solve the pitch angle and roll angle; the geomagnetic field vector measured by the magnetometer and corrected by soft and hard ferromagnetic correction can be used to solve the heading angle; and the instantaneous angular velocity signal output by the gyroscope can be integrated in time to obtain the relative attitude increment.

[0004] However, significant linear acceleration caused by wave excitation, magnetic field distortion caused by complex sea area electromagnetic environment, and inherent zero drift of low-cost gyroscopes all reduce the accuracy of single sensor solution. In engineering practice, Kalman filter and its variant algorithms are widely used to fuse multi-source sensor data by constructing system state equations and observation equations and using a prediction-update mechanism. This method can fully utilize the measurement characteristics of different sensors, effectively suppress measurement noise interference and model error accumulation, and significantly improve the accuracy and accuracy of attitude estimation.

[0005] The Kalman filter can achieve optimal estimation in the sense of minimum mean square error when the noise satisfies the linear Gaussian assumption, and its performance is highly dependent on the accuracy of the noise statistics. Therefore, the reasonable setting of the process noise covariance matrix Q and the observation noise covariance matrix R is the key to ensuring the accuracy of state estimation and the numerical stability of the algorithm.

[0006] In the attitude monitoring scenario of the ocean engineering structure, it is difficult to accurately simulate the time-varying noise characteristics caused by complex factors such as waves and ocean currents in the actual marine environment in the laboratory or simulation environment; in addition, due to the limitations of offshore operation conditions, there is often a lack of high-precision attitude "true value" benchmarks for long-term comparison, making it difficult for the process noise covariance matrix Q and the observation noise covariance matrix R obtained by traditional offline calibration to remain effective for a long time.

[0007] The application of traditional Kalman filter algorithm in the attitude estimation of ocean engineering structures faces three key problems: firstly, the Kalman filter algorithm relies on pre-calibrated fixed noise parameters, which cannot adapt to the complex and changeable noise characteristics in the marine environment, resulting in significant fluctuations in estimation accuracy with environmental changes; secondly, due to the limitations of space and cost in the operating water area, it is difficult to configure a high-precision attitude reference for a long time, so there is a lack of objective and continuous performance evaluation means, and the adjustment of Kalman filter parameters often relies on experience; thirdly, the existing adaptive Kalman filter algorithm often uses a feedback mechanism based on innovation sequence when recursively estimating the process noise covariance matrix Q, when the initial state estimation has a deviation, the deviation will be amplified and continuously accumulated in the subsequent update, resulting in continuous degradation of filter performance over time, and under sudden sea conditions or strong interference conditions, the mechanism is also prone to parameter jump, further deteriorating the attitude estimation result. SUMMARY

[0008] In view of the deficiencies in the prior art, the purpose of the present application is to provide an ocean engineering structure extended Kalman filter parameter estimation method based on attitude angle virtual observation, which has the advantages of accuracy and high precision.

[0009] To achieve the above purpose, the present application provides the following technical scheme: an ocean engineering structure extended Kalman filter parameter estimation method based on attitude angle virtual observation, comprising the following steps:

[0010] S1, initializing the system process noise covariance matrix Q and the observation noise covariance matrix R, and realizing the optimal fusion of accelerometer, gyroscope and magnetometer data based on the extended Kalman filter to obtain the extended Kalman filter attitude angle estimation value;

[0011] S2, establishing a high-precision angular velocity-attitude angle dynamic conversion model based on the structure kinematics model, generating attitude angle virtual observation and using it to evaluate the Kalman filter result online;

[0012] S3, setting up a multi-dimensional performance evaluation index system, and quantitatively evaluating the amplitude error and trend stability of the extended Kalman filter estimated attitude angle;

[0013] S4, based on the attitude angle virtual observation of step S2 and the error measurement of the multi-dimensional performance evaluation index of S3, adjusting the proportion factor γ of the system process noise covariance matrix Q and the observation noise covariance matrix R using the gradient descent algorithm in the exponential space, and iterating γ in real time.

[0014] By adopting the technical scheme, the relative attitude angle in a given time interval can be accurately obtained under various complex sea conditions by adopting the angular velocity-attitude angle dynamic conversion model, the cumulative error of the traditional numerical integral method can be effectively avoided, a robust and reliable attitude angle evaluation benchmark is provided for adaptive estimation of process noise and observation noise covariance of the extended Kalman filter, the multi-dimensional performance evaluation index can quantitatively evaluate the amplitude error and trend stability of the estimated attitude angle of the extended Kalman filter, and the estimated attitude angle of the extended Kalman filter is ensured to meet the expected accuracy, and through real-time iterative updating of γ, the change of the statistical characteristics of the sensor noise under complex working conditions can be quickly tracked and compensated, so that the key system parameters of the extended Kalman filter are quickly and stably estimated.

[0015] Further, in step S1, the extended Kalman filter is divided into a prediction stage and an update stage, wherein,

[0016] Prediction stage:

[0017] (1)

[0018] Update stage:

[0019] (2)

[0020] In formula (1), is the prior estimation of the system state at the kth sampling time; is the posterior state estimation at k-1 time; is the control input acting on the system in the interval ; f is a nonlinear dynamic model function of the system; is the prediction mean square error matrix, which recursively depends on the posterior covariance at the previous time and the process noise covariance matrix ; F is the Jacobian matrix of the nonlinear dynamic model function f with respect to the state vector x;

[0021] In formula (2), is the deviation between the actual observation and the model prediction at k time, which is called innovation; is the observation value at k time; h is a nonlinear observation function model, which maps the state space to the measurement space; S k is the innovation covariance matrix at k time; H is the Jacobian matrix of the observation function model h with respect to the state vector x; is the observation noise covariance matrix; is the Kalman gain, used to balance the weight of model prediction and actual observation; is the state estimation mean square error matrix.

[0022] By adopting the above technical solution, the aforementioned quantities together constitute the extended Kalman filter in... The entire process of observation and update.

[0023] Furthermore, in step S1, the system process noise covariance matrix Q is:

[0024] (3)

[0025] In the formula, It is the system process noise covariance matrix at the k-th sampling time; The variance of the isotropic white noise of the three-axis gyroscope; W k The Jacobian matrix is ​​the derivative of the state transition matrix with respect to the angular velocity;

[0026] The observation noise covariance matrix R is:

[0027] (4)

[0028] In the formula, This indicates the level of white noise, which is isotropic across the three axes of the accelerometer. Indicates the level of white noise, which is isotropic across the three axes of the magnetometer. It is a 3×3 identity matrix that reflects the same and complementary noise statistical performance of the sensor in three orthogonal directions.

[0029] By adopting the above technical solution, since the system process noise Q is mainly affected by the noise of the gyroscope in various directions, and the observation noise covariance matrix R mainly reflects the sensor noise characteristics of the accelerometer and magnetometer; after setting the initial values ​​of the process noise covariance matrix Q, the observation noise covariance matrix R and the state estimation mean square error matrix P, the attitude angle estimation can be completed based on the current parameters by calculating according to the iterative calculation formulas (1) and (2) of the extended Kalman filter.

[0030] Furthermore, in step S2, the dynamic conversion model of angular velocity-attitude angle is as follows:

[0031] (5)

[0032] in, ;

[0033] In the formula, Let represent the three-axis angular velocity vector at the k-th sampling time, where , , These correspond to the angular velocities about the x, y, and z axes, respectively. This represents the attitude angle of the marine engineering structure at the k-th sampling time; It is the order of the angular velocity-attitude angle dynamic conversion model; It is the complex amplitude coefficient of the component, used to characterize the amplitude and phase offset of the component; These are the complex characteristic roots of the signal component, used to reflect the oscillation frequency and damping coefficient of that component; and These are the amplitude and initial phase of the p-th mode, respectively; and These are the damping ratio and natural frequency of the p-th mode, respectively; It is the sampling frequency of the signal; This indicates the operation of taking the real part of a complex number; i represents the imaginary unit.

[0034] By adopting the above technical solution, dynamic conversion of angular velocity to attitude angle can be realized when marine engineering structures move at small angles, and the accumulation of errors caused by traditional numerical integration can be effectively suppressed.

[0035] Furthermore, in step S3, the multi-dimensional performance evaluation index system includes amplitude error index and trend error index, wherein,

[0036] The amplitude error index is:

[0037] (6)

[0038] In the formula, It is the attitude angle at time k calculated by the extended Kalman filter; It is the k-time trend component of the attitude angle calculated by the extended Kalman filter; It is the virtual observation value of the attitude angle at time k obtained from the dynamic conversion model of angular velocity and attitude angle;

[0039] The trend error index is:

[0040] (7)

[0041] In the formula, It is the trend component of the attitude angle calculated by the extended Kalman filter; It is the trend component of the attitude angle at time k, obtained by geometric mapping relationship of the three-axis acceleration; It is the trend component of the virtual attitude angle observation value obtained from the angular velocity-attitude angle dynamic conversion model at time k; N is the sample length; k=1,…,N.

[0042] By adopting the above technical solution, the estimation effect of the extended Kalman filter can be quantitatively evaluated from both instantaneous tracking error and long-term trend error, and the output performance of the extended Kalman filter under given parameter conditions can be evaluated more comprehensively.

[0043] Furthermore, in step S4, the gradient descent algorithm is implemented as follows:

[0044] (8)

[0045] (9)

[0046] In the formula, Let be the objective function of the gradient descent algorithm; It is a logarithmic scaling variable; For small perturbations, usually take ; This represents the gradient of the objective function under the current parameters, and is used to guide parameter updates.

[0047] By adopting the above technical solutions, it is possible to efficiently find the globally optimal solution.

[0048] Furthermore, the gradient descent update rule is as follows:

[0049] (10)

[0050] In the formula, η is the learning rate, which controls the step size of each iteration; The maximum change in a single step is usually taken as... ; It is a symbolic function; The gradient of the objective function is calculated at the k-th iteration; The parameter value is for the k-th iteration. The updated parameters; The operator indicates taking the smaller of the learning rate adjustment value and the maximum step size constraint.

[0051] By adopting the above technical solutions, the stability of the parameter optimization process and the real-time performance of the algorithm can be guaranteed.

[0052] Furthermore, the termination condition for the iteration process is set to satisfy any of the following conditions:

[0053] (11)

[0054] (12)

[0055] (13)

[0056] Formula (11) indicates that the rate of change of the logarithmic parameter is less than the threshold. Formula (12) indicates that the change in the evaluation index is less than the threshold. Formula (13) indicates that the number of iterations k reaches its maximum value. .

[0057] By adopting the above technical solution and setting different threshold parameters, different accuracy requirements and optimization speeds can be achieved.

[0058] In summary, the present invention has the following beneficial effects:

[0059] The method of this invention is based on the engineering characteristics of the limited rotation range of marine engineering structures. It establishes a dynamic conversion model of angular velocity-attitude angle based on structural kinematics to realize virtual observation of attitude angle, providing accurate virtual attitude observation for extended Kalman filter estimation of attitude angle. This innovative method avoids the reliance of traditional methods on expensive external reference equipment such as optical motion capture systems or high-precision IMUs for parameter calibration, and is especially suitable for application scenarios such as marine engineering structures that lack external reference true values.

[0060] The method of this invention establishes a multi-dimensional evaluation index system that comprehensively considers the amplitude error and drift term changes of the filter output; this index system provides guidance for the optimization of parameters of the extended Kalman filter system.

[0061] The method of this invention couples virtual observation error with evaluation index into gradient information, and uses a gradient descent strategy to adjust the process noise and observation noise covariance in real time, so as to achieve rapid parameter adaptation. It can effectively cope with the time-varying nature of sensor noise characteristics under complex working conditions, significantly improve the accuracy and robustness of attitude estimation algorithm, and provide a new technical approach for attitude perception in complex environments such as marine engineering structures. Attached Figure Description

[0062] Figure 1 This is the distribution curve of the optimization variable X and the comprehensive evaluation standard J(x) in the method of this invention;

[0063] Figure 2 The results are the Extended Kalman (EKF) calculations and the Optical Motion Capture System (OMCS) measurements after the parameters are determined by this method; where (a) is the time history curve of the entire test process and (b) is the time history curve from the 10th to the 30th second. Detailed Implementation

[0064] The present invention will be further described in detail below with reference to the embodiments.

[0065] The present invention provides a method for estimating the parameters of an extended Kalman filter for marine engineering structures based on virtual observation of attitude angles, comprising the following steps:

[0066] S1. Initialize the system process noise covariance matrix Q and the observation noise covariance matrix R, and achieve optimal fusion of accelerometer, gyroscope and magnetometer data based on the extended Kalman filter to obtain the extended Kalman filter attitude angle estimate.

[0067] An inertial measurement unit, consisting of an accelerometer, gyroscope, and magnetometer, is deployed on the main structure of a marine engineering structure, and a time period T is selected. c (where T) c The acceleration (which should be no less than one complete structural vibration cycle) angular velocity and magnetometer The data sequence.

[0068] Based on the typical system noise values ​​specified in the technical manuals of each sensor. , and Set the initial values ​​for the system process noise covariance matrix Q and the observation noise covariance matrix R in the extended Kalman filter algorithm. and .

[0069] Using a quaternion q as the system variable, angular velocity observations are directly used as control inputs, and acceleration and magnetometer readings are used as observations. Through the prediction-update iterative process of the extended Kalman filter algorithm, the three-axis spatial attitude angles of marine engineering structures are estimated in real time. .

[0070] In the prediction phase:

[0071] (1)

[0072] Update phase:

[0073] (2)

[0074] In equation (1), It is the prior estimate of the system state at the k-th sampling time; It is the posterior state estimate at time k-1; Is The control input acting on the system within the interval; f is the nonlinear dynamic model function of the system; It is the prediction mean squared error matrix, which recursively depends on the posterior covariance of the previous time step. and process noise covariance matrix F is the Jacobian matrix of the nonlinear dynamic model function f with respect to the state vector x;

[0075] In equation (2), The difference between the actual observation and the model prediction at time k is called the innovation. is the observation value at time k; h is the nonlinear observation function model that maps the state space to the measurement space; S k yes kThe information covariance matrix at time t; H is the Jacobian matrix of the observation function model h with respect to the state vector x; It is the observation noise covariance matrix; It is the Kalman gain, used to balance the weights of model predictions and actual observations; It is the mean square error matrix of the state estimation.

[0076] The system process noise covariance matrix Q is:

[0077] (3)

[0078] In the formula, It is the system process noise covariance matrix at the k-th sampling time; The variance of the isotropic white noise of the three-axis gyroscope; W k The Jacobian matrix is ​​the derivative of the state transition matrix with respect to the angular velocity.

[0079] The observation noise covariance matrix R is:

[0080] (4)

[0081] In the formula, This indicates the level of white noise, which is isotropic across the three axes of the accelerometer. I3 represents the white noise level of the magnetometer, which is isotropic across its three axes; I3 is a 3×3 identity matrix that reflects the same and complementary noise statistical performance of the sensor in the three orthogonal directions.

[0082] S2. A high-precision dynamic conversion model of angular velocity-attitude angle is established based on the structural kinematics model, and virtual observations of attitude angle are generated and used for online evaluation of Kalman filter results.

[0083] The dynamic conversion model of angular velocity-attitude angle is as follows:

[0084] (5)

[0085] in, ;

[0086] In the formula, Let represent the three-axis angular velocity vector at the k-th sampling time, where , , These correspond to the angular velocities about the x, y, and z axes, respectively. This represents the attitude angle of the marine engineering structure at the k-th sampling time; It is the order of the angular velocity-attitude angle dynamic conversion model; It is the complex amplitude coefficient of the component, used to characterize the amplitude and phase offset of the component; These are the complex characteristic roots of the signal component, used to reflect the oscillation frequency and damping coefficient of that component; and These are the amplitude and initial phase of the p-th mode, respectively; and These are the damping ratio and natural frequency of the p-th mode, respectively; It is the sampling frequency of the signal; This indicates the operation of taking the real part of a complex number; i represents the imaginary unit.

[0087] For the main direction of motion of marine engineering structures, the roll angle of rotation about the X-axis is used. For example, The structure is obtained by decomposing, analytically summing, and transforming according to formula (5). Relative rotation angle over time As a virtual observation.

[0088] S3. Establish a multi-dimensional performance evaluation index system and quantitatively evaluate the magnitude error and trend stability of the attitude angle estimation by the extended Kalman filter.

[0089] The multi-dimensional performance evaluation index system includes amplitude error index and trend error index, among which,

[0090] The amplitude error index is:

[0091] (6)

[0092] In the formula, It is the attitude angle at time k calculated by the extended Kalman filter; It is the k-time trend component of the attitude angle calculated by the extended Kalman filter; It is the virtual observation value of the attitude angle at time k obtained from the dynamic conversion model of angular velocity and attitude angle;

[0093] The trend error index is:

[0094] (7)

[0095] In the formula, It is the trend component of the attitude angle calculated by the extended Kalman filter; It is the trend component of the attitude angle at time k, obtained by geometric mapping relationship of the three-axis acceleration; It is the trend component of the virtual attitude angle observation value obtained from the angular velocity-attitude angle dynamic conversion model at time k; N is the sample length; k=1,…,N.

[0096] Calculate the amplitude error and trend error indices under the current parameter configuration according to formulas (6) and (7).

[0097] S4. Based on the virtual attitude angle observation in step S2 and the error measurement of the multi-dimensional performance evaluation index in S3, the scaling factor γ of the system process noise covariance matrix Q and the observation noise covariance matrix R is adjusted in the exponential space using the gradient descent algorithm, and γ is iterated in real time.

[0098] Based on the scale-invariant property of Kalman filtering, this method reduces the original multivariate optimization problem to adjusting the relative weights of the process noise covariance matrix Q and the observation noise covariance matrix R: Let the standard deviations of the gyroscope, accelerometer, and magnetometer noises be respectively... The standard deviation of gyroscope noise Normalized to a constant of 1; then, for the estimation of pitch and roll angles, a logarithmic scaling variable is introduced. As the sole independent variable for optimization; for the heading angle, then... The same weight adjustment is performed; by searching for the optimal solution in the one-dimensional parameter x space, the optimal trade-off between Q and R can be achieved while ensuring computational efficiency, thereby significantly improving the estimation accuracy and robustness of the extended Kalman filter in each attitude channel.

[0099] To account for the contributions of different metrics to system performance, this method introduces the following root mean square evaluation metrics:

[0100] (8)

[0101] In the formula, Let be the objective function of the gradient descent algorithm. Indicates the amplitude error index; Indicator of trend error;

[0102] The gradient descent algorithm is implemented as follows:

[0103] (9)

[0104] In the formula, Let be the objective function of the gradient descent algorithm; It is a logarithmic scaling variable; For small perturbations, usually take ; This represents the gradient of the objective function with the current parameters, used to guide parameter updates. The gradient descent update rule is:

[0105] (10)

[0106] In the formula, η is the learning rate, which controls the step size of each iteration; The maximum change in a single step is usually taken as... ; It is a symbolic function; The gradient of the objective function is calculated at the k-th iteration; The parameter value is for the k-th iteration. The updated parameters; The operator indicates taking the smaller of the learning rate adjustment value and the maximum step size constraint.

[0107] Perform gradient descent iterations according to formulas (8), (9), and (10) until convergence to the globally optimal parameter configuration. This makes the evaluation function J(x) reach its minimum value.

[0108] The following experiment using a floating pontoon in a water tank serves as an example to verify the method of this invention. The experimental setup is as follows: an inertial measurement unit (IMU) is fixed near the center of mass of the pontoon, and optical reflection markers are affixed to the IMU's outer shell; simultaneously, a high-precision optical motion capture system is deployed around the perimeter of the water tank. Both systems synchronously acquire the transient attitude information of the pontoon under wave excitation using a unified time base, providing a consistent data source for subsequent algorithm evaluation and comparison.

[0109] Process the data according to steps S1-S4, record the iteration process of parameter x, and obtain the distribution curve of the optimization variable x and the comprehensive evaluation standard J(x), as follows. Figure 1 As shown in the figure, the pitch angle calculated using the extended Kalman filter parameters determined by this method is compared with the measured results of the optical motion capture system. The results are as follows. Figure 2 As shown, this fully demonstrates the accuracy and reliability of the proposed method.

[0110] This specific embodiment is merely an explanation of the present invention and is not intended to limit the invention. After reading this specification, those skilled in the art can make modifications to this embodiment without contributing any inventive step, but such modifications are protected by patent law as long as they are within the scope of the claims of the present invention.

Claims

1. A method for estimating the parameters of an extended Kalman filter for marine engineering structures based on virtual attitude angle observation, characterized in that, Includes the following steps: S1. Initialize the system process noise covariance matrix Q and the observation noise covariance matrix R, and achieve optimal fusion of accelerometer, gyroscope and magnetometer data based on the extended Kalman filter to obtain the extended Kalman filter attitude angle estimate. S2. Based on the structural kinematics model, a high-precision dynamic conversion model of angular velocity-attitude angle is established to generate virtual observations of attitude angles and use them to evaluate the Kalman filter results online. The dynamic conversion model of angular velocity-attitude angle is as follows: ， in, ; In the formula, Let represent the three-axis angular velocity vector at the k-th sampling time, where , , These correspond to the angular velocities about the x, y, and z axes, respectively. This represents the attitude angle of the marine engineering structure at the k-th sampling time; It is the order of the angular velocity-attitude angle dynamic conversion model; It is the complex amplitude coefficient of the component, used to characterize the amplitude and phase offset of the component; These are the complex characteristic roots of the signal component, used to reflect the oscillation frequency and damping coefficient of that component; and These are the amplitude and initial phase of the p-th mode, respectively; and These are the damping ratio and natural frequency of the p-th mode, respectively; It is the sampling frequency of the signal; This indicates the operation of taking the real part of a complex number; i represents the imaginary unit; S3. Set up a multi-dimensional performance evaluation index system and quantitatively evaluate the magnitude error and trend stability of the attitude angle estimation by the extended Kalman filter. S4. Based on the virtual observation of attitude angle in step S2 and the error measurement of multi-dimensional performance evaluation index in S3, the gradient descent algorithm is used to adjust the scaling factor γ of the system process noise covariance matrix Q and the observation noise covariance matrix R in the exponential space, and γ is iterated in real time. The gradient descent algorithm is implemented as follows: （8）， （9）， In the formula, Let be the objective function of the gradient descent algorithm; It is a logarithmic scaling variable; For small perturbations, usually take ; This represents the gradient of the objective function under the current parameters, used to guide parameter updates; The gradient descent update rule is as follows: （10）， In the formula, η is the learning rate, which controls the step size of each iteration; The maximum change in a single step is usually taken as... ; It is a symbolic function; The gradient of the objective function is calculated at the k-th iteration; The parameter value is for the k-th iteration. The updated parameters; The operator indicates taking the smaller of the learning rate adjustment value and the maximum step size constraint.

2. The method according to claim 1, characterized in that, In step S1, the extended Kalman filter is divided into a prediction phase and an update phase. The prediction phase includes: ， Update phase: ， In equation (1), It is the prior estimate of the system state at the k-th sampling time; It is the posterior state estimate at time k-1; Is The control input acting on the system within the interval; f is the nonlinear dynamic model function of the system; It is the prediction mean squared error matrix, which recursively depends on the posterior covariance of the previous time step. and process noise covariance matrix F is the Jacobian matrix of the nonlinear dynamic model function f with respect to the state vector x; In equation (2), The difference between the actual observation and the model prediction at time k is called the innovation. It is the observation value at time k; h is the nonlinear observation function model that maps the state space to the measurement space; S k H is the information covariance matrix at time k; H is the Jacobian matrix of the observation function model h with respect to the state vector x. It is the observation noise covariance matrix; It is the Kalman gain, used to balance the weights of model predictions and actual observations; It is the mean square error matrix of the state estimation.

3. The method according to claim 1, characterized in that, In step S1, the system process noise covariance matrix Q is: ， In the formula, It is the system process noise covariance matrix at the k-th sampling time; The variance of the isotropic white noise of the three-axis gyroscope; W k The Jacobian matrix is ​​the derivative of the state transition matrix with respect to the angular velocity; The observation noise covariance matrix R is: ， In the formula, This indicates the level of white noise, which is isotropic across the three axes of the accelerometer. I3 represents the white noise level of the magnetometer, which is isotropic across its three axes; I3 is a 3×3 identity matrix that reflects the same and complementary noise statistical performance of the sensor in the three orthogonal directions.

4. The method according to claim 1, characterized in that, In step S3, the multi-dimensional performance evaluation index system includes amplitude error index and trend error index, among which, The amplitude error index is: ， In the formula, It is the attitude angle calculated by the extended Kalman filter at the k-th sampling time; It is the time-trend component of the attitude angle calculated by the extended Kalman filter at the k-th sampling time; It is the virtual observation value of the attitude angle at the kth sampling moment obtained from the angular velocity-attitude angle dynamic conversion model; The trend error index is: ， In the formula, It is the trend component of the attitude angle calculated by the extended Kalman filter; It is the trend component of the attitude angle at time k, obtained by geometric mapping relationship of the three-axis acceleration; It is the trend component of the virtual attitude angle observation value obtained from the angular velocity-attitude angle dynamic conversion model at time k; N is the sample length; k=1,…,N.

5. The method according to claim 1, characterized in that, The termination condition for the iterative process is set to satisfy any of the following conditions: （11）， （12）， （13）， Formula (11) indicates that the rate of change of the logarithmic parameter is less than the threshold. Formula (12) indicates that the change in the evaluation index is less than the threshold. Formula (13) indicates that the number of iterations k reaches its maximum value. .

Images