A method and system for estimating the altitude state of an aircraft with strong disturbance rejection

Abstract

This invention provides a method and system for estimating the altitude state of an aircraft with strong anti-disturbance capabilities. The method includes the following steps: Step S1: Acquire sensor data and estimate sensor delay, then align the sensor delay to obtain sensor observations; Step S2: Estimate the sensor error variance based on the sensor observations; Step S3: Calculate the second derivative of the sensor observations and calculate the correlation between the second derivative and inertial measurement acceleration; Step S4: Perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground; Step S5: Filter the relative altitude and absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observations to calculate the aircraft's altitude, and finally use a first-order Kalman filter algorithm to estimate the aircraft's altitude above the ground. This invention provides accurate altitude state observation in aircraft, resulting in high flight safety and reliability.

Description

Technical Field

[0001] This invention relates to the field of aircraft technology, and in particular to a method and system for estimating the altitude state of an aircraft with strong anti-disturbance capabilities. Background Technology

[0002] Currently, due to the rapid development of the microelectronics field, various configurations of unmanned aerial vehicles (UAVs) are widely used in reconnaissance, rescue, and plant protection. The design of UAV flight control systems mainly includes two aspects: UAV motion control and system state estimation. System state estimation refers to using sensors to estimate the UAV's position, velocity, attitude angle, angular velocity, and other state variables. The accuracy of the calculated UAV motion state directly affects the stability and safety of UAV flight.

[0003] Currently, many aircraft controller developers have developed methods for estimating aircraft altitude state: the APM flight controller uses a third-order complementary filter algorithm to estimate the aircraft's altitude state. The PX4 flight controller uses an extended Kalman filter algorithm to estimate the aircraft's altitude state.

[0004] However, the aircraft altitude state estimation methods used by the above-mentioned aircraft only consider the case where the sensors are working normally, and do not adequately consider the detection of sensor failures. When a sensor fails and the sensor data changes abruptly, if no special processing is performed, the altitude and speed of the aircraft will be incorrectly estimated, which may easily lead to loss of control of the aircraft. Secondly, the state estimation algorithm used by the PX4 flight controller contains a 24-dimensional state equation, which has a high degree of integration and is not conducive to distributed computing, resulting in poor aircraft altitude state estimation performance. Summary of the Invention

[0005] To address the shortcomings of the aforementioned related technologies, this invention proposes a strong anti-disturbance altitude state estimation method for aircraft, which aims to solve the problems of easy loss of control, poor reliability, and poor stability of existing aircraft during flight.

[0006] To address the aforementioned technical problems, embodiments of the present invention provide a method for estimating the altitude state of an aircraft with strong anti-disturbance capabilities, the method comprising the following steps:

[0007] Step S1: Acquire sensor data and estimate sensor delay, then align the sensor delay to obtain sensor observations;

[0008] Step S2: Estimate the error variance of the sensor based on the sensor observations;

[0009] Step S3: Calculate the second derivative of the sensor observation and calculate the correlation between the second derivative and the inertial measurement acceleration;

[0010] Step S4: Perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground;

[0011] Step S5: Obtain the current absolute altitude, filter the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observation to calculate the aircraft's altitude, and finally use a first-order Kalman filter algorithm to estimate the aircraft's altitude above the ground.

[0012] Preferably, in step S1, the sensor delay represents the time difference between the data received by the processor and the actual data, which is the time required for the sensor to process and send the data after it has acquired the data.

[0013] Preferably, the current data output by a certain sensor is defined as z(k), and the historical data output by the sensor is defined as z(k+1), z(k+2), ... z(k+n), where n is an integer. The data after sensor delay alignment is the sensor observation, which is shown by expression (1), and the sensor delay is shown by expression (2).

[0014] Z delay =z(k+Td)…(1);

[0015] Td=T delay / ΔT…(2);

[0016] Among them, T delay ΔT represents the time delay relative to the sensor with the largest time delay, and ΔT is the sensor's sampling period.

[0017] Preferably, step S2 specifically includes the following sub-steps:

[0018] Based on the data at the current moment and the historical data; let the data at the current moment be z(0) and the historical data be z(k), where k = 1, 2, ... n;

[0019] Estimate the actual distance measured by the sensor, as shown in expression (3):

[0020] Y est (k)=b+ka, k=0,1,2…n…(3);

[0021] The error of the sensor is calculated as shown in expression (4):

[0022] Err(k) = Y est (k)-Z(k), k=0,1,2…n…(4);

[0023] Calculate the variance of the error and substitute it into the Kalman filtering algorithm, as shown in expression (5):

[0024] Variance = E(Err) 2 )-E(Err) 2 …(5);

[0025] Among them, Y est The coefficients a and b in the expression are calculated by the following expressions (6) and (7), respectively:

[0026]

[0027]

[0028] Preferably, step S3 specifically includes the following sub-steps:

[0029] Given the current data as z(0) and the historical data as z(k), k = 0, 1, 2, ..., n-1, the first derivative of the sensor observations is calculated as follows:

[0030]

[0031] Record the sequence value z1(k) of the first derivative of the sensor observation, k = 0, 1, 2, ..., n-1, and calculate the second derivative of the sensor observation:

[0032]

[0033] Record the second derivative sequence values ​​of the sensor observations, and calculate and record the sequence value of the celestial acceleration in the inertial coordinate system:

[0034] z2(k)(k=0,1,2,…n-1)…(10);

[0035] a z =C bn ai z (i z =[0 0 1] T )…(11);

[0036] a z (k)(k=0,1,2,…n-1)…(12);

[0037] Where a is the value of the accelerometer across its three axes, and C... bn The transformation matrix from the aircraft coordinate system to the inertial coordinate system is given; finally, the correlation function between the second derivative of the observations and the UAV's celestial acceleration is calculated:

[0038] The related functions

[0039] Preferably, in step S4, the method for calculating the relative height is as follows:

[0040] The aircraft is presumably equipped with n ranging sensors, and the observation noise variance of each ranging sensor is R. i The correlation coefficient between the second derivative term of each ranging sensor and the celestial acceleration is G. i The observation of each of the ranging sensors is Z. i Where i = 1, 2, ..., n, and n is a positive integer;

[0041] Then the weighting coefficient K for each of the ranging sensors i Let i = 1, 2, ..., n, and calculate as follows:

[0042] make

[0043] but

[0044] The observations are weighted and fused to calculate the relative altitude of the aircraft above the ground:

[0045] r_H=K1Z1+K2Z2+…+K n Z n …(16);

[0046] Where H is the altitude of the aircraft.

[0047] Preferably, step S5 specifically includes the following sub-steps:

[0048] First, define the state equation for the Kalman filter:

[0049]

[0050] Where h represents the aircraft's altitude, v represents the aircraft's celestial velocity, and a biase For celestial acceleration A z The bias error, where Δt is the sampling period;

[0051] Define the Kalman filter observation equation:

[0052]

[0053] The Kalman filter algorithm is obtained by combining the Kalman filter state equation and the Kalman filter observation equation.

[0054] Then, a first-order Kalman filter algorithm is used to fuse the observations from the ranging sensor with the celestial acceleration data to estimate the distance between the UAV and the ground.

[0055] The state equation and observation equation for the first-order Mann filter are as follows:

[0056]

[0057] Where rH is the relative altitude of the aircraft, and V z The denoted velocity is the UAV's upward velocity estimated by the third-order Kalman filter algorithm.

[0058] Preferably, the height observation switching mechanism is as follows:

[0059] Step S51: Preset height switching threshold;

[0060] Step S52: Determine whether the relative height is less than the height switching threshold; if yes, mark the relative height at position 1; if no, mark the relative height at position 0; wherein, position 1 and position 0 are assigned values.

[0061] Step S53: Determine whether the position of the relative altitude marker changes audibly. If yes, record the current relative altitude, absolute altitude, and altitude of the aircraft. If no, return to step S52.

[0062] In a second aspect, embodiments of the present invention provide a strong anti-disturbance altitude state estimation system for an aircraft, the system comprising:

[0063] The acquisition module is used to acquire sensor data and estimate sensor delay, and then align the sensor delay to obtain sensor observations.

[0064] An estimation module is used to estimate the error variance of the sensor based on the sensor observations;

[0065] The differential calculation module is used to calculate the second derivative of the sensor observation and to calculate the correlation between the second derivative and the inertial measurement acceleration.

[0066] The altitude calculation module is used to perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground;

[0067] The processing module is used to obtain the current absolute altitude, filter the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observation to calculate the aircraft's altitude, and finally use a first-order Kalman filter algorithm to estimate the aircraft's altitude above the ground.

[0068] Compared with related technologies, this invention, through steps S1-S5, employs an adaptive estimation technique for observation noise variance to estimate the observation noise of each sensor in real time. The estimated noise variance is then incorporated into the Kalman filter algorithm to enhance the filtering effect. Sensor fault diagnosis is performed using the correlation between the second derivative of the sensor data and the aircraft's celestial acceleration, avoiding situations where a sensor malfunctions but the observation noise is low. Based on the relative altitude between the aircraft and the ground, the observations of absolute and relative altitudes are switched to enhance altitude data stability. The structure is simple and easy to debug. By estimating the observation noise variance of the sensors in real time and using the Kalman filter algorithm to calculate the UAV's altitude and speed status, the probability of UAV loss of control due to sensor malfunctions is reduced, improving UAV flight safety and reliability. Attached Figure Description

[0069] The present invention will now be described in detail with reference to the accompanying drawings. The above and other aspects of the present invention will become clearer and more readily understood through the detailed description following the accompanying drawings. In the drawings:

[0070] Figure 1 This is a flowchart of the high-disturbance-resistance altitude state estimation method for the aircraft of the present invention;

[0071] Figure 2 This is an overall flowchart of the strong disturbance rejection altitude state estimation method for the aircraft of the present invention;

[0072] Figure 3 This is a flowchart of the height observation switching mechanism in step S5 of the present invention;

[0073] Figure 4 This is a flowchart illustrating the specific process of the height observation switching mechanism in step S5 of the present invention.

[0074] Figure 5 This is a block diagram of the high-disturbance-resistance altitude state estimation segmentation system for the aircraft of the present invention. Detailed Implementation

[0075] The specific embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0076] The specific embodiments / examples described herein are specific implementations of the present invention, used to illustrate the concept of the invention, and are illustrative and exemplary, and should not be construed as limiting the implementation methods or scope of the present invention. In addition to the embodiments described herein, those skilled in the art can employ other obvious technical solutions based on the content disclosed in the claims and specification of this application. These technical solutions include those that make any obvious substitutions and modifications to the embodiments described herein, all of which are within the protection scope of the present invention.

[0077] Example 1

[0078] like Figures 1-2 As shown, this invention provides a method for estimating the altitude state of an aircraft with strong anti-disturbance capabilities. The aircraft include drones, airplanes, satellites, etc. This invention mainly uses drones, which have good flight control and diverse functions.

[0079] The method includes the following steps:

[0080] Step S1: Acquire sensor data and estimate sensor delay, then align the sensor delay to obtain sensor observations;

[0081] Step S2: Estimate the error variance of the sensor based on the sensor observations;

[0082] Step S3: Calculate the second derivative of the sensor observation, and calculate the correlation between the second derivative and the inertial measurement acceleration. The inertial measurement acceleration is the IMU (Inertial Measurement Unit) acceleration, used to measure the three-axis attitude angles (or angular rates) and acceleration of an object.

[0083] Step S4: Perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground;

[0084] Step S5: Obtain the current absolute altitude, filter the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observation to calculate the aircraft's altitude, and finally use a first-order Kalman filter algorithm to estimate the aircraft's altitude above the ground.

[0085] Specifically, sensor observations are obtained by acquiring sensor data and estimating sensor delays, then aligning the sensor delays; based on these observations, the error variance of the sensors is estimated; the second derivative of the sensor observations is calculated, and the correlation between the second derivative and inertial measurement acceleration is calculated; the sensor observations and the correlation are weighted and fused to calculate the relative altitude of the aircraft above the ground; the current absolute altitude is obtained, and both the relative and absolute altitudes are filtered using a third-order Kalman filter algorithm; a height observation switching mechanism is then used to switch the height observations used to calculate the aircraft's altitude; finally, a first-order Kalman filter algorithm is used to estimate the aircraft's altitude above the ground. This method employs an adaptive estimation technique for observation noise variance, estimating the observation noise of each sensor in real time, and then incorporating the estimated noise variance into the Kalman filter algorithm to enhance the filtering effect; sensor fault diagnosis is performed using the correlation between the second derivative of the sensor data and the aircraft's celestial acceleration, avoiding situations where a sensor malfunctions but the observation noise is low; and the stability of the altitude data is enhanced by switching between the absolute and relative altitude observations based on the relative altitude of the aircraft above the ground. The structure is simple and easy to debug. By estimating the observation noise variance of the sensor in real time and using the Kalman filter algorithm, the altitude and speed status of the UAV are calculated, which reduces the probability of UAV loss of control caused by sensor failure and improves the flight safety and reliability of the UAV.

[0086] In this embodiment, in step S1, the sensor delay represents the time difference between the data received by the processor and the actual data, which is the time required for the sensor to process and send the data after it has collected the data.

[0087] Specifically, after data is collected by sensors, it takes a certain amount of time to process and transmit the data. The time difference between the data received by the processor and the actual data is the sensor latency. Different types and models of sensors have different data latencies, so it is necessary to align the observations with shorter latencies to those with longer latencies to ensure data consistency.

[0088] More specifically, the current data output by a certain sensor is defined as z(k), and the historical data output by the sensor is defined as z(k+1), z(k+2), ... z(k+n), where n is an integer. The data after sensor delay alignment is the sensor observation, which is shown by expression (1), and the sensor delay is shown by expression (2).

[0089] Z delay =z(k+Td)…(1);

[0090] Td=T delay / ΔT…(2);

[0091] Among them, T delay ΔT represents the time delay relative to the sensor with the largest time delay, and ΔT is the sensor's sampling period.

[0092] In this embodiment, step S2 specifically includes the following sub-steps:

[0093] Based on the data at the current moment and the historical data; let the data at the current moment be z(0) and the historical data be z(k), where k = 1, 2, ... n;

[0094] Estimate the actual distance measured by the sensor, as shown in expression (3):

[0095] Y est (k)=b+ka, k=0,1,2…n…(3);

[0096] The error of the sensor is calculated as shown in expression (4):

[0097] Err(k) = Y est (k)-Z(k), k=0,1,2…n…(4);

[0098] Calculate the variance of the error and substitute it into the Kalman filtering algorithm, as shown in expression (5):

[0099] Variance = E(Err) 2 )-E(Err) 2 …(5);

[0100] Among them, Y est The coefficients a and b in the expression are calculated by the following expressions (6) and (7), respectively:

[0101]

[0102]

[0103] Specifically, an adaptive sensor observation noise variance estimation algorithm is adopted. Based on the sensor observation sequence, the actual value of the sensor measurement is estimated, and then the sensor measurement error is calculated using this actual value. Finally, the error variance is calculated. This reduces the probability of UAV loss of control caused by sensor failure and improves the flight safety and reliability of UAV.

[0104] In this embodiment, step S3 specifically includes the following sub-steps:

[0105] Given the current data as z(0) and the historical data as z(k), k = 0, 1, 2, ..., n-1, the first derivative of the sensor observations is calculated as follows:

[0106]

[0107] Record the sequence value z1(k) of the first derivative of the sensor observation, k = 0, 1, 2, ..., n-1, and calculate the second derivative of the sensor observation:

[0108]

[0109] Record the second derivative sequence values ​​of the sensor observations, and calculate and record the sequence value of the celestial acceleration in the inertial coordinate system:

[0110] z2(k)(k=0,1,2,…n-1) … (10);

[0111] a z =C bn ai z (i z =[0 0 1] T )…(11);

[0112] a z (k)(k=0,1,2,…n-1)…(12);

[0113] Where a is the value of the accelerometer across its three axes, and C... bn The transformation matrix from the aircraft coordinate system to the inertial coordinate system is given; finally, the correlation function between the second derivative of the observations and the UAV's celestial acceleration is calculated:

[0114] The related functions

[0115] In this embodiment, the method for calculating the relative height in step S4 is as follows:

[0116] The aircraft is presumably equipped with n ranging sensors, and the observation noise variance of each ranging sensor is R. i The correlation coefficient between the second derivative term of each ranging sensor and the celestial acceleration is G. i The observation of each of the ranging sensors is Z. i Where i = 1, 2, ..., n, and n is a positive integer;

[0117] Then the weighting coefficient K for each of the ranging sensors i Let i = 1, 2, ..., n, and calculate as follows:

[0118] make

[0119] but

[0120] The observations are weighted and fused to calculate the relative altitude of the aircraft above the ground:

[0121] r_H=K1Z1+K2Z2+…+K n Z n …(16);

[0122] Where H is the altitude of the aircraft.

[0123] In this embodiment, step S5 specifically includes the following sub-steps:

[0124] First, define the state equation for the Kalman filter:

[0125]

[0126] Where h represents the aircraft's altitude, v represents the aircraft's celestial velocity, and a biase For celestial acceleration A z The bias error, where Δt is the sampling period;

[0127] Define the Kalman filter observation equation:

[0128]

[0129] The Kalman filter algorithm is obtained by combining the Kalman filter state equation and the Kalman filter observation equation. In (17) and (18) above, the Kalman filter state equation is X = AX + BU. This is achieved by referencing a discrete control process system, which can be represented by a linear stochastic differential equation. In Z = HX, X represents the system state at the current moment, U is the control quantity of the system at the current moment, A is the system state transition matrix, and B is the system control matrix. For multi-model systems, these can also be called matrices. Z is the measured value at the current moment, and H is the parameter of the measurement system. For multi-measurement systems, H is the measurement matrix.

[0130] Then, a first-order Kalman filter algorithm is used to fuse the observations from the ranging sensor with the celestial acceleration data to estimate the distance between the UAV and the ground.

[0131] The state equation and observation equation for the first-order Mann filter are as follows:

[0132]

[0133] Where rH is the relative altitude of the aircraft, and V z The denoted velocity is the UAV's upward velocity estimated by the third-order Kalman filter algorithm.

[0134] Specifically, r_H represents the drone's ground clearance obtained through relative sensor fusion, and V... zThe image shows the UAV's skyward velocity estimated by a third-order Kalman filter algorithm. In the linear Kalman filter algorithm described above, two parameters need to be tuned: process noise Q and observation noise R. A larger Q results in faster convergence of the system state to the observation Z, while a smaller Q makes the system state more dependent on the system model. The Q parameter needs to be adjusted multiple times to obtain the optimal filtering effect. The observation noise R can be obtained by estimating the variance of the observation error of the altitude observation.

[0135] In this embodiment, as Figures 3-4 As shown, the height observation switching mechanism is specifically as follows:

[0136] Step S51: Preset height switching threshold;

[0137] Step S52: Determine whether the relative height is less than the height switching threshold; if yes, mark the relative height at position 1; if no, mark the relative height at position 0; wherein, position 1 and position 0 are assigned values.

[0138] Step S53: Determine whether the position of the relative altitude marker changes audibly. If yes, record the current relative altitude, absolute altitude, and altitude of the aircraft. If no, return to step S52.

[0139] Specifically, let the height switching threshold be H_switch, the relative height after fusion filtering be r_H, and the absolute height after fusion filtering be abs_H.

[0140] The specific mechanism for switching altitude observations in this invention is as follows: when the UAV is near the ground, relative distance is used as the altitude observation measure. At this time, the altitude and azimuth velocity are calculated from data measured by a relative distance sensor. When the UAV flies higher, its altitude is calculated from data from absolute altitude sensors such as barometers and GPS. By switching between absolute and relative altitude observations based on the relative altitude between the UAV and the ground, the stability of the altitude data is enhanced.

[0141] Example 2

[0142] like Figure 5 As shown, this embodiment of the invention provides a strong anti-disturbance altitude state estimation system 200 for an aircraft, the system 200 comprising:

[0143] The acquisition module 201 is used to acquire sensor data and estimate sensor delay, and then align the sensor delay to obtain sensor observations.

[0144] The estimation module 202 is used to estimate the error variance of the sensor based on the sensor observations;

[0145] The differential calculation module 203 is used to calculate the second derivative of the sensor observation and to calculate the correlation between the second derivative and the inertial measurement acceleration.

[0146] Altitude calculation module 204 is used to perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground;

[0147] The processing module 205 is used to obtain the current absolute altitude, filter the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observation to calculate the altitude of the aircraft, and finally use a first-order Kalman filter algorithm to estimate the altitude of the aircraft above the ground.

[0148] Specifically, the acquisition module 201 acquires sensor data and estimates sensor delay, then aligns the sensor delay to obtain sensor observations; the estimation module 202 estimates the sensor error variance based on the sensor observations; the differential calculation module 203 calculates the second derivative of the sensor observations and the correlation between the second derivative and the inertial measurement acceleration; the altitude calculation module 204 performs data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground; the processing module 205 acquires the current absolute altitude, filters the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then uses an altitude observation switching mechanism to switch the altitude observations to calculate the aircraft's altitude, and finally uses a first-order Kalman filter algorithm to estimate the aircraft's altitude above the ground. This approach employs an adaptive estimation technique for observation noise variance to estimate the observation noise of each sensor in real time. The estimated noise variance is then fed into the Kalman filter algorithm to enhance the filtering effect. Sensor fault diagnosis is performed using the correlation between the second derivative of the sensor data and the aircraft's celestial acceleration, preventing situations where a sensor malfunctions despite low observation noise. Based on the relative altitude between the aircraft and the ground, the observations of absolute and relative altitudes are switched to enhance altitude data stability. The structure is simple and easy to debug. By estimating the observation noise variance of the sensors in real time and using the Kalman filter algorithm to calculate the UAV's altitude and velocity status, the probability of UAV loss of control due to sensor malfunctions is reduced, improving UAV flight safety and reliability.

[0149] In this embodiment, in the acquisition module, the sensor delay represents the time difference between the data received by the processor and the actual data, which is the time required for the sensor to process and send the data after it has acquired it.

[0150] Specifically, after data is collected by sensors, it takes a certain amount of time to process and transmit the data. The time difference between the data received by the processor and the actual data is the sensor latency. Different types and models of sensors have different data latencies, so it is necessary to align the observations with shorter latencies to those with longer latencies to ensure data consistency.

[0151] In this embodiment, the technical effects and principles achieved by the strong anti-disturbance altitude state estimation system 200 of the aircraft are the same as those achieved by the strong anti-disturbance altitude state estimation method of the aircraft provided in the first embodiment of the present invention, and will not be repeated here.

[0152] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any alterations, equivalent substitutions, or improvements made within the spirit and principles of the present invention should be included within the scope of the claims of the present invention.

Claims

1. A method for estimating the altitude state of an aircraft with strong disturbance rejection, characterized in that, The method includes the following steps: Step S1: Acquire sensor data and estimate sensor delay, then align the sensor delay to obtain sensor observations; Step S2: Estimate the error variance of the sensor based on the sensor observations; Step S3: Calculate the second derivative of the sensor observation and calculate the correlation between the second derivative and the inertial measurement acceleration; Step S4: Perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground; Step S5: Obtain the current absolute altitude, filter the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observation to calculate the aircraft's altitude, and finally use a first-order Kalman filter algorithm to estimate the aircraft's altitude above the ground. Step S3 specifically includes the following sub-steps: Given the current data as z(0) and the historical data as z(k), where k=1,2,…n-1, the first derivative of the sensor observations is calculated as follows: …（8）； Then record the sequence value of the first derivative of the sensor observation. (k), k=1,2,…n-1, and calculate the second derivative of the sensor observations: …（9）； Record the second derivative sequence values ​​of the sensor observations, and calculate and record the sequence value of the celestial acceleration in the inertial coordinate system: (k),k=1,2,…n-1 …(10); ， …(11)； …(12)； in, The values ​​for the three axes of the accelerometer. The transformation matrix from the aircraft coordinate system to the inertial coordinate system is given; finally, the correlation function g(X) between the second derivative of the observations and the UAV's celestial acceleration is calculated: The related functions …(13) In step S4, the method for calculating the relative height is as follows: The aircraft is presumably equipped with n ranging sensors, and the observation noise variance of each ranging sensor is... The correlation coefficient between the second derivative term of each ranging sensor and the celestial acceleration is: The observations of each of the ranging sensors are Where i = 1, 2, ..., n, and n is a positive integer; Then the weighting coefficients for each of the ranging sensors For i = 1, 2, ..., n, the calculation is as follows: make … (14); but …(15); The observations are weighted and fused to calculate the relative altitude r_H of the aircraft above the ground: …(16)。 2. The method for estimating the altitude state of an aircraft with strong disturbance rejection as described in claim 1, characterized in that, In step S1, the sensor delay refers to the time difference between the data received by the processor and the actual data, which is the time required for the sensor to process and send the data after it has collected the data.

3. The method for estimating the altitude state of an aircraft with strong disturbance rejection as described in claim 2, characterized in that, Let z(k) be the data output by a certain sensor at the current moment, and z(k+1) and z(k+2) be the historical data output by the sensor. z(k+n), where n is an integer, the data after sensor delay alignment is the sensor observation Z_delay, the sensor observation Z_delay is shown by expression (1), and the sensor delay Td is shown by expression (2): …（1）； …（2）； in, For the sensor with the largest time delay, This is the sampling period of the sensor.

4. The method for estimating the altitude state of an aircraft with strong disturbance rejection as described in claim 3, characterized in that, Step S2 specifically includes the following sub-steps: Based on the data at the current moment and the historical data; let the data at the current moment be z(0) and the historical data be z(k), where k = 1, 2, ... n; Estimate the actual distance measured by the sensor The actual distance is shown in expression (3): ， …（3）； Calculate the error of the sensor The error is shown in expression (4): …（4）； Calculate the variance of the error and substitute it into the Kalman filtering algorithm, as shown in expression (5): …（5）； in, The coefficients a and b in the expression are calculated by the following expressions (6) and (7), respectively: …（6）； …（7）。 5. The method for estimating the altitude state of an aircraft with strong disturbance rejection as described in claim 4, characterized in that, Step S5 specifically includes the following sub-steps: First, define the state equation for the Kalman filter: …(17)； in, h The aircraft's altitude status. v This refers to the aircraft's celestial velocity status. For celestial acceleration The bias error, The sampling period is X; X represents the system state at the current moment, U is the control input to the system at the current moment, A is the system state transition matrix, and B is the system control matrix. Define the Kalman filter observation equation: …(18); The Kalman filter algorithm is obtained by combining the Kalman filter state equation and the Kalman filter observation equation. Then, a first-order Kalman filter algorithm is used to fuse the observations from the ranging sensor with the celestial acceleration data to estimate the distance between the UAV and the ground. The state equation and observation equation for the first-order Mann filter are as follows: …(19)； in, r_H The relative altitude of the aircraft. The denoted velocity is the UAV's upward velocity estimated by the third-order Kalman filter algorithm.

6. The method for estimating the altitude state of an aircraft with strong disturbance rejection as described in claim 5, characterized in that, The specific mechanism for switching altitude observations is as follows: Step S51: Preset height switching threshold; Step S52: Determine whether the relative height is less than the height switching threshold; if yes, mark the relative height at position 1; if no, mark the relative height at position 0; wherein, position 1 and position 0 are assigned values. Step S53: Determine whether the position of the relative altitude marker changes audibly. If yes, record the current relative altitude, absolute altitude, and altitude of the aircraft. If no, return to step S52.

7. A strong disturbance rejection altitude state estimation system for an aircraft, characterized in that, The system includes: The acquisition module is used to acquire sensor data and estimate sensor delay, and then align the sensor delay to obtain sensor observations. An estimation module is used to estimate the error variance of the sensor based on the sensor observations; The differential calculation module is used to calculate the second derivative of the sensor observation and to calculate the correlation between the second derivative and the inertial measurement acceleration. The altitude calculation module is used to perform data weighted fusion of the sensor observations and the correlation to calculate the relative altitude of the aircraft above the ground; The processing module is used to obtain the current absolute altitude, filter the relative altitude and the absolute altitude using a third-order Kalman filter algorithm, then use an altitude observation switching mechanism to switch the altitude observation to calculate the altitude of the aircraft, and finally use a first-order Kalman filter algorithm to estimate the altitude of the aircraft above the ground. The differential calculation module is also used for: Given the current data as z(0) and the historical data as z(k), where k=1,2,…n-1, the first derivative of the sensor observations is calculated as follows: …（8）； Then record the sequence value of the first derivative of the sensor observation. (k), k=1,2,…n-1, and calculate the second derivative of the sensor observations: …（9）； Record the second derivative sequence values ​​of the sensor observations, and calculate and record the sequence value of the celestial acceleration in the inertial coordinate system: (k),k=1,2,…n-1 …(10); ， …(11)； …(12)； in, The values ​​for the three axes of the accelerometer. The transformation matrix from the aircraft coordinate system to the inertial coordinate system is given; finally, the correlation function g(X) between the second derivative of the observations and the UAV's celestial acceleration is calculated: The related functions …(13) The specific method for calculating the relative height is as follows: The aircraft is presumably equipped with n ranging sensors, and the observation noise variance of each ranging sensor is... The correlation coefficient between the second derivative term of each ranging sensor and the celestial acceleration is: The observations of each of the ranging sensors are Where i = 1, 2, ..., n, and n is a positive integer; Then the weighting coefficients for each of the ranging sensors For i = 1, 2, ..., n, the calculation is as follows: make … (14); but …(15); The observations are weighted and fused to calculate the relative altitude r_H of the aircraft above the ground: …(16)。

Images