WO2020114301A1

WO2020114301A1 - Magnetic-side roll angular velocity information-based rotary shell flight posture high-precision estimation method

Info

Publication number: WO2020114301A1
Application number: PCT/CN2019/121491
Authority: WO
Inventors: 龙达峰; 曹建忠; 魏晓慧; 罗中良; 徐瑜; 徐德明; 孙俊丽; 谢珩; 蔡远创
Original assignee: 惠州学院
Priority date: 2018-12-07
Filing date: 2019-11-28
Publication date: 2020-06-11
Also published as: CN109596018A; CN109596018B

Abstract

With respect to the problem in which "three highs," namely high overloading, high self-spinning, and high dynamic, of a rotary shell launch restrict an existing mature shell-mounted posture measurement system from being directly migrated for application to a rotary shell, employed is a rotary shell flight posture measurement solution consisting of a triaxial geomagnetic sensor, two monoaxial gyroscopes, and a satellite receiver. With respect to the measurement solution, provided is a high-precision filter estimation method applicable for rotary shell flight posture, comprising the following steps: (1) a rotary shell flight posture combined measurement, comprising 1.1, a shell-mounted sensor and a mounting scheme and 1.2, a shell flight posture high-precision combined filter structure; (2) a rotary shell high-precision offset model, comprising 2.1, a rotary shell posture offset model, 2.2, a rotary shell speed offset model, and 2.3, a position offset equation; (3) a shell body posture high-order nonlinear filter model; and (4) a shell posture high-precision real-time filtering algorithm for a filter updating process.

Description

High-precision estimation method for flying attitude of rotating projectile based on information of magnetic measurement roll angle rate

Technical field

The invention relates to a method for measuring the three-dimensional attitude of an aircraft or a missile space, in particular to a high-precision method for measuring the flying attitude of a rotating bomb.

Background technique

Due to the "three high" special bomb-loading application environment of high overload, high self-rotation and high dynamics of rotating bombs, the existing mature bomb-borne attitude measurement system cannot be directly transplanted to rotating ammunition, which has poor reliability and flight three-dimensional attitude parameters Problems such as incomplete testing or low measurement accuracy. In order to solve the above problems, currently combined measurement schemes such as INS+GPS combination, geomagnetism+GPS or geomagnetism+INS+GPS are used, and the corresponding filter algorithm is used to complete the estimation of the flight attitude parameters of the projectile, but the filter structure and filter model used Or filtering algorithm, etc. have certain limitations, the convergence, real-time performance and estimation accuracy of the designed filter are not ideal. For example, the existing attitude filter model based on the assumption of small misalignment error angle, which ignores the high-order filter model and cannot accurately describe the strong nonlinear characteristics of the rotating projectile system, these will directly affect the design of the projectile attitude filter Problems such as the convergence speed and filter accuracy of the filter, and even the filter divergence, can also cause the large error parameter estimation results of the filter.

Therefore, it is of great theoretical value and practical significance to find a high-precision measurement method suitable for the rotational attitude of the rotating missile, which is of great theoretical value and practical significance in solving the difficult problem of the flying attitude measurement in the guidance and transformation of the rotating missile.

Summary of the invention

The present invention addresses the problem that the "three heights" of high overload, high self-rotation and high dynamics of the launch of the rotating bomb limit the existing mature bomb-borne attitude measurement system and cannot be directly transplanted to the rotating bomb. A single-axis gyroscope and a satellite receiver form a rotating missile flight attitude measurement scheme, and a high-precision filtering estimation method suitable for the rotating bomb flight attitude is proposed for this measurement scheme.

The present invention is implemented using the following technical solution:

A high-precision estimation method for the flying attitude of a rotating projectile based on magnetic measurement of roll angular rate includes the following steps:

(1) Combined measurement of the flying attitude of the rotating bomb

1.1, bomb-mounted sensor and installation method

The bomb-mounted sensor uses a three-axis geomagnetic sensor, two single-axis gyroscopes and a satellite receiver. Among them, the three-axis geomagnetic sensor is used to measure the vector information of the geomagnetic field inside the projectile. The directions of each sensitive axis Mx, My and Mz and the projectile The coordinate system OX _b Y _b Z _{b has} the same direction of each axis; two single-axis gyroscopes Gy and Gz are respectively installed on the Y _b and Z _b axes, and the gyroscopes are used to measure the Y-axis and Z-axis angular rates of the projectile Information, the X-axis angular rate information is estimated by filtering the geomagnetic sensor measurement information; the satellite receiver is used to measure the velocity and position information of the projectile, and the receiver antenna is installed on the surface of the projectile in a loop.

1.2. High-precision combined filter structure for missile flight attitude

The combined filter is composed of two filters, a roll angle rate estimation filter and a high-precision filter for the flight attitude of the projectile, which are used to estimate the roll rate of the projectile and the high-precision estimation of the flight attitude of the projectile, respectively.

(2), high-precision error model of rotating bomb

According to the strapdown inertial navigation theory, the attitude quaternion from the body coordinate system to the navigation coordinate system

The differential equation of is expressed as:

In the above formula,

It is a quaternion matrix, which is in the form of an anti-matrix matrix:

among them,

Is the projected component of the angular velocity of the projectile coordinate system relative to the inertial coordinate system on the projectile coordinate system

It is the projection component of the angular velocity of the navigation coordinate system relative to the inertial coordinate system on the navigation coordinate system

It is the projection component of the angular velocity of the navigation coordinate system relative to the inertial coordinate system on the carrier coordinate system.

with

Represented as:

Due to angular rate component

It is measured by the sensor and its measured value

There must be measurement error

It is expressed as:

The angular velocity obtained by the strapdown solution

There is also a solution error, which is expressed as:

Substituting the above equations (3) and (4) into the attitude quaternion differential equation (1), the more differential equation of the attitude quaternion when the actual solution is obtained is:

Therefore, subtracting equation (1) from equation (5) yields the following form:

make

Then the error equation of the attitude quaternion is:

The last two expressions in the above formula (7) satisfy the following relationship:

among them,

with

They are defined as:

The above formula

with

In, satisfy the following relationship:

Substituting the above relationship into (7), the attitude quaternion error equation is obtained:

2.2. Rotating bomb speed error model

The velocity error equation derived from the specific force equation is:

In the above formula,

Is the calculation error of the attitude transformation matrix, because

Is about quaternion error

Nonlinear function, so

Also about

Nonlinear function

Is the equivalent zero offset of the accelerometer under the navigation system, satisfying

δK _A and δA are respectively the scale factor error matrix and the installation error matrix of the accelerometer; V ⁿ is the speed component under the navigation system; δ V ⁿ is the speed error;

Earth's rotation angular velocity;

Earth rotation angular velocity error;

The angular velocity of the navigation coordinate system relative to the earth;

Is the angular velocity error of the navigation coordinate system relative to the earth; f ^b is the accelerometer specific force.

If the accelerometer is calibrated beforehand, δK _A and δA can be disregarded, then the body velocity error equation is:

According to the conversion relationship described in the above formula (11), the obtained body attitude transformation matrix is:

Therefore, the calculation error of the attitude transformation matrix

for:

As mentioned before,

Substitute into the above formula (16) to get

Again

then

Simplified to:

When the attitude angle error is large,

Correct

Is nonlinear, where

The calculation formula is:

From the above derivation, we can see that the velocity error equation is a nonlinear error equation, where

The term is its nonlinear part. If the nonlinear part in the velocity error equation (14) is assumed to be f ₁ (x, t):

When linearizing the nonlinear velocity error equation, f ₁ (x, t) is

The formula of the Jacobian matrix is:

The above formula δu _i (i=1, 2, 3, 4) represents

The i-th line, where,

with

They are defined as:

Therefore, the comprehensive equations (14) and (18) are used to accurately describe the transmission law of the projectile velocity error in the case of large misalignment error angle, and it is a model of projectile velocity error applicable to the state of large error angle. .

2.3. Position error equation

The body position error equation is:

In equation (24), V _E , V _N , and V _U are the velocity of the projectile in the east, north, and sky directions; R _M and R _N are the radius of curvature of the meridian circle and the unitary circle where the projectile is located; L Is the latitude of the point where the projectile is located, λ is the longitude of the point where the projectile is located, h is the height of the point where the projectile is located; δV _E , δV _N , and δV _U are the velocity error parameters of the rotating projectile, and δh, δλ, and δL are the position error parameters.

(3) High-order nonlinear filtering model of projectile attitude

The quaternion error δQ, velocity error δv and position error δp of the rotating projectile, the gyro drift ε ^b , and the accelerometer bias are selected as the state variables of the system

It can be expressed as

The flight attitude error equation (12), velocity error equation (14) and position error equation (24) obtained by the above derivation form the filter state equation group, which is simplified to the following general form:

In the above formula, f[X(t), t] is a nonlinear function about the independent variable X(t), and the system process white noise w(t) satisfies E[w(t)]=0 and E[w(t) , W ^T (τ)] = Q(t)δ(t-τ);

The subtraction result of the position and velocity measured by the satellite navigation system and the corresponding position and velocity calculated by the inertial navigation system is selected as the observation variable Z(t) of the combined filter system, which is expressed as:

In the above formula, L _GPS , λ _GPS , h _GPS and v _{i, GPS} (i=N, E, U) represent the position and speed information measured and output by the satellite navigation system, respectively, and the subscript is the INS variable, which represents the inertial navigation solution. Position and velocity information; measurement noise v(t) satisfies E[v(t)]=0 and E[v(t), v ^T (τ)]=R(t)δ(t-τ);

Therefore, the above-mentioned state equation (25) and measurement equation (26) are combined to form a high-order nonlinear filtering model for the attitude of the projectile in the state of large error angle.

(4) High-precision real-time filtering algorithm for projectile attitude

The SRUKF filtering algorithm process based on the correction coefficient is as follows:

4.1, filter initialization:

For any period of time, the UKF filter algorithm calculates the specific steps of a cycle:

4.2. For a given

And P _k-1 (k = 0, 2, ... n), first calculate the state and predict one step

And one-step forecast error covariance matrix P _k,k-1 , including sigma point calculation and its propagation process:

4.2.1. Calculate sigma points

That is:

4.2.2, calculation

The sigma point propagating through the state equation f(·) is:

In the above formula, qr{·} means orthogonal triangulation of the matrix of the matrix, and

The table is the return value R after QR decomposition; the formula S′=cholupdate{S,u,±v} is equivalent to SS ^T =P, S′=chol{P±vuu ^T };

4.3. Use UT transform to find sigma points

Propagation of P _k,k-1 through the measurement equation includes the following two update calculation processes:

4.3.1. First, calculate the sigma point

And P _k,k-1 by measuring the propagation of equation h(·) to X _k :

4.3.2 Then, calculate the output one-step forecast value in advance:

4.4. Finally, use the new measured values to perform the filter update process:

The solution of the present invention has the following advantages:

1. The present invention adopts a high-precision combined filter structure of the missile's flight attitude based on magnetic measurement roll angle information suitable for the attitude measurement scheme. The combined filter is composed of a roll angle rate estimation filter (EKF) and a high missile flight attitude. The precision filter (SRUKF) consists of the estimation of the roll rate of the projectile and the high-precision estimation of the flight attitude of the projectile.

2. The present invention adopts a high-order non-linear filtering model of a high-precision projectile body suitable for a rotating projectile with a large error angle state. The filter state equation mainly includes an additive quaternion error model to describe the attitude error model of the rotating projectile in the state of large error angle, and a velocity error model and position error model to describe the rotating projectile based on the large error angle. The position and velocity of the satellite navigation system measurement output and the position and velocity subtraction result of the inertial navigation solution are selected as the observation of the combined filter system. The high-order filter model of the high-precision projectile attitude is formed by the above state equation and observation equation. The invention adopts a high-order filtering model of the body attitude to describe the characteristics of its nonlinear system more accurately, and can improve the accuracy of the three-dimensional attitude parameter estimation of the body.

3. The present invention adopts the SRUKF body attitude filtering estimation algorithm based on the correction coefficient, which is used to complete the high-precision filtering estimation of the flying attitude of the rotating missile. The SRUKF filter estimation algorithm based on the correction coefficient is different from the conventional SRUKF filter method in that there is an additional correction coefficient μ in the sigma point calculation, and its size must be added to the selection and determination through a priori knowledge. The improved correction coefficient SRUKF filter algorithm The problem of convergence and convergence will be better suppressed, while also taking into account the real-time performance of the estimation algorithm and the accuracy of the filter estimation.

BRIEF DESCRIPTION

Fig. 1 shows a schematic diagram of the installation method of the bomb-mounted sensor.

FIG. 2 shows a schematic diagram of a posture combination filtering structure of magnetic measurement roll angle information.

detailed description

The specific embodiments of the present invention will be described in detail below with reference to the drawings.

A high-precision estimation method for the flying attitude of a rotating projectile based on magnetic measurement of roll angular rate information includes the following steps:

1. The combined measurement scheme of the flying attitude of the rotating bomb

1.1, bomb-mounted sensor and installation method

The rotating projectile attitude measurement system adopts a combined geomagnetic/inertial/satellite solution. The onboard sensors mainly include three-axis geomagnetic sensors, two single-axis gyroscopes and satellite receivers. The strapdown installation of each sensor is shown in Figure 1. The geomagnetic sensor is used to measure the geomagnetic field vector information in the projectile. The directions of its sensitive axes (Mx, My, and Mz) are completely consistent with the directions of the axes of the projectile coordinate system (OX _b Y _b Z _b ); two single-axis gyros The gyroscopes (Gy and Gz) are installed on the Y _b and Z _b axes respectively. The gyroscope is used to measure the Y and Z angular rate information of the projectile. The X axial angular rate information needs to be estimated by filtering the geomagnetic sensor measurement information. The satellite receiver is used to measure the speed and position information of the projectile, and the receiver antenna is installed on the surface of the projectile in a loop.

1.2. High-precision combined filter structure for missile flight attitude

The high-precision combined filter structure of the missile's flight attitude based on the magnetic roll angle information is shown in Figure 2. The combined filter is composed of two roll angle rate estimation filters (EKF) and a high-resolution missile flight attitude filter (SRUKF). It is used to estimate the roll rate of the projectile and the high-precision estimation of the flight attitude parameters of the projectile, respectively. Design of high-precision filter for missile flight attitude: 1) Select the missile attitude angle error, velocity error and position error model and inertial device error model to construct the state equation of the combined filter system, in which the additive quaternion error model is used to describe Rotating projectile error model; using large error angle model to describe rotating projectile velocity error model and position error model. 2) Select the speed and position of the measurement output of the satellite navigation system and the corresponding strapdown inertial solution result to be subtracted as the observation of the combined filter system. 3) The high-precision filter of the missile's flight attitude adopts a correction coefficient SRUUF filter algorithm to complete the real-time high-precision estimation of the missile's flight attitude parameters. The algorithm takes into account the state parameter convergence speed and estimation accuracy.

2. High precision error model of rotating bomb

2.1. Rotating projectile error model

Considering that the inertial navigation system itself is non-linear, especially if the gyroscope is not installed on the X axis of the elastic axis, the angular rate in the X axis is estimated using geomagnetic information, but the angular rate estimation accuracy is not high. If the lower accuracy is used again When the combined measurement scheme of the inertial device is used, the nonlinearity of the system is more obvious. If the existing error model based on the small misalignment angle assumption is used, the filter model ignoring the higher-order terms cannot accurately describe the rotating elastic system. Non-linear characteristics, these will directly affect the convergence speed and filtering accuracy of the designed body attitude filter, and even the filter is open. Therefore, the present invention uses an additive quaternion error model to describe the flying attitude error model of the rotating missile, which can more accurately describe the characteristics of its nonlinear system, so as to improve the accuracy of the three-dimensional attitude parameter estimation of the projectile. According to the strapdown inertial navigation theory, the attitude quaternion from the body coordinate system to the navigation coordinate system

The differential equation of can be expressed as:

In the above formula,

It is a quaternion matrix, which is in the form of an anti-matrix matrix:

among them,

Is the projected component of the angular velocity of the body coordinate system (b system) relative to the inertial coordinate system (i system) on the b system

That is, the projected component of the angular velocity of the navigation coordinate system (n system) relative to the inertial coordinate system (i system) on the n system

The projected component of the angular velocity of the navigation coordinate system relative to the inertial coordinate system on the carrier coordinate system;

with

Represented as:

The above equation (1) is the differential equation applicable to the update of the attitude quaternion under any angle.

Due to angular rate component

It is measured by the sensor and its measured value

There must be measurement errors

It can be expressed as:

The angular velocity obtained by the strapdown solution

There are also solution errors, which can also be expressed as:

Substituting the above equations (3) and (4) into the attitude quaternion differential equation (1), the more differential equation of the attitude quaternion during actual solution can be obtained as:

Therefore, subtracting equation (1) from equation (5) gives the following form:

make

Then we can get the attitude quaternion error equation as:

among them,

with

They are defined as:

The above formula

with

In, satisfy the following relationship:

Substituting the above relationship into (7), we can get the attitude quaternion error equation:

It should be noted that the derivation process of the attitude quaternion error equation (12) of the present invention does not assume the case of a small misalignment error angle, so it can be used to describe the flight of a missile under a large misalignment error The attitude error propagation characteristic is a model of the attitude error of the projectile that is suitable for a large error angle.

2.2. Rotating bomb speed error model

From the specific force equation, the velocity error equation of the projectile can be derived as:

In the above formula,

Is the calculation error of the attitude transformation matrix, because

Is about quaternion error

Nonlinear function, so

Also about

Nonlinear function

δK _A and δA are the scale factor error matrix and installation error matrix of the accelerometer respectively; V ⁿ is the velocity component under the navigation system. δV ⁿ is the speed error;

Earth's rotation angular velocity;

Earth rotation angular velocity error;

The angular velocity of the navigation coordinate system relative to the earth;

Is the angular velocity error of the navigation coordinate system relative to the earth; f ^b is the specific force of the accelerometer.

If the accelerometer is calibrated beforehand, δK _A and δA can be disregarded, then the velocity error equation of the projectile is:

According to the conversion relationship described in equation (11) above, the available body attitude transformation matrix is:

Therefore, the calculation error of the attitude transformation matrix

Can be written as:

As mentioned before,

Substituting into the above formula (16) can be sorted out

Again

then

Can be abbreviated as:

When the attitude angle error is large,

Correct

Is nonlinear, where

The calculation formula is:

When linearizing the nonlinear velocity error equation, f ₁ (x, t) is

The formula of the Jacobian matrix is:

The above formula δu _i (i=1, 2, 3, 4) represents

The i-th line, where,

with

They are defined as:

Therefore, the comprehensive equations (14) and (18) can be used to accurately describe the transmission law of the projectile speed error under the condition of large misalignment error angle, and it is a kind of projectile speed error applicable to the state of large error angle model.

2.3. Position error equation

The body position error equation is:

3. High-order nonlinear filtering model of projectile attitude

The quaternion error δQ, velocity error δv and position error δp of the rotating projectile, gyro drift ε ^b and accelerometer bias are selected as the state variables of the system

It can be expressed as

In the above formula, f[X(t), t] is a nonlinear function about the independent variable X(t), and the system process white noise w(t) satisfies E[w(t)]=0 and E[w(t) , W ^T (τ)]=Q(t)δ(t-τ).

The position and velocity of the satellite navigation system measurement output and the corresponding position and velocity of the inertial navigation are subtracted as the observation variable Z(t) of the combined filter system, which can be expressed as:

In the above formula, L _GPS , λ _GPS , h _GPS and v _{i, GPS} (i=N, E, U) represent the position and velocity information measured and output by the satellite navigation system, respectively, and the subscript is the INS variable, which represents the inertial navigation solution. Location and speed information. The measurement noise v(t) satisfies E[v(t)]=0 and E[v(t), v ^T (τ)]=R(t)δ(t-τ). Therefore, the above-mentioned equation of state (25) and measurement equation (26) are combined to form a high-order nonlinear filtering model for the attitude of the projectile in the state of large error angle.

4. High-precision real-time filtering algorithm for projectile attitude

The above-mentioned high-precision filtering model of the rotating projectile attitude is a strong nonlinear equation system. The filtering methods such as UKF and SRUKF based on the nonlinear function probability density distribution can effectively avoid the model error caused by the approximation of the nonlinear equation. The problem of filtering accuracy. However, in some cases, SRUKF and traditional UKF filtering methods are prone to state covariance matrix P losing positive definiteness, resulting in filtering divergence. In response to the above problems, the present invention adopts a SRUKF body attitude filtering algorithm based on correction coefficients to improve the convergence speed and parameter estimation accuracy of the filtering algorithm.

4.1, filter initialization:

4.2. For a given

And P _k-1 (k = 0, 2, ... n), first calculate the state and predict one step

4.2.1. Calculate sigma points

That is:

4.2.2, calculation

The sigma point propagating through the state equation f(·) is:

In the above formula, qr{·} means to perform orthogonal triangulation (QR decomposition) of the matrix of the matrix, and

The table is the return value R after QR decomposition; the formula S′=cholupdate{S,u,±v} is equivalent to SS ^T =P, S′=chol{P±vuu ^T }.

4.3. Use UT transform to find sigma points

4.3.1. First, calculate the sigma point

And P _k,k-1 by measuring the propagation of equation h(·) to X _k :

4.3.2 Then, calculate the output one-step forecast value in advance:

4.4. Finally, use the new measured values to perform the filter update process:

The above-mentioned modified coefficient SRUKF filter estimation algorithm is different from the conventional SRUKF filter method in the calculation of sigma points. In the sigma point calculation formula (28), there is an additional correction coefficient μ, and its size must be added to the selection through prior knowledge. . Since the correction coefficient satisfies μ>1, the sigma point after time update

Error variance S _{k, k-1} and the predicted value in advance

The error variance S _zk will increase, but the observation noise variance matrix R _k will not change. Therefore, the measurement correction gain K _k will also increase, which means that the weight of the new measurement value is added Large, which reduces the impact of stale measurement information on the estimated value, so the problem of open convergence of the improved correction coefficient SRUKF filter algorithm will be better suppressed. Through the SRUKF filter algorithm flow based on the correction coefficients mentioned above, the high-precision estimation of the flight parameters of the rotating bomb is finally achieved.

It should be noted that, for a person of ordinary skill in the art, without departing from the principles of the present invention, several improvements and applications can be made, and these improvements and applications are also considered to be within the scope of the present invention.

Claims

A high-precision estimation method for the flying attitude of a rotating projectile based on magnetic measurement of rolling angular rate information is characterized by the following steps:

(1) Combined measurement of the flying attitude of the rotating bomb

1.1, bomb-mounted sensor and installation method

The bomb-mounted sensor uses a three-axis geomagnetic sensor, two single-axis gyroscopes and a satellite receiver. Among them, the three-axis geomagnetic sensor is used to measure the vector information of the geomagnetic field in the body of the missile. The coordinate system OX b Y b Z b has the same direction of each axis; two single-axis gyroscopes Gy and Gz are respectively installed on the Y b and Z b axes, and the gyroscopes are used to measure the Y-axis and Z-axis angular rates of the projectile Information, the X-axis angular rate information is estimated by filtering the geomagnetic sensor measurement information; the satellite receiver is used to measure the speed and position information of the projectile, and the receiver antenna is installed on the surface of the projectile in a loop;

1.2. High-precision combined filter structure for missile flight attitude

The combined filter is composed of two filters, a roll angle rate estimation filter and a high-precision filter for the flying attitude of the missile, which are used to complete the estimation of the roll rate of the missile and the high-precision estimation of the flying attitude of the missile, respectively;

(2), high-precision error model of rotating bomb

2.1. Rotating projectile error model

According to the strapdown inertial navigation theory, the attitude quaternion from the body coordinate system to the navigation coordinate system
The differential equation of is expressed as:

In the above formula,
It is a quaternion matrix, which is in the form of an anti-matrix matrix:

among them,
Is the projected component of the angular velocity of the projectile coordinate system relative to the inertial coordinate system on the projectile coordinate system
It is the projection component of the angular velocity of the navigation coordinate system relative to the inertial coordinate system on the navigation coordinate system
with
Represented as:

Due to angular rate component
It is measured by the sensor and its measured value
There must be measurement error
It is expressed as:

The angular velocity obtained by the strapdown solution
There is also a solution error, which is expressed as:

Substituting the above equations (3) and (4) into the attitude quaternion differential equation (1), the more differential equation of the attitude quaternion when the actual solution is obtained is:

Therefore, subtracting equation (1) from equation (5) yields the following form:

make
Then the error equation of the attitude quaternion is:

The last two expressions in the above formula (7) satisfy the following relationship:

among them,
with
They are defined as:

The above formula
with
In, satisfy the following relationship:

Substituting the above relationship into (7), the attitude quaternion error equation is obtained:

2.2. Rotating bomb speed error model

The velocity error equation derived from the specific force equation is:

In the above formula,
Is the calculation error of the attitude transformation matrix, because
Is about quaternion error
Nonlinear function, so
Also about
Nonlinear function
Is the equivalent zero offset of the accelerometer under the navigation system, satisfying
δK A and δA are respectively the scale factor error matrix and the installation error matrix of the accelerometer; V n is the speed component under the navigation system; δ V n is the speed error;
Earth's rotation angular velocity;
Earth rotation angular velocity error;
The angular velocity of the navigation coordinate system relative to the earth;
Is the angular velocity error of the navigation coordinate system relative to the earth; f b is the specific force of the accelerometer;

If the accelerometer is calibrated beforehand, δK A and δA are not considered, then the velocity error equation of the projectile is:

According to the conversion relationship described in the above formula (11), the obtained body attitude transformation matrix is:

Therefore, the calculation error of the attitude transformation matrix
for:

As mentioned before,
Substitute into the above formula (16) to get

Again
then
Simplified to:

Correct
Is nonlinear, where
The calculation formula is:

From the above derivation, we can see that the velocity error equation is a nonlinear error equation, where
The term is its nonlinear part. If the nonlinear part in the velocity error equation (14) is assumed to be f 1 (x, t):

When linearizing the nonlinear velocity error equation, f 1 (x, t) is
The formula of the Jacobian matrix is:

The above formula δu i (i=1, 2, 3, 4) represents
The i-th line, where,
with
They are defined as:

The integrated equations (14) and (18) are the velocity error models of the projectile that are suitable for the large error angle state;

2.3. Position error equation

The body position error equation is:

In equation (24), V E , V N , and V U are the velocity of the projectile in the east, north, and sky directions; R M and R N are the radius of curvature of the meridian circle and the unitary circle where the projectile is located; L Is the latitude of the point where the projectile is located, λ is the longitude of the point where the projectile is located, h is the height of the point where the projectile is located; δV E , δV N , δV U are the parameters of the velocity error of the rotating projectile, δh, δλ and δL are the position error parameters;

(3) High-order nonlinear filtering model of projectile attitude

The quaternion error δQ, velocity error δ V , position error δp, gyro drift ε b and accelerometer bias of the rotating projectile are selected as the state variables of the system
Which is expressed as

The flight attitude error equation (12), velocity error equation (14) and position error equation (24) obtained by the above derivation form the filter state equation group, which is simplified to the following general form:

In the above formula, f[X(t), t] is a nonlinear function about the independent variable X(t), and the system process white noise w(t) satisfies E[w(t)]=0 and E[w(t) , W T (τ)] = Q(t)δ(t-τ);

The subtraction result of the position and velocity measured by the satellite navigation system and the corresponding position and velocity calculated by the inertial navigation system is selected as the observation variable Z(t) of the combined filter system, which is expressed as:

In the above formula, L GPS , λ GPS , h GPS and v i, GPS (i=N, E, U) represent the position and speed information measured and output by the satellite navigation system, respectively, and the subscript is the INS variable, which represents the inertial navigation solution. Position and velocity information; measurement noise v(t) satisfies E[v(t)]=0 and E[v(t), v T (τ)]=R(t)δ(t-τ);

Combining the above equations of state (25) and measurement equations (26) together constitute a high-order nonlinear filtering model of the body attitude suitable for large error angle states;

(4) High-precision real-time filtering algorithm for projectile attitude

The SRUKF filtering algorithm process based on the correction coefficient is as follows:

4.1, filter initialization:

For any period of time, the UKF filter algorithm calculates the specific steps of a cycle:

4.2. For a given
And P k-1 (k = 0, 2, ... n), first calculate the state and predict one step
And one-step forecast error covariance matrix P k,k-1 , including sigma point calculation and its propagation process:

4.2.1. Calculate sigma points
That is:

4.2.2, calculation
The sigma point propagating through the state equation f(·) is:

In the above formula, qr{·} means orthogonal triangulation of the matrix of the matrix, and
The table is the return value R after QR decomposition; the formula S′=cholupdate{S,u,±v} is equivalent to SS T =P, S′=chol{P±vuu T };

4.3. Use UT transform to find sigma points
Propagation of P k,k-1 through the measurement equation includes the following two update calculation processes:

4.3.1. First, calculate the sigma point
And P k,k-1 by measuring the propagation of equation h(·) to X k :

4.3.2 Then, calculate the output one-step forecast value in advance:

4.4. Finally, use the new measured values to perform the filter update process: