A hybrid height measurement method using dynamic integral Kalman filtering

Abstract

This invention relates to a hybrid altitude measurement method employing dynamic integral Kalman filtering. First, a SiA200 MEMS accelerometer is used to measure the carrier's acceleration, and the acceleration is integrated twice to obtain inertial altitude information. An FYC-LG24-800 radio altimeter is used to measure the carrier's radio altitude information. A GPS device is used to measure the GPS altitude information of the carrier. A second-order square root nonlinear smoothing filter is designed to obtain smoothed signals from the three measured altitude signals. Then, a dynamic integral Kalman filter is designed based on the altitude error after the combined Kalman filtering and the nonlinear integral to adjust the variance information, thereby solving for the pairwise Kalman filtered signals of the three altitude smoothing signals. Finally, the error signal is superimposed with its nonlinear integral signal to generate a dynamic combined weighting factor in real time. The three Kalman filtered altitudes are then optimized and combined to obtain the final total altitude output signal.

Description

Technical Field

[0001] This invention relates to the field of high-precision altitude measurement for vehicles, moving bodies, and unmanned aerial vehicles, and more specifically, to a design method for hybrid altitude measurement employing dynamic integral Kalman filtering. Background Technology

[0002] Kalman filtering was originally a method for optimally estimating the system state using the state equations of a linear system and input-output data. Due to the presence of noise and interference in the data, it can be used as a data processing technique for removing noise and restoring accurate data. Various improved algorithms based on Kalman filtering have also seen widespread research and application in recent years, and it is currently the most widely used filtering algorithm in the fields of communication, navigation, and guidance. There are many methods for measuring altitude, such as GPS, radio altimeter, inertial navigation system, and barometric altimeter, each with its own advantages and disadvantages. How to organically combine and fuse various altitude measurement information is a significant and challenging problem. Based on the above background, this invention proposes a Kalman filtering method using dynamic integration to fuse information from multiple altitude measurements, resulting in altitude information with high accuracy and reliability, and thus significant engineering application value.

[0003] It should be noted that the signals described in the background section above are only used to enhance the understanding of the background of the present invention, and therefore may include signals that do not constitute prior art known to those skilled in the art. Summary of the Invention

[0004] The purpose of this invention is to provide a hybrid height measurement method using dynamic integral Kalman filtering, thereby overcoming, to at least a certain extent, the problem of difficulty in fusing information from multiple height measurement devices or low accuracy of the fusion effect caused by the limitations and defects of related technologies.

[0005] According to one aspect of the present invention, a hybrid height measurement method employing dynamic integral Kalman filtering is provided, comprising the following steps:

[0006] Step S10: Install a SiA200 MEMS accelerometer on the carrier and integrate it to obtain the inertial velocity and inertial altitude; install a GPS receiver on the carrier and measure the GPS altitude of the carrier; install a radio altimeter on the carrier and measure the radio altitude of the carrier.

[0007] Step S20: Design a second-order square root nonlinear smoothing filter. Pass the inertial altitude signal data through the second-order square root nonlinear smoothing filter to obtain a smoothed inertial altitude signal. Then, pass the radio altitude signal data through the second-order square root nonlinear smoothing filter to obtain a smoothed radio altitude signal. Finally, pass the GPS altitude signal data through the second-order square root nonlinear smoothing filter to obtain a smoothed GPS signal.

[0008] Step S30: Combine the GPS smoothed signal and the radio altitude smoothed signal into a Kalman filter to obtain a GPS radio combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, then perform nonlinear integration to obtain the error integral signal, and then perform online covariance signal calculation to adjust the Kalman filter.

[0009] Step S40: Combine the GPS smoothed signal and the inertial altitude smoothed signal into a Kalman filter to obtain a GPS-inertial combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, perform nonlinear integration to obtain the error integral signal, and then perform online covariance signal calculation to adjust the Kalman filter.

[0010] Step S50: Combine the inertial smoothed signal and the radio altitude smoothed signal using Kalman filtering to obtain the inertial-radio combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, perform nonlinear integration to obtain the error integral signal, and then perform online covariance signal calculation to adjust the Kalman filter.

[0011] Step S60: Dynamically combine the three combined Kalman filter height signals to obtain an optimized combined height signal, solve for the error signal between the three combined Kalman filter height signals and the optimized combined height signal, perform nonlinear integration and superimpose the corresponding variance signal, generate a dynamic combination weighting factor in real time, and obtain the final total height output signal after optimizing the combination of the three Kalman filter height signals.

[0012] The final altitude output signal is the altitude information obtained by dynamic integral Kalman filtering, which integrates the three measurement information of radio altitude, inertial altitude, and GPS altitude. It is output to the measured and controlled objects such as aircraft as the optimal value of altitude information.

[0013] In one exemplary embodiment of the present invention, a SiA200 MEMS accelerometer is mounted on a carrier, and inertial velocity and inertial altitude are obtained by integration; a GPS receiver is mounted on the carrier to measure the GPS altitude of the carrier; a radio altimeter is mounted on the carrier to measure the radio altitude of the carrier, including:

[0014] v = ∫adt;

[0015] h=∫vdt;

[0016] Where a(n) corresponds to the carrier acceleration signal measured by the SiA200 MEMS accelerometer at time t = n* = T, abbreviated as a, where ΔT is the data output sampling period, dt represents the integral of the time signal, v is the inertial velocity signal, and h is the inertial altitude signal. An FYC-LG24-800 radio altimeter is installed on the carrier to measure the carrier's altitude, denoted as y. w This is called the radio altitude signal. Then, according to the data interval ΔT of the accelerometer, the altitude data is discretized to obtain the data y. w (n), where n = 1, 2, 3…, corresponds to the radio altitude signal data at time t = n * ΔT, where ΔT is the output sampling period of the data. A GPS receiver is installed on the carrier to measure the GPS altitude of the carrier, denoted as y. g This is called GPS altitude information. Then, according to the data interval ΔT of the accelerometer, the altitude data is discretized to obtain the data y. g (n), where n = 1, 2, 3..., corresponds to the GPS altitude signal data at time t = n * ΔT, where ΔT is the output sampling period of the data.

[0017] In one exemplary embodiment of the present invention, passing the inertial altitude signal data through a second-order square root nonlinear smoothing filter to obtain an inertial altitude smoothed signal includes:

[0018]

[0019] e h =h(n)-h l (n);

[0020]

[0021] Where T1, T2, and T3 are constant parameters of the filter; k1 and k2 are constant parameters of the filter gain; ε1 is the square root softening coefficient of the filter, which is constant, and its detailed selection is explained in the case implementation below. Where h1 is the first-order differential signal of inertial height; h l1 The first-order differential signal of the inertial smoothing height; h l2 The signal is the second-order differential of the inertial smoothing height; e h The inertial altitude filtering error signal; h l It is an inertial highly smooth signal.

[0022] In one exemplary embodiment of the present invention, obtaining a smoothed radio altitude signal by passing radio altitude data through a second-order square root nonlinear smoothing filter includes:

[0023]

[0024] e h1 =y w (n)-y wl (n);

[0025]

[0026] Where k3 and k4 are filter gain parameters, which are constants; detailed selection is explained in the case implementation below. Where y w1 The first-order differential signal of radio altitude; y wl1 For radio-smoothed first-order differential signal; y wl2 For radio-smoothed second-order differential signals; e h1 For radio altitude filtering error signal; y wl It is a highly smoothed radio signal.

[0027] In one exemplary embodiment of the present invention, obtaining a smoothed GPS signal by passing GPS altitude signal data through a second-order square root nonlinear smoothing filter includes:

[0028]

[0029] e h2 =y g (n)-y gl (n);

[0030]

[0031] Where k5 and k6 are filter gain parameters, which are constants; detailed selection is explained in the case study below. Where y g1 The first-order differential signal of GPS altitude; y gl1 The first-order differential signal of GPS smoothed altitude; y gl2 e is the second-order differential signal of GPS smoothed altitude; h2 This is the GPS altitude filtering error signal; y gl This is a GPS altitude-smoothed signal.

[0032] In one exemplary embodiment of the present invention, a combined Kalman filter is applied to the GPS smoothed signal and the radio altitude smoothed signal to obtain a GPS-radio combined Kalman filtered altitude signal. The altitude error is then calculated from the combined Kalman filtered altitude signal, followed by nonlinear integration to obtain the error integral signal. Finally, online variance calculation is performed, including:

[0033]

[0034] p w1 (n)=p w (n-1)+Qw1 ;

[0035] b w (n)=p w1 (n)·[p w1 (n)+R w1 ] -1 ;

[0036] y wgk (n)=y wl (n)+b w (n)·[y gl (n)-y wl (n)];

[0037] e a1 (n)=y wgk (n)-y wl (n);

[0038] e a2 (n)=y wgk (n)-y gl (n);

[0039]

[0040] Where p w (n) is the radio error covariance signal in the Kalman filter, p w (1) is p w (n) The value at the first sampling point; b w (n) is the Kalman radio gain, p w1 (n) represents the predicted value of the radio error covariance; Q w1 The prior variance of radio frequency is initially chosen to be constant; detailed calculations are described later. R w1 The initial value of the radio observation variance is chosen to be constant; y wgk (n) represents the GPS radio combined Kalman filtered altitude signal. a1 (n) represents the radio combination altitude error signal; e a2 (n) represents the GPS combined altitude error signal. a1 The radio combination altitude error integral signal and s a2 ρ1 is the GPS combined altitude error integral signal. ρ2 is a constant parameter signal. ρ3 is a constant parameter signal. k a1 k a2 k a3 k a4 This is a constant parameter signal.

[0041] In one exemplary embodiment of the present invention, a combined Kalman filter is applied to the GPS smoothed signal and the inertial altitude smoothed signal to obtain a GPS-inertial combined Kalman filtered altitude signal. The altitude error is then calculated from the combined Kalman filtered altitude signal, followed by nonlinear integration to obtain the error integral signal. Finally, online variance calculation is performed, including:

[0042]

[0043] p i1 (n)=p i (n-1)+Q i1 ;

[0044] b i (n)=p i1 (n)·[p i1 (n)+R i1 ] -1 ;

[0045] y igk (n)=h l (n)+b i (n)·[y gl (n)-h l (n)];

[0046] e b1 (n)=y igk (n)-h l (n);

[0047] e b2 (n)=y igk (n)-y gl (n);

[0048]

[0049] Where p i (n) is the inertial error covariance signal in the Kalman filter, p i (1) is p i (n) The value at the first sampling point; b i (n) is the Kalman inertial gain, p i1 (n) represents the predicted value of the inertial error covariance; Q i1 The inertial prior variance is initially chosen to be constant; detailed calculations are described later. R i1 The initial value of the inertial observation variance is chosen to be constant; y igk (n) represents the GPS inertial combined Kalman filtered altitude signal. b1 (n) represents the inertial combination altitude error signal; e b2 (n) represents the GPS inertial navigation system altitude error signal.b1 The integral signal of the inertial combination height error and s b2 This is the integrated signal of GPS inertial navigation altitude error. k a5 k a6 k a7 k a8 This is a constant parameter signal.

[0050] In one exemplary embodiment of the present invention, the inertial smoothed signal and the radio altitude smoothed signal are combined using Kalman filtering to obtain an inertial-radio combined Kalman filtered altitude signal. The altitude error is then calculated for each of the combined Kalman filtered altitude signals, followed by nonlinear integration to obtain the error integral signal. Finally, the variance signal is calculated online, including:

[0051]

[0052] p wi1 (n)=p wi (n-1)+Q wi1 ;

[0053] b wi (n)=p wi1 (n)·[p wi1 (n)+R wi1 ] -1 ;

[0054] y wik (n)=y wl (n)+b wi (n)·ph l (n)-y wl (n)];

[0055] e c1 (n)=y wik (n)-h l (n);

[0056] e c2 (n)=y wik (n)-y wl (n);

[0057]

[0058] Where p wi (n) is the inertial radio error covariance signal in the Kalman filter, p wi (1) is p wi (n) The value at the first sampling point; b wi (n) represents the Kalman inertial radio gain, p wi1 (n) represents the predicted value of the inertial radio error covariance; Q wi1The a priori variance of inertial radio is initially chosen to be constant; detailed calculations are described later. R wi1 The initial value of the inertial radio observation variance is chosen to be constant; y wik (n) represents the GPS inertial radio combined Kalman filtered altitude signal. c1 (n) represents the inertial relative combination error signal; e c2 (n) represents the radio relative combination error signal. c1 The integral signal of the inertial relative combination error signal and s c2 ε3 is the integral signal of the radio relative combination error signal. k is a constant parameter signal. a9 k a10 k a11 k a12 This is a constant parameter signal.

[0059] In one exemplary embodiment of the present invention, the three combined Kalman filter height signals are dynamically combined to obtain an optimized combined height signal. The error signals of the three combined Kalman filter height signals relative to the optimized combined height signal are calculated, nonlinearly integrated, and then superimposed with the corresponding variance signals. A dynamic combination weighting factor is generated in real time. The final total height output signal obtained after optimizing the combination of the three Kalman filter height signals includes:

[0060] e w1 (n)=y o (n)-y wgk (n)+Q w1 (n)+R w1 (n);

[0061] e w2 (n)=y o (n)-y igk (n)+Q i1 (n)+R i1 (n);

[0062] e w3 (n)=y o (n)-y wik (n)+Q wi1 (n)+R wi1 (n);

[0063] d 11 (n+1)=d 11 (n)+e w1 (n)ΔT;

[0064] d 12 (n+1)=d 12 (n)+e w2 (n)ΔT;

[0065] d 13 (n+1)=d 13 (n)+e w3 (n)ΔT;

[0066]

[0067] y o (n)=d1y wgk (n)+d2y igk (n)+d3y wik (n);

[0068] Where e w1 (n) represents the total error signal of the radio GPS; e w2 (n) represents the total error of inertial GPS, e w3 (n) represents the total radio inertial error signal. d 11 For the total error integration signal of radio GPS, d 12 The inertial GPS total error integral signal, d 13 This represents the integrated signal of the total radio inertial error. d1 is the radio GPS weighting factor, d2 is the inertial GPS weighting factor, and d3 is the radio inertial weighting factor. y o (n) represents the total output signal at height.

[0069] Beneficial effects

[0070] This invention provides a hybrid height measurement method employing dynamic integral Kalman filtering, with the following two innovative aspects: First, it proposes a second-order square root nonlinear smoothing filter, which possesses both nonlinear variation and smoothing functions, effectively smoothing data and filtering out noise. Second, it proposes a Kalman filtering method based on dynamically adjusting the variance signal using error and error integral, and this method can effectively fuse information from three height measurement devices, thereby achieving excellent height measurement results.

[0071] It should be understood that the above general description and the following detailed description are exemplary and explanatory only, and are not intended to limit the invention. Attached Figure Description

[0072] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the invention and, together with the description, serve to explain the principles of the invention. It is obvious that the drawings described below are merely some embodiments of the invention, and those skilled in the art can obtain other drawings based on these drawings without any inventive effort.

[0073] Figure 1This is a flowchart of a hybrid height measurement method using dynamic integral Kalman filtering provided by the present invention;

[0074] Figure 2 This is a physical image of the BN-880Q GPS location receiver used in the method provided in this embodiment of the invention;

[0075] Figure 3 This is a physical image of the ACS100 high-precision MEMS accelerometer provided in the embodiments of the present invention;

[0076] Figure 4 This is the acceleration signal curve (unit: meters per second squared) of the carrier of the method provided in the embodiments of the present invention;

[0077] Figure 5 This is the inertial velocity signal curve (unit: meters per second) of the carrier of the method provided in the embodiments of the present invention;

[0078] Figure 6 This is the inertial height signal curve (unit: meters) of the carrier of the method provided in the embodiments of the present invention;

[0079] Figure 7 This is the GPS altitude signal curve (unit: meters) of the carrier of the method provided in the embodiments of the present invention;

[0080] Figure 8 It is the radio altitude signal (unitless) of the carrier of the method provided in the embodiments of the present invention;

[0081] Figure 9 This is the inertial height smoothing signal curve (unit: meters) of the method provided in the embodiments of the present invention;

[0082] Figure 10 This is the GPS altitude smoothing signal curve (unit: meters) of the method provided in the embodiments of the present invention;

[0083] Figure 11 It is the radio altitude smoothed signal (unit: meters) of the method provided in the embodiments of the present invention;

[0084] Figure 12 This is the total signal curve of the carrier height output (unit: meters) of the method provided in the embodiments of the present invention. Detailed Implementation

[0085] Exemplary embodiments will now be described more fully with reference to the accompanying drawings. However, these exemplary embodiments can be implemented in many forms and should not be construed as limited to the examples set forth herein; rather, these embodiments are provided to make the invention more comprehensive and complete, and to fully convey the concept of the exemplary embodiments to those skilled in the art. The described features, structures, or characteristics can be combined in any suitable manner in one or more embodiments. In the following description, numerous specific details are provided to give a full understanding of embodiments of the invention. However, those skilled in the art will recognize that the technical solutions of the invention may be practiced with one or more of these specific details omitted, or other methods, components, apparatus, steps, etc., may be employed. In other instances, well-known technical solutions are not shown or described in detail to avoid obscuring various aspects of the invention.

[0086] This invention provides a hybrid altitude measurement method employing dynamic integral Kalman filtering. First, a SiA200 MEMS accelerometer is used to measure the carrier's acceleration, and the acceleration is integrated twice to obtain inertial altitude information. An FYC-LG24-800 radio altimeter is used to measure the carrier's radio altitude information. A GPS device is used to measure the GPS altitude information. A second-order square root nonlinear smoothing filter is designed to obtain smoothed signals from the three measured altitude signals. Then, a dynamic integral Kalman filter is designed based on the altitude error after the combined Kalman filtering and the nonlinear integral to adjust the variance information, thereby solving for the pairwise Kalman filtered signals of the three altitude smoothing signals. Finally, the error signal is superimposed with its nonlinear integral signal to generate a dynamic combined weighting factor in real time. The three Kalman filtered altitudes are then optimized and combined to obtain the final total altitude output signal, which serves as the altitude measurement output signal. This method can organically integrate information from three measurement devices and has the advantage of high accuracy.

[0087] The following will further explain and illustrate a hybrid height measurement method using dynamic integral Kalman filtering according to the present invention, with reference to the accompanying drawings. (Reference) Figure 1 As shown, this hybrid height measurement method using dynamic integral Kalman filtering includes the following steps:

[0088] Step S10: Install a SiA200 MEMS accelerometer on the carrier and integrate it to obtain the inertial velocity and inertial altitude; install a GPS receiver on the carrier and measure the GPS altitude of the carrier; install a radio altimeter on the carrier and measure the radio altitude of the carrier.

[0089] Specifically, firstly, a SiA200 MEMS accelerometer is mounted on a carrier, with a physical size of a small 9*9nm package. 2 It is very small, its physical size is as Figure 2As shown. Power consumption is less than 10mW, and the operating temperature range is -40°C to +125°C. The acceleration range is selectable from 2g to 30g, and the measurement resolution can reach 0.00005g. It is mounted on a carrier, and the acceleration signal in the vertical direction of the carrier is measured, denoted as a(n). Where n = 1, 2, 3…, corresponding to the acceleration signal at time t = n*ΔT, where ΔT is the data output sampling period.

[0090] Next, the acceleration measurement data of the carrier is integrated to obtain the inertial velocity signal, denoted as v(n). Then, the velocity signal is further integrated to obtain the inertial height signal, denoted as h(n). The integration process is shown below:

[0091] v = ∫adt; h = ∫vdt;

[0092] Where dt represents the integral over the time signal.

[0093] Secondly, an FYC-LG24-800 radio altimeter was mounted on the carrier. Its physical dimensions are 120×66×27mm, and its weight is less than 400g. Figure 3 As shown, the data update rate is 20Hz. The height of the measured carrier is denoted as y. w Then, according to the data interval ΔT of the accelerometer, the height data is discretized to obtain the data y. w (n), where n = 1, 2, 3..., corresponds to the radio altitude measurement data at time t = n * ΔT, where ΔT is the output sampling period of the data.

[0094] Finally, a GPS receiver is installed on the carrier, and the GPS altitude of the carrier is measured and recorded as y. g Then, according to the data interval ΔT of the accelerometer, the height data is discretized to obtain the data y. g (n), where n = 1, 2, 3..., corresponds to the GPS altitude measurement data at time t = n * ΔT, where ΔT is the data output sampling period.

[0095] Step S20: Design a second-order square root nonlinear smoothing filter. Pass the inertial altitude data through the second-order square root nonlinear smoothing filter to obtain an inertial altitude smoothed signal. Then, pass the radio altitude data through the second-order square root nonlinear smoothing filter to obtain a radio altitude smoothed signal. Finally, pass the GPS altitude data through the second-order square root nonlinear smoothing filter to obtain a GPS smoothed signal.

[0096] Specifically, the following nonlinear second-order smoothing filter is first designed, and the inertial altitude signal data is then passed through this filter:

[0097]

[0098] e h =h(n)-h l (n);

[0099]

[0100] Where T1, T2, and T3 are constant parameters of the filter; k1 and k2 are constant parameters of the filter gain; ε1 is the square root softening coefficient of the filter, which is constant, and its detailed selection is explained in the case implementation below. Where h1 is the first-order differential signal of inertial height; h l1 The first-order differential signal of the inertial smoothing height; h l2 The signal is the second-order differential of the inertial smoothing height; e h The inertial altitude filtering error signal; h l It is an inertial highly smooth signal;

[0101] Secondly, the radio altitude data is passed through a second-order square root nonlinear smoothing filter to obtain the smoothed radio altitude signal as follows:

[0102]

[0103] e h1 =y w (n)-y wl (n);

[0104]

[0105] Where k3 and k4 are filter gain parameters, which are constants; detailed selection is explained in the case implementation below. Where y w1 The first-order differential signal of radio altitude; y wl1 For radio-smoothed first-order differential signal; y wl2 For radio-smoothed second-order differential signals; e h1 For radio altitude filtering error signal; y wl It is a highly smooth radio signal;

[0106] Finally, the GPS altitude data is passed through a second-order square root nonlinear smoothing filter to obtain the smoothed GPS signal as follows:

[0107]

[0108] e h2 =y g (n)-y gl (n);

[0109]

[0110] Where k5 and k6 are filter gain parameters, which are constants; detailed selection is explained in the case study below. Where yg1 The first-order differential signal of GPS altitude; y gl1 The first-order differential signal of GPS smoothed altitude; y gl2 e is the second-order differential signal of GPS smoothed altitude; h2 This is the GPS altitude filtering error signal; y gl This is a GPS altitude-smoothed signal.

[0111] Step S30: Combine the GPS smoothed signal and the radio altitude smoothed signal into a Kalman filter to obtain a GPS radio combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, then perform nonlinear integration to obtain the error integral signal, and then perform online covariance signal calculation to adjust the Kalman filter.

[0112] Specifically, the GPS smoothed signal and the radio altitude smoothed signal are combined using Kalman filtering to obtain the GPS-radio combined Kalman filtered altitude signal as follows:

[0113]

[0114] p w1 (n)=p w (n-1)+Q w1 ;

[0115] b w (n)=p w1 (n)·[p w1 (n)+R w1 ] -1 ;

[0116] y wgk (n)=y wl (n)+b w (n)·[y gl (n)-y wl (n)];

[0117] Where p w (n) is the radio error covariance signal in the Kalman filter, p w (1) is p w (n) The value at the first sampling point; b w (n) is the Kalman radio gain, p w1 (n) represents the predicted value of the radio error covariance; Q w1 The prior variance of radio frequency is initially chosen to be constant; detailed calculations are described later. R w1 The initial value of the radio observation variance is chosen to be constant; y wgk (n) represents the GPS radio combined Kalman filtered altitude signal.

[0118] Secondly, the combined height error is calculated from the combined Kalman filter height signal as follows:

[0119] e a1 (n)=y wgk (n)-y wl (n);

[0120] e a2 (n)=y wgk (n)-y gl (n);

[0121] Where e a1 (n) represents the radio combination altitude error signal; e a2 (n) represents the GPS combined altitude error signal.

[0122] Then, nonlinear integration is performed on the radio combined altitude error signal and the GPS combined altitude error signal to obtain the integrated radio combined altitude error signal and the integrated GPS combined altitude error signal as follows:

[0123]

[0124] Where s a1 The radio combination altitude error integral signal and s a2 ε2 is the GPS combined altitude error integral signal. ε2 is a constant parameter signal.

[0125] Finally, based on the above combined altitude error integral signal and combined altitude error signal, the covariance signal is calculated online to solve the radio prior variance and radio observation variance, and the Kalman filter is adjusted as follows:

[0126]

[0127] Where ε3 is a constant parameter signal. a1 k a2 k a3 k a4 This is a constant parameter signal. Q w1 For radio prior variance and R w1 This represents the variance of radio observations.

[0128] Step S40: Combine the GPS smoothed signal and the inertial altitude smoothed signal into a Kalman filter to obtain a GPS-inertial combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, perform nonlinear integration to obtain the error integral signal, and then perform online covariance signal calculation to adjust the Kalman filter.

[0129] Specifically, the GPS smoothed signal and the inertial altitude smoothed signal are combined using Kalman filtering to obtain the GPS-inertial combined Kalman filtered altitude signal as follows:

[0130]

[0131] p i1 (n)=p i (n-1)+Q i1 ;

[0132] b i (n)=p i1 (n)·[p i1 (n)+R i1 ] -1 ;

[0133] y igk (n)=h l (n)+b i (n)·[y gl (n)-h l (n)];

[0134] Where p i (n) is the inertial error covariance signal in the Kalman filter, p i (1) is p i (n) The value at the first sampling point; b i (n) is the Kalman inertial gain, p i1 (n) represents the predicted value of the inertial error covariance; Q i1 The inertial prior variance is initially chosen to be constant; detailed calculations are described later. R i1 The initial value of the inertial observation variance is chosen to be constant; y igk (n) represents the GPS inertial Kalman filter altitude signal.

[0135] Secondly, the combined height error is calculated from the combined Kalman filter height signal as follows:

[0136] e b1 (n)=y igk (n)-h l (n);

[0137] e b2 (n)=y igk (n)-y gl (n);

[0138] Where e b1 (n) represents the inertial combination altitude error signal; e b2 (n) represents the GPS inertial combination altitude error signal.

[0139] Then, nonlinear integration is performed on the inertial combined altitude error signal and the GPS inertial combined altitude error signal to obtain the following inertial combined altitude error integrated signal and GPS inertial combined altitude error integrated signal:

[0140]

[0141]

[0142] Where s b1 The integral signal of the inertial combination height error and s b2 This is the GPS inertial combination altitude error integral signal.

[0143] Finally, based on the above combined height error integral signal and combined height error signal, the covariance signal is calculated online to solve the inertial prior variance and inertial observation variance, and the Kalman filter is adjusted as follows:

[0144]

[0145] Where k a5 k a6 k a7 k a8 This is a constant parameter signal. Q i1 For inertial prior variance and R i1 This represents the variance of inertial observations.

[0146] Step S50: Combine the inertial smoothing signal and the radio altitude smoothing signal into a Kalman filter to obtain the inertial-radio combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, then perform nonlinear integration to obtain the error integral signal, and then perform online covariance signal calculation to adjust the Kalman filter.

[0147] Specifically, the inertial smoothed signal and the radio altitude smoothed signal are combined using Kalman filtering to obtain the inertial-radio combined Kalman filtered altitude signal as follows:

[0148]

[0149] p wi1 (n)=p wi (n-1)+Q wi1 ;

[0150] b wi (n)=p wi1 (n)·[p wi1 (n)+R wi1 ] -1 ;

[0151] y wik (n)=y wl(n)+b wi (n)·[h l (n)-y wl (n)];

[0152] Where p wi (n) is the inertial radio error covariance signal in the Kalman filter, p wi (1) is p wi (n) The value at the first sampling point; b wi (n) represents the Kalman inertial radio gain, p wi1 (n) represents the predicted value of the inertial radio error covariance; Q wi1 The a priori variance of inertial radio is initially chosen to be constant; detailed calculations are described later. R wi1 The initial value of the inertial radio observation variance is chosen to be constant; y wik (n) represents the GPS inertial radio combined Kalman filtered altitude signal.

[0153] Secondly, the combined height error is calculated from the combined Kalman filter height signal as follows:

[0154] e c1 (n)=y wik (n)-h l (n);

[0155] e c2 (n)=y wik (n)-y wl (n);

[0156] Where e c1 (n) represents the inertial relative combination error signal; e c2 (n) represents the radio relative combination error signal.

[0157] Then, nonlinear integration is performed on the inertial relative combination error signal and the radio relative combination error signal to obtain the inertial relative combination error integral signal and the radio relative combination error integral signal as follows:

[0158]

[0159] Where s c1 The integral signal of the inertial relative combination error signal and s c2 It is the integrated signal of the relative combination error signal of radio.

[0160] Finally, based on the above relative combination error integral signal and relative combination error signal, the covariance signal is calculated online to solve the inertial radio prior variance and inertial radio observation variance, and the Kalman filter is adjusted as follows:

[0161]

[0162] Where ε3 is a constant parameter signal. a9 k a10 k a11 k a12 This is a constant parameter signal. Q wi1 For inertial radio prior variance and R wi1 This represents the variance of inertial radio observations.

[0163] Step S60: Dynamically combine the three combined Kalman filter height signals to obtain an optimized combined height signal, solve for the error signal between the three combined Kalman filter height signals and the optimized combined height signal, perform nonlinear integration and superimpose the corresponding variance signal, generate a dynamic combination weighting factor in real time, and obtain the final total height output signal after optimizing the combination of the three Kalman filter height signals.

[0164] Specifically, based on the aforementioned data of radio prior variance and radio observation variance, inertial prior variance and inertial observation variance, and inertial radio prior variance and inertial radio observation variance, the total radio GPS error, total inertial GPS error, and total radio inertial error signals are calculated as follows:

[0165] e w1 (n)=y o (n)-y wgk (n)+Q w1 (n)+R w1 (n);

[0166] e w2 (n)=y o (n)-y igk (n)+Q i1 (n)+R i1 (n);

[0167] e w3 (n)=y o (n)-y wik (n)+Q wi1 (n)+R wi1 (n);

[0168] Where e w1 (n) represents the total error signal of the radio GPS; e w2 (n) represents the total error of inertial GPS, e w3 (n) represents the total radio inertial error signal.

[0169] Then, the total radio GPS error, total inertial GPS error, and total radio inertial error signals are integrated separately to obtain the integrated signals of the total radio GPS error, total inertial GPS error, and total radio inertial error, as follows:

[0170] d 11 (n+1)=d 11 (n)+e w1 (n)ΔT;

[0171] d 12 (n+1)=d 12 (n)+e w2 (n)ΔT;

[0172] d 13 (n+1)=d 13 (n)+e w3 (n)ΔT;

[0173] Where d 11 For the total error integration signal of radio GPS, d 12 The inertial GPS total error integral signal, d 13 It is the integrated signal of the total radio inertial error.

[0174] The integrated signals of the total error of radio GPS, inertial GPS, and radio inertial are then weighted to obtain the radio GPS weighting factor, inertial GPS weighting factor, and radio inertial weighting factor, as follows:

[0175]

[0176] Where d1 is the radio GPS weighting factor, d2 is the inertial GPS weighting factor, and d3 is the radio inertial weighting factor.

[0177] Finally, based on the weighting factors mentioned above, the three combined Kalman filter height signals are dynamically combined to obtain the optimized combined height signal as follows:

[0178] y o (n)=d1y wgk (n)+d2y igk (n)+d3y wik (n)

[0179] Where y o (n) represents the total output signal at height.

[0180] Case Implementation and Computer Simulation Results Analysis

[0181] In step S10, ΔT = 0.001 is set, and a SiA200 MEMS accelerometer is installed on the carrier to measure the acceleration signal of the carrier. Figure 4 As shown, the inertial velocity signal of the carrier is obtained by integration. Figure 5 As shown, the inertial altitude signal of the carrier is obtained by further integration. Figure 6 As shown, the GPS altitude of the carrier was obtained using GPS measurements. Figure 7 As shown. The radio altitude of the carrier was obtained by measuring with a radio altimeter, as shown below. Figure 8 As shown.

[0182] In step S20, T1 = 0.1, T2 = 0.7, T3 = 0.005, k1 = 1.5, k2 = 3.2, ε1 = 0.58; k3 = 2.5, k4 = 1.8; k5 = 1.5, k6 = 2.6; in step S30, ε2 = 0.4, ε3 = 0.5. a1 =0.35, k a2 =0.25, k a3 =0.62, k a4 =0.25. In step S40, k is selected. a5 =0.32, k a6 =0.55, k a7 =0.22, k a8 =0.63. In step S50, k is selected. a9 =0.25, k a10 =0.56, k a11 =0.38, k a12 =0.42.

[0183] In step S20, the inertial smoothing height signal is obtained as follows: Figure 9 As shown, GPS smoothed altitude is as follows Figure 10 As shown, the radio smoothing height is as follows Figure 11 As shown. In step S60, the final total height output signal is obtained as follows: Figure 12 As shown. By Figure 12 It can be seen that the final total output height measurement accuracy reached about 0.15 meters in the static experiment, while the accuracy of each individual measurement method was above 0.4 meters. This shows that the method provided by the present invention has a good height measurement effect and thus has high engineering application value.

[0184] Other embodiments of the invention will readily occur to those skilled in the art upon consideration of the specification and practice of such inventions. This application is intended to cover any variations, uses, or adaptations of the invention that follow the general principles of the invention and include common knowledge or customary techniques in the art not specified herein. The specification and embodiments are to be considered exemplary only, and the true scope and spirit of the invention are indicated by the claims.

Claims

1. A hybrid height measurement method employing dynamic integral Kalman filtering, characterized in that, Includes the following steps: Step S10: Install a SiA200 MEMS accelerometer on the carrier and integrate the data to obtain the inertial velocity and inertial altitude; install a GPS receiver on the carrier and measure the carrier's GPS altitude; install a radio altimeter on the carrier and measure the carrier's radio altitude as follows: v = ∫adt; h=∫vdt; Where a(n) corresponds to the carrier acceleration signal measured by the SiA200 MEMS accelerometer at time t = n*ΔT, abbreviated as a, where ΔT is the data output sampling period, dt represents the integral of the time signal, v is the inertial velocity signal, and h is the inertial altitude signal; an FYC-LG24-800 radio altimeter is installed on the carrier to measure the carrier altitude, denoted as y. w This is called the radio altitude signal. Then, according to the data interval ΔT of the accelerometer, the altitude data is discretized to obtain the data y. w (n), where n = 1, 2, 3…, corresponds to the radio altitude signal data at time t = n*ΔT; a GPS receiver is installed on the carrier to measure the GPS altitude of the carrier, denoted as y. g This is called the GPS altitude signal. Then, according to the data interval ΔT of the accelerometer, the altitude data is discretized to obtain the data y. g (n), where n = 1, 2, 3..., corresponds to the GPS altitude signal data at time t = n*ΔT; Step S20: Design a second-order square root nonlinear smoothing filter. Pass the inertial altitude signal data through the second-order square root nonlinear smoothing filter to obtain a smoothed inertial altitude signal. Then, pass the radio altitude signal data through the second-order square root nonlinear smoothing filter to obtain a smoothed radio altitude signal. Finally, pass the GPS altitude signal data through the second-order square root nonlinear smoothing filter to obtain the smoothed GPS signal as follows: e h =h(n)-h l (n); and h1 / and w (n)-y wl (n); and h2 / and g (n)-y gl (n); Where T1, T2, and T3 are constant parameters of the filter; k1, k2, k3, k4, k5, and k6 are constant parameters of the filter gain; ε1 is the square root softening coefficient of the filter; h1 is the first-order differential signal of the inertial height; h l1 The first-order differential signal of the inertial smoothing height; h l2 The signal is the second-order differential of the inertial smoothing height; e h The inertial altitude filtering error signal; h l For highly smoothed inertial signals; y w1 The first-order differential signal of radio altitude; y wl1 For radio-smoothed first-order differential signal; y wl2 For radio-smoothed second-order differential signals; e h1 For radio altitude filtering error signal; y wl For highly smoothed radio signals; y g1 The first-order differential signal of GPS altitude; y gl1 The first-order differential signal of GPS smoothed altitude; y gl2 e is the second-order differential signal of GPS smoothed altitude; h2 This is the GPS altitude filtering error signal; y gl This is a GPS altitude-smoothed signal; Step S30: Perform a combined Kalman filter on the GPS smoothed signal and the radio altitude smoothed signal to obtain a GPS-radio combined Kalman filtered altitude signal. Solve for the altitude error on the combined Kalman filtered altitude signal, then perform nonlinear integration to obtain the error integral signal. Finally, perform online covariance signal calculation and adjust the Kalman filter as follows: p w1 (n)=p w (n-1)+Q w1 ; b w (n)=p w1 (n)·[p w1 (n)+R w1 ] -1 ; and wgk (n)=y wl (n)+b w (n)·[and gl (n)-y wl (n)]; and a1 (n)=y wgk (n)-y wl (n); and a2 (n)=y wgk (n)-y gl (n); Where p w (n) is the radio error covariance signal in the Kalman filter, p w (1) is p w (n) The value at the first sampling point; b w (n) is the Kalman radio gain, p w1 (n) represents the predicted value of the radio error covariance; Q w1 The prior variance of radio waves is initially chosen to be constant, Ra. w1 The initial value of the radio observation variance is chosen to be constant; y wgk (n) represents the GPS radio combined Kalman filtered altitude signal; e a1 (n) represents the radio combination altitude error signal; e a2 (n) represents the GPS combined altitude error signal; s a1 The radio combination altitude error integral signal and s a2 ε1 is the GPS combined altitude error integral signal; ε2 is the constant parameter signal; ε3 is the constant parameter signal; k a1 k a2 k a3 k a4 For constant parameter signals; Step S40: Combine the GPS smoothed signal and the inertial altitude smoothed signal using Kalman filtering to obtain the GPS-inertial combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, then perform nonlinear integration to obtain the error integral signal. Finally, perform online covariance signal calculation and adjust the Kalman filter as follows: p i1 (n)=p i (n-1)+Q i1 ; b i (n)=p i1 (n)·[p i1 (n)+R i1 ] -1 ; y igk (n)=h l (n)+b i (n)·[y gl (n)-h l (n)]; e b1 (n)=y igk (n)-h l (n); and b2 (n)=y igk (n)-y gl (n); Where p i (n) is the inertial error covariance signal in the Kalman filter, p i (1) is p i (n) The value at the first sampling point; b i (n) is the Kalman inertial gain, p i1 (n) represents the predicted value of the inertial error covariance; Q i1 The inertial prior variance is initially chosen to be a constant, R. i1 The initial value of the inertial observation variance is chosen to be constant; y igk (n) represents the GPS inertial combined Kalman filtered altitude signal; e b1 (n) represents the inertial combination altitude error signal; e b2 (n) represents the GPS inertial combination altitude error signal; s b1 The integral signal of the inertial combination height error and s b2 The integral signal of GPS inertial combination altitude error; k a5 k a6 k a7 k a8 For constant parameter signals; Step S50: Combine the inertial smoothed signal and the radio altitude smoothed signal using Kalman filtering to obtain the inertial-radio combined Kalman filtered altitude signal. Solve for the altitude error of the combined Kalman filtered altitude signal, then perform nonlinear integration to obtain the error integral signal. Finally, perform online covariance signal calculation and adjust the Kalman filter as follows: p wi1 (n)=p wi (n-1)+Q wi1 ; b wi (n)=p wi1 (n)·[p wi1 (n)+R wi1 ] -1 ; y wik (n)=y wl (n)+b wi (n)·[h l (n)-y wl (n)]; e c1 (n)=y wik (n)-h l (n); and c2 (n)=y wik (n)-y wl (n); Where p wi (n) is the inertial radio error covariance signal in the Kalman filter, p wi (1) is p wi (n) The value at the first sampling point; b wi (n) represents the Kalman inertial radio gain, p wi1 (n) represents the predicted value of the inertial radio error covariance; Q wi1 The prior variance of inertial radio is initially chosen to be constant, R. wi1 The initial value of the inertial radio observation variance is chosen to be constant; y wik (n) represents the GPS inertial radio combined with Kalman filtered altitude signal; e c1 (n) represents the inertial relative combination error signal; e c2 (n) represents the radio relative combination error signal; s c1 The integral signal of the inertial relative combination error signal and s c2 ε3 is the integrated signal of the radio relative combination error signal; k is the constant parameter signal; a9 k a10 k a11 k a12 For constant parameter signals; Step S60: Dynamically combine the three combined Kalman filter height signals to obtain an optimized combined height signal. Solve for the error signal between the three combined Kalman filter height signals and the optimized combined height signal. Perform nonlinear integration and superimpose the corresponding variance signal to generate a dynamic combination weighting factor in real time. After optimizing the combination of the three Kalman filter height signals, the final total height output signal is obtained as follows: e w1 (n)=y o (n)-y wgk (n)+Q w1 (n)+R w1 (n); e w2 (n)=y o (n)-y igk (n)+Q i1 (n)+R i1 (n); e w3 (n)=y o (n)-y wik (n)+Q wi1 (n)+R wi1 (n); d 11 (n+1)=d 11 (n)+e w1 (n)ΔT; d 12 (n+1)=d 12 (n)+e w2 (n)ΔT; d 13 (n+1)=d 13 (n)+e w3 (n)ΔT; y o (n)=d1y wgk (n)+d2y igk (n)+d3y wik (n); where e w1 (n) represents the total error signal of the radio GPS; e w2 (n) represents the total error of inertial GPS, e w3 (n) represents the total radio inertial error signal; d 11 For the total error integration signal of radio GPS, d 12 The inertial GPS total error integral signal, d 13 d1 is the integrated signal of total radio inertial error; d2 is the radio GPS weighting factor, d3 is the inertial GPS weighting factor, and d4 is the radio inertial weighting factor; y o (n) represents the total output signal at height.

Images