CN115046551A - Dynamic attitude measurement and calculation method based on MEMS inertial sensor - Google Patents

Abstract

The invention discloses a dynamic attitude measuring and calculating method based on an MEMS sensor, which is characterized in that measurement data of an accelerometer and a gyroscope are collected, errors of the two data are eliminated through a mathematical model, and then an attitude angle is calculated by using a quaternion algorithm, so that dynamic attitude information is obtained. Therefore, the values of the accelerometer and the gyroscope can be compensated more accurately, more accurate attitude information can be obtained by using the more accurate values of the accelerometer and the gyroscope, and the method can be used in a dynamic environment so as to enable a dynamic control system to operate stably.

Description

A dynamic attitude measurement method based on MEMS inertial sensor

technical field

The invention relates to the technical field of MEMS sensing, in particular to a method for dynamic attitude measurement and calculation based on a MEMS sensor.

Background technique

There are many solution algorithms for dynamic pose research in the prior art, in which it is very important to accurately model inertial sensors. The dynamic attitude calculation algorithm needs to consider various errors generated by the sensor, such as installation error, manufacturing error, non-orthogonal error and zero offset error. Due to the characteristics of MEMS sensors, traditional inertial sensors are prone to drift to varying degrees due to temperature and noise. Using only the raw data from the gyroscope and accelerometer will have a large error in the angle measurement, which will have a significant impact on the dynamic attitude solution. Therefore, how to make full use of the advantages of the two sensors, eliminate the interference generated by the sensors, and obtain more practical and accurate data has become one of the key issues studied by many experts and scholars.

SUMMARY OF THE INVENTION

In order to solve the above technical problems, the present invention provides a method for dynamic attitude measurement and acceleration calculation based on a MEMS inertial sensor. First, the measurement data of the accelerometer and the gyroscope are obtained, and then the above two measurement data are applied using the quaternion algorithm. The attitude angle is solved to obtain dynamic attitude information.

The technical scheme of the present invention is as follows: a dynamic attitude measurement method based on a MEMS inertial sensor, wherein the MEMS inertial sensor integrates a three-axis accelerometer and a three-axis gyroscope, and obtains attitude information according to the following steps:

1) Obtain the output data of the three-axis gyroscope and eliminate the error to obtain the angular velocity;

2) Obtain the output data of the three-axis accelerometer and eliminate the error to obtain the acceleration;

3) According to the angular velocity and acceleration obtained in steps 1) and 2), use the quaternion algorithm to calculate the attitude angle.

In step 1), the error model of the three-axis gyroscope is established, as follows:

为陀螺仪误差消除后的测量值，ω_x，ω_y，ω_z为陀螺仪测量原始值，f_x，f_y，f_z为加速度计测量值的比力值，w_x，w_y，w_z为陀螺仪三轴上的零偏漂移误差，l_x，l_y，l_z为刻度因子误差，k_xy，k_xz，k_yx，k_yz，k_zy，k_zx为对应两轴间的安装误差，m_xy，m_xz，m_yx，m_yz，m_zy，m_zx为对应两轴之间的比力误差系数，ε_gx，ε_gy，ε_gz为陀螺仪三轴的随机误差。in,

are the measured values after the gyroscope error has been eliminated, ω _x , ω _y , ω _z are the original values measured by the gyroscope, f _x , f _y , f _z are the specific force values of the accelerometer measured values, w _x , w _y , w _z is the zero offset drift error on the three axes of the gyroscope, l _x , _ly , l _z are the scale factor errors, k _xy , k _xz , k _yx , k _yz , k _zy , k _zx are the installations between the corresponding two axes Errors, m _xy , m _xz , m _yx , m _yz , m _zy , and m _zx are the error coefficients of the specific force between the corresponding two axes, ε _gx , ε _gy , and ε _gz are the random errors of the three axes of the gyroscope.

For the deterministic error coefficient, it can be determined by testing and calibrating the gyroscope: first perform multi-position static measurement to obtain _{Wi, m ij ,} and then perform orthogonal angular rate measurement. Since the sensitive axis is stationary when the gyroscope is running at a constant angular velocity orthogonal to the axis to be measured, the scale factor error can be omitted, so the above error model is simplified into the following formula in this step:

表示互耦误差，由于已知加在陀螺仪上的与待测定轴正交的恒定加速度，所以能求解出k_ij in,

Represents the mutual coupling error. Since the constant acceleration applied to the gyroscope and orthogonal to the axis to be measured is known, k _ij can be solved

For random errors, Allan analysis of variance can be used to estimate and compensate. First, take N consecutive gyroscope data as the total sample, and form a subsample group of the consecutive n sampled data, n<N/2, this subsample group is the data cluster, and then calculate the data samples in each data cluster average of.

Assuming that the output of the gyroscope at time t is Ω(t), the average value of the kth data cluster is:

为时间长度为T的含有n个输出数据的第k个数据簇的平均值。由此可知，第k+1个数据簇的平均值为：in,

is the average value of the kth data cluster with n output data of time length T. It can be seen that the average value of the k+1th data cluster is:

To find the difference between the averages of two adjacent data clusters, we have:

So for a data cluster of time length T, the Allan variance is defined as:

Among them, the meaning of the symbol < > is to average the data in all symbols, then there are:

Substitute the calculated variance value data into the ARMA model to use the autoregressive model, as shown below:

When the time series {x _k } obeys the n-order autoregressive model, the expression is as follows:

x _k =α ₁ x _k-1 +…+α _n x _kn +ε _t

The autocorrelation function is:

In the above formula:

γ _k =E(x _t , x _t+k )

=Ex _t (α ₁ x _t+k-1 +α ₂ x _t+k-1 +…α _n x _t+kn +ε _t+k )

=α ₁ γ _k-1 +α ₂ γ _k-2 +…+α _n γ _kn

ρ _k =ρ _−k , ρ ₀ =1

Yule-Walker equations:

AR(n) transfer function:

Random errors are thus estimated and compensated.

In step 2), the error model of the three-axis accelerometer is established, as follows:

表示加速度计三轴测量值经过误差消除算法后的值，f_x，f_y，f_z表示加速计三轴测量的原始值，F_x，F_y，F_z为加速度计三轴零偏漂移误差，h_x，h_y，h_z表示加速度计三轴刻度因子误差，p_xy，p_yx，p_zx，p_xz，p_yx，p_zy表示加速度计三轴两两之间的安装误差，ε_fx，ε_fy，ε_fz表示加速度计三轴上的随机误差。in,

Represents the three-axis measurement value of the accelerometer after the error elimination algorithm, f _x , f _y , f _z represent the original value of the three-axis measurement of the accelerometer, F _x , F _y , F _z are the three-axis zero offset drift error of the accelerometer , h _x , h _y , h _z represent the error of the three-axis scale factor of the accelerometer, p _xy , p _yx , p _zx , p _xz , p _yx , p _zy represent the installation error between the three axes of the accelerometer, ε _fx , ε _fy , ε _fz represent random errors on the three axes of the accelerometer.

The Kalman filter is used to eliminate errors in the acceleration data. First, Allan variance analysis method is used, assuming that the output of the accelerometer at time t is Ω(t), and T is the time length of the data cluster, then the average value of the kth data cluster is:

It can be seen that the average value of the k+1th data cluster is:

The difference between the mean values of two adjacent data clusters is:

For a data cluster of time length T, the Allan variance is defined as:

In the above formula, n is the number of data samples in the data cluster with time length T, and N is the total number of data samples.

According to IEEE, the quantitative relationship between the Allan variance and the noise power spectral density in the original data is as follows:

Then the Allan variance of the total output of the signal is:

为Allan方差；

为角度随机游走噪声方差；

为速率随机游走噪声方差；

为零偏不稳定性噪声方差；

为速率斜坡噪声方差；

为量化噪声方差。in,

is the Allan variance;

is the angle random walk noise variance;

is the rate random walk noise variance;

zero bias instability noise variance;

is the rate ramp noise variance;

is the quantization noise variance.

The above formula can be rewritten as:

Simplified to have:

Comparing the above formula is:

Set up the Kalman equation of state and measurement equation:

X(k)=AX(k-1)+BW(k)

Z(k)=y(k)+V(k)=CX(k)+V(k)

where X(k) is the n-dimensional state matrix of the system, A is the n×n-dimensional state transition matrix of the system, B is the n×p-dimensional noise input matrix of the system, and W(k) is the p-dimensional process noise matrix of the system ; Z(k) is the m-dimensional state matrix of the system, C is the m×n-dimensional observation matrix, and V(k) is the m-dimensional observation noise matrix.

The process noise and observation noise of the system satisfy:

Among them, Q _k is the paired non-negative definite variance matrix of W(k), R _k is the paired positive definite variance matrix of V(k), δ _ki is the Kronecker-δ function; the recursive Kalman filter formula is:

Among them, R is the measurement noise matrix, its value is the variance of the estimation error of the ARMA model, and Q is the system noise.

Step 3) Substitute the acceleration and angular velocity data after eliminating the error into the quaternion algorithm to calculate the attitude angle, and the quaternion equation is:

That is:

in:

The relationship between quaternion and attitude matrix:

Including:

The relationship between the attitude angle of the carrier and the attitude matrix is obtained:

remember

Get attitude angle:

The fourth-order Runge-Kutta method is used to solve the quaternion differential equation:

The state of the system is:

Z _k =[ω _bx ω _by ω _bz ] ^T

The state equation of the system is:

The observation equation is:

The measurement matrix of the system is obtained as:

The best estimate for the system is:

The attitude angle output is:

Thereby, dynamic pose information is obtained.

Beneficial effects of the present invention:

Dynamic attitude measurement is a very important aspect in the design of high-precision control systems. It needs to measure the angle change very accurately. Since a single axial attitude tilt sensor cannot meet the requirements, we use multi-axis sensors to obtain higher accuracy and precision. And establish an error model, which can more accurately compensate the values of the accelerometer and gyroscope. With more accurate values of the accelerometer and gyroscope, more accurate attitude information can be obtained, and accurate angle values can be obtained through the quaternion algorithm. , and can be used in a dynamic environment, so that the dynamic control system can run stably. Combined with the targeted hardware acceleration processor, the real-time performance of the system is greatly improved to meet the needs of high-speed and low-latency scenarios.

Description of drawings

Fig. 1 is the flow chart of the specific embodiment method of the present invention;

2 is a schematic diagram of the system structure of a specific embodiment of the present invention, wherein: 1 is an accelerometer, 2 is a gyroscope, 3 is an accelerometer error compensation model, 4 is a gyroscope error compensation model, and 5 is a solution algorithm and filter.

FIG. 3 is a comparison diagram of before (A) and after (B) filtering of gyroscope data in a specific embodiment of the present invention.

4 is a random drift curve of a MEMS accelerometer before (A) and after (B) filtering in an embodiment of the present invention.

5 is a comparison diagram of the angle output on the x-axis in the swing experiment of the specific embodiment of the present invention, wherein A is the pitch angle diagram obtained by the existing algorithm, and B is the pitch angle diagram obtained by the method of the present invention.

Fig. 6 is in the rocking experiment of the specific embodiment of the present invention, in the angle output comparison diagram of y-axis, wherein A is the roll angle diagram that the existing algorithm obtains, and B is the roll angle diagram that the method of the present invention obtains.

Detailed ways

The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand the present invention and implement it, but the embodiments are not intended to limit the present invention.

For a MEMS inertial sensor that integrates a three-axis accelerometer and a three-axis gyroscope, as shown in Figure 2, obtain the angular velocity information output by the three-axis gyroscope, and use the AR(n) model to eliminate the three-axis angular velocity error; obtain the three-axis accelerometer For the output acceleration information, Kalman filter is used to eliminate the acceleration data error; the quaternion algorithm is used to solve the attitude angle, and the real-time dynamic attitude value is given.

According to the analysis of the error generation mechanism, the error is divided into deterministic error and random error. Deterministic errors mainly include zero drift, scale factor error, temperature influence factor, installation error and lever arm effect error. Calibration to get its stable model, and then it can be compensated; the random errors mainly include start-up drift, rate random walk, angle random walk and deviation instability, etc. This part of the error is caused by random constants, first-order Markov process and It is composed of white noise, and its main characteristics are contingency and randomness, and there are no rules to follow. The statistical rules can only be obtained by statistical methods, and then processed by filtering methods.

(1) Error elimination of gyroscope

According to the basic principle and structure of MEMS gyroscope, the possible deterministic error is analyzed, and the normal temperature error model is established as follows:

为陀螺仪误差消除后的测量值，ω_x，ω_y，ω_z为陀螺仪测量原始值，f_x，f_y，f_z为加速度计测量值的比力值，w_x，w_y，w_z为陀螺仪三轴上的零偏漂移误差，l_x，l_y，l_z为刻度因子误差，k_xy，k_xz，k_yx，k_yz，k_zy，k_zx为对应两轴间的安装误差，m_xy，m_xz，m_yx，m_yz，m_zy，m_zx为对应两轴之间的比力误差系数，ε_gx，ε_gy，ε_gz为陀螺仪三轴的随机误差。in,

are the measured values after the gyroscope error has been eliminated, ω _x , ω _y , ω _z are the original values measured by the gyroscope, f _x , f _y , f _z are the specific force values of the accelerometer measured values, w _x , w _y , w _z is the zero offset drift error on the three axes of the gyroscope, l _x , _ly , l _z are the scale factor errors, k _xy , k _xz , k _yx , k _yz , k _zy , k _zx are the installations between the corresponding two axes Errors, m _xy , m _xz , m _yx , m _yz , m _zy , and m _zx are the error coefficients of the specific force between the corresponding two axes, ε _gx , ε _gy , and ε _gz are the random errors of the three axes of the gyroscope.

After the establishment of the MEMS inertial sensor error model, in order to determine each error coefficient of the deterministic error in the model, the MEMS inertial sensor needs to be tested and calibrated in order to complete the deterministic error compensation and improve the measurement output accuracy of the MEMS inertial measurement unit. The output error characteristics of inertial sensors, especially the deterministic error characteristics, can be understood through test experiments. The so-called calibration is to compare the output of the device with the known standard information, and determine some factors to make the output and the standard information consistent within a certain range.

First perform multi-position static measurement to obtain W _i , m _ij , and then perform orthogonal angular rate measurement, which refers to the measurement under the motion state of adding a constant angular velocity orthogonal to the axis to be measured to the MEMS gyroscope. When running at a constant angular velocity orthogonal to the axis to be measured, the sensitive axis is stationary, so the scale factor error can be ignored, so the error model can be simplified to the following formula in this step:

表示互耦误差，由于已知加在陀螺仪上的与待测定轴正交的恒定加速度，所以就能求解出k_ij。in

Represents the mutual coupling error. Since the constant acceleration applied to the gyroscope orthogonal to the axis to be measured is known, k _ij can be solved.

The random error of MEMS sensor is the main error source that affects the output accuracy of MEMS sensor. In order to improve the output accuracy of MEMS sensor, a reasonable error model must be established for the random error of MEMS sensor, and then the random error is estimated and compensated.

The processing of MEMS gyroscope data by Allan variance analysis is carried out on the premise that the random part of the gyroscope data is generated by specific noise. In this way, the variance of each noise source can be estimated by using the gyroscope data.

Assuming that N consecutive gyroscope data are taken as the total samples, the data sampling time interval is t ₀ , and the consecutive n (n<N/2) sampled data are formed into a sub-sample group, and this sub-sample group is called a data cluster , and then calculate the mean of the data samples in each data cluster.

Assuming that the output of the gyroscope at time t is Ω(t), the average value of the kth data cluster is:

为时间长度为T的含有n个输出数据的第k个数据簇的平均值。由此可知，第k+1个数据簇的平均值为：in the formula

is the average value of the kth data cluster with n output data of time length T. It can be seen that the average value of the k+1th data cluster is:

To find the difference between the averages of two adjacent data clusters, we have:

So for a data cluster of time length T, the Allan variance is defined as:

The meaning of the symbol <> is to average the data in all symbols, then there are:

From the above definition of Allan variance, it can be seen that as long as the number of data points in the sample is limited, several clusters of fixed data length can be obtained. It can be seen that the above formula is a quantitative estimation formula, and its estimation accuracy depends on each independent cluster. The smaller the data length of the cluster, the higher the estimation accuracy.

The calculated variance value data is substituted into the ARMA model to use the autoregressive model, as shown below:

When the time series {x _k } obeys the n-order autoregressive model, the expression is as follows:

x _k =α ₁ x _k-1 +…+α _n x _kn +ε _t

The autocorrelation function is:

In the above formula:

γ _k =E(x _t , x _t+k )

=Ex _t (α ₁ x _t+k-1 +α ₂ x _t+k-1 +…α _n x _t+kn +ε _t+k )

=α ₁ γ _k-1 +α ₂ γ _k-2 +…+α _n γ _kn

ρ _k =ρ _−k , ρ ₀ =1

Yule-Walker equations:

AR(n) transfer function:

(2) Error compensation of accelerometer

Create the following error model:

表示加速度计三轴测量值经过误差消除算法后的值，f_x，f_y，f_z表示加速计三轴测量的原始值，F_x，F_y，F_z为加速度计三轴零偏漂移误差，h_x，h_y，h_z表示加速度计三轴刻度因子误差，p_xy，p_yx，p_zx，p_xz，p_yz，p_zy表示加速度计三轴两两之间的安装误差，ε_fx，ε_fy，ε_fz表示加速度计三轴上的随机误差。in,

Represents the three-axis measurement value of the accelerometer after the error elimination algorithm, f _x , f _y , f _z represent the original value of the three-axis measurement of the accelerometer, F _x , F _y , F _z are the three-axis zero offset drift error of the accelerometer , h _x , _hy , h _z represent the error of the three-axis scale factor of the accelerometer, p _xy , p _yx , p _zx , p _xz , p _yz , p _zy represent the installation error between the three axes of the accelerometer, ε _fx , ε _fy , ε _fz represent random errors on the three axes of the accelerometer.

The random error of MEMS sensor is the main error source that affects the output accuracy of MEMS sensor. In order to improve the output accuracy of MEMS sensor, a reasonable error model must be established for the random error of MEMS sensor, and then the random error is estimated and compensated.

Using the Allan variance analysis method, assuming that the output of the accelerometer at time t is Ω(t), and T is the time length of the data cluster, the average value of the kth data cluster is:

It can be seen that the average value of the k+1th data cluster is:

The difference between the mean values of two adjacent data clusters is:

For a data cluster of time length T, the Allan variance is defined as:

In the above formula, n is the number of data samples in the data cluster with time length T, and N is the total number of data samples.

According to IEEE, the quantitative relationship between the Allan variance and the noise power spectral density in the original data is as follows:

Then the Allan variance of the total output of the signal is:

为惯性传感器的Allan方差；

为角度随机游走噪声方差；

为速率随机游走噪声方差；

为零偏不稳定性噪声方差；

为速率斜坡噪声方差；

为量化噪声方差。in,

is the Allan variance of the inertial sensor;

is the angle random walk noise variance;

is the rate random walk noise variance;

zero bias instability noise variance;

is the rate ramp noise variance;

is the quantization noise variance.

The above formula can be rewritten as:

Simplified to have:

Comparing the above formula is:

Set up the Kalman equation of state and measurement equation:

X(k)=AX(k-1)+BW(k)

Z(k)=y(k)+V(k)=CX(k)+V(k)

Among them, X(k) is the n-dimensional state matrix of the system (here refers to the accelerometer), A is the n×n-dimensional state transition matrix of the system, B is the n×p-dimensional noise input matrix of the system, and W(k) is The p-dimensional process noise matrix of the system; Z(k) is the m-dimensional state matrix of the system, C is the m×n-dimensional observation matrix, and V(k) is the m-dimensional observation noise matrix.

The process noise and observation noise of the system satisfy:

where Q _k is the pairwise non-negative definite variance matrix of W( _k ), Rk is the pairwise positive definite variance matrix of V(k), and _δki is the Kronecker-δ function. The Kalman filter recursive formula is obtained as:

Among them, R is the measurement noise matrix, its value is the variance of the estimation error of the ARMA model, and Q is the system noise. The matrix obtained from the random data of the MEMS accelerometer is used as input for filtering, and the random drift curve of the MEMS accelerometer before and after filtering can be obtained after calculation, as shown in Figure 4.

(3) Solving algorithm:

The original data of the accelerometer and gyroscope are used to calculate the attitude angle in the quaternion algorithm after eliminating the error. Use the accelerometer data to calculate the angle at the start of the measurement, and then integrate the gyroscope output value to obtain the real-time attitude angle. Quaternion equation:

That is:

in:

The relationship between quaternion and attitude matrix:

Including:

The relationship between the attitude angle of the carrier and the attitude matrix is obtained:

remember

Get attitude angle:

Solving the quaternion uses the fourth-order Runge-Kutta method to solve the quaternion differential equation:

The state of the system is:

x _k =[a _bx a _by a _bz ω _bx ω _by ω _bz ] ^T

System observations are:

Z _k =[ω _bx ω _by ω _bz ] ^T

The state equation of the system is:

The observation equation is:

The measurement matrix of the system is obtained as:

The best estimate for the system is:

The attitude angle output is:

Design experiments to collect data under classical dynamic motion conditions to verify the settlement accuracy of the algorithm. Under the most representative swing condition, the swing amplitude is 2° and the frequency is 10hz to simulate the engine shaking scene during the operation of the car. It can be seen from Figure 5 that on the x-axis, the accuracy of the result calculated by this method is within 0.1° and the drift phenomenon caused by the long-term integration of the gyroscope is completely eliminated. The phenomenon is eliminated in this method. It can be seen from Figure 6 that there should be no swaying data on the y-axis at this time. From the theoretical analysis, there will be vibrations caused by the swaying of the x-axis. The value calculated by the existing algorithm under this motion condition is exactly The error value cannot reflect the y-axis vibration phenomenon caused by the x-axis swing, and the angle value calculated by this method reflects this physical phenomenon well. The method of the present invention has great practical value.

The above-mentioned embodiments are only preferred embodiments for fully illustrating the present invention, and the protection scope of the present invention is not limited thereto. Equivalent substitutions or transformations made by those skilled in the art on the basis of the present invention are all within the protection scope of the present invention. The protection scope of the present invention is subject to the claims.

Claims (7)

1. a dynamic attitude measurement method based on a MEMS inertial sensor, the MEMS inertial sensor has integrated a three-axis accelerometer and a three-axis gyroscope, and obtains attitude information according to the following steps: 1) Obtain the output data of the three-axis gyroscope and eliminate the error to obtain the angular velocity; 2) Obtain the output data of the three-axis accelerometer and eliminate the error to obtain the acceleration; 3) According to the angular velocity and acceleration obtained in steps 1) and 2), use the quaternion algorithm to calculate the attitude angle. 2. dynamic attitude measurement method as claimed in claim 1, is characterized in that, in step 1) set up following error model:

in,

are the measured values after the gyroscope error has been eliminated, ω _x , ω _y , ω _z are the original values measured by the gyroscope, f _x , f _y , and f _z are the specific force values of the accelerometer measured values, w _x , w _y , w _z is the zero offset drift error on the three axes of the gyroscope, l _x , _ly , and l _z are the scale factor errors, k _xy , k _xz , k _yx , k _yz , k _zy , k _zx are the installations between the corresponding two axes Error, m _xy , m _xz , m _yx , m _yz , m _zy , m _zx are the error coefficients of the specific force between the corresponding two axes, ε _gx , ε _gy , ε _gz are the random errors of the three axes of the gyroscope. 3. dynamic attitude measurement method as claimed in claim 2, is characterized in that, in the error model of three-axis gyroscope, for deterministic error coefficient, by carrying out test calibration experiment to gyroscope to determine: first carry out multi-position static measurement W _i , m _ij are obtained, and then the orthogonal angular rate measurement is carried out; since the sensitive axis is stationary when the gyroscope is running at a constant angular velocity orthogonal to the axis to be measured, the scale factor error can be omitted, so the above The error model simplifies to the following equation at this step:

in,

Represents the mutual coupling error. Since the constant acceleration applied to the gyroscope orthogonal to the axis to be measured is known, k _ij can be solved. 4. dynamic attitude measurement method as claimed in claim 2, is characterized in that, for the random error of three-axis gyroscope, utilize Allan variance analysis method to carry out estimation and compensation; At first, get N continuous gyroscope data as total sample , group the consecutive n sampled data into a sub-sample group, n<N/2, this sub-sample group is the data cluster, and then calculate the average value of the data samples in each data cluster; Assuming that the output of the gyroscope at time t is Ω(t), the average value of the kth data cluster is:

in,

is the average value of the kth data cluster containing n output data with a time length of T. It can be seen that the average value of the k+1th data cluster is:

To find the difference between the averages of two adjacent data clusters, we have:

So for a data cluster of time length T, the Allan variance is defined as:

Among them, the meaning of the symbol < > is to average the data in all symbols, then there are:

Substitute the calculated variance value data into the ARMA model to use the autoregressive model, as shown below: When the time series {x _k } obeys the n-order autoregressive model, the expression is as follows: x _k =α ₁ x _k-1 +…+α _n x _kn +ε _t The autocorrelation function is:

In the above formula: γ _k =E(x _t , x _t+k ) =Ex _t (α ₁ x _t+k-1 +α ₂ x _t+k-1 +…α _n x _t+kn +ε _t+k ) =α ₁ γ _k-1 +α ₂ γ _k-2 +…+α _n γ _kn ρ _k =ρ _−k , ρ ₀ =1 Yule-Walker equations:

AR(n) transfer function:

Random errors are thus estimated and compensated. 5. dynamic attitude measurement method as claimed in claim 1, is characterized in that, in step 2) set up following error model:

in,

Indicates the value of the three-axis measurement of the accelerometer after the error elimination algorithm, f _x , f _y , f _z represent the original value of the three-axis measurement of the accelerometer, F _x , F _y , F _z are the three-axis zero offset drift error of the accelerometer , h _x , _hy , h _z represent the error of the three-axis scale factor of the accelerometer, P _xy , P _yx , p _zx , p _xz , p _yz , p _zy represent the installation error between the three axes of the accelerometer, ε _fx , ε _fy , ε _fz represent random errors on the three axes of the accelerometer. 6. dynamic attitude measurement method as claimed in claim 5 is characterized in that, in step 2) adopt Allan variance analysis method, suppose that the output of accelerometer at time t is Ω (t), T is the time length of data cluster, then The mean of the kth data cluster is:

It can be seen that the average value of the k+1th data cluster is:

The difference between the mean values of two adjacent data clusters is:

For a data cluster of time length T, the Allan variance is defined as:

In the above formula, n is the number of data samples in the data cluster with time length T, and N is the total number of data samples; According to IEEE, the quantitative relationship between the Allan variance and the noise power spectral density in the original data is as follows:

Then the Allan variance of the total output of the signal is:

in,

is the Allan variance;

is the angle random walk noise variance;

is the rate random walk noise variance;

zero bias instability noise variance;

is the rate ramp noise variance;

is the quantization noise variance; The above formula can be rewritten as:

Simplified to have:

Comparing the above formula is:

Set up the Kalman equation of state and measurement equation: X(k)=AX(k-1)+BW(k) Z(k)=y(k)+V(k)=CX(k)+V(k) where X(k) is the n-dimensional state matrix of the system, A is the n×n-dimensional state transition matrix of the system, B is the n×p-dimensional noise input matrix of the system, and W(k) is the p-dimensional process noise matrix of the system ; Z(k) is the m-dimensional state matrix of the system, C is the m×n-dimensional observation matrix, and V(k) is the m-dimensional observation noise matrix; The process noise and observation noise of the system satisfy:

Among them, Q _k is the paired non-negative definite variance matrix of W(k), R _k is the paired positive definite variance matrix of V(k), and δ _ki is the Kronecker-δ function; the recursive Kalman filter formula is:

Among them, R is the measurement noise matrix, and its value is the variance of the estimation error of the ARMA model, and Q is the system noise; Therefore, Kalman filtering is used to eliminate the acceleration data errors. 7. dynamic attitude measurement method as claimed in claim 1, is characterized in that, step 3) the acceleration and angular velocity data after eliminating error are substituted in quaternion algorithm to calculate attitude angle, and quaternion equation is:

That is:

in:

The relationship between quaternion and attitude matrix:

Including:

The relationship between the attitude angle of the carrier and the attitude matrix is obtained:

remember

Get attitude angle:

Solving the quaternion uses the fourth-order Runge-Kutta method to solve the quaternion differential equation:

The state of the system is: Z _k =[ω _bx ω _by ω _bz ] ^T The state equation of the system is:

The observation equation is:

The measurement matrix of the system is obtained as:

The best estimate for the system is:

The attitude angle output is:

Thereby, dynamic pose information is obtained.

Images