A GPS / AOA / SINS integrated navigation method based on federated Kalman filter

Abstract

The present invention discloses a GPS / AOA / SINS integrated navigation method based on federated Kalman filtering, which belongs to the technical field of calculation, estimation or counting. The method combines the complementary advantages of GPS, AOA and SINS, and performs optimal fusion through federated Kalman, thereby greatly improving positioning accuracy. A fault detection and fault processing module is added between the federated Kalman sub-filter and the main filter. When an abnormality is detected, the filter gain matrix K of the fault sub-filter is adaptively adjusted using the ratio of the threshold set by the residual chi-square detection method to the fault detection function, thereby changing the proportion of the predicted value and the measured value in the fault sub-filter, and further fusing the federated Kalman filter result after fault processing with the time update value of the fault sub-filter. The method can effectively suppress positioning divergence caused by abnormal values ​​and meet the requirements of the integrated navigation system for reliability and stability.

Description

Technical Field

[0001] The present invention relates to an integrated navigation and positioning technology, and specifically discloses a GPS / AOA / SINS integrated navigation method based on a federated Kalman filter, which is used to compensate for the error of individual GPS positioning in a complex outdoor environment and belongs to the technical field of calculation, estimation or counting. Background Art

[0002] Navigation and positioning systems have gradually developed to meet people's needs in daily life and production. The Global Positioning System (GPS) is widely used and offers high positioning accuracy, with positioning errors that do not accumulate over time. However, because GPS is non-autonomous, its positioning results are significantly affected by the environment. In urban canyons, tunnels, and mountainous areas, satellite signals are often blocked and cannot function properly. GPS also has inherent shortcomings, such as low navigation data sampling frequency and an inability to provide carrier attitude information.

[0003] With urban development, traffic conditions have become increasingly complex, with severe obstruction between buildings. GPS positioning accuracy has plummeted due to severe interference with satellite signals. Using GPS alone has proven difficult to meet navigation performance requirements in complex urban environments. Integrated navigation technology has become an effective approach to improving the overall performance of navigation systems. Strapdown Inertial Navigation Systems (SINS) can compensate for the inherent shortcomings of GPS. SINS utilizes gyroscopes and accelerometers mounted on a carrier to measure the carrier's motion parameters relative to inertial space and calculates navigation information such as the carrier's velocity, position, and three-dimensional attitude through integration. While offering numerous advantages, such as high sampling frequency, low noise, strong stealth, and immunity to external interference, SINS positioning errors accumulate over time due to the integration involved in the solution process, resulting in poor long-term accuracy. Furthermore, SINS boasts a simple hardware structure, small size, and light weight, making it commonly used in land-based navigation. Using SINS to assist GPS positioning can increase redundant observations, improve the dynamic characteristics of the satellite receiver, and correct for accumulated SINS errors.

[0004] However, when satellite signals are severely interfered with, GPS positioning errors increase, and SINS becomes ineffective due to the inability to correct accumulated errors. The introduction of 5G base station positioning offers new possibilities for compensating for GPS errors. 5G base station positioning uses the Angle of Arrival (AOA)-based positioning method, which uses array antennas to estimate the arrival angle of the transmitted signal and construct rays centered at two base stations. The intersection of these two rays represents the location of the transmitting source. 5G massive antenna array technology increases the accuracy of AOA estimation. Ultra-dense networking provides a higher probability of Line of Sight (LOS) for AOA positioning. Angle estimation eliminates the need for time synchronization between base stations. These technical conditions significantly improve AOA positioning accuracy.

[0005] The Federated Kalman Filter (FKF), proposed by Carlson, is based on the principle of first processing the information from multiple subfilters and then integrating them together. It utilizes the variance upper bound technique and the principle of information conservation to eliminate the correlation between local filters. It also offers excellent fault tolerance, high filtering accuracy, and a simple, computationally inefficient fusion algorithm from local to global filtering. However, when a GPS subsystem malfunctions, conventional FKFs cannot effectively suppress positioning divergence. Furthermore, existing FKFs have limited fault detection methods and low-accuracy fault handling methods. Therefore, further improvements to the Federated Kalman Filter are needed.

[0006] The present invention aims to integrate the advantages of GPS, AOA and SINS through FKF, so that the combined navigation system composed of the three can effectively solve the problems of GPS positioning alone, provide carrier position, speed and attitude information, enhance the system's continuous positioning capability, overcome the influence of complex environments, and achieve high-precision positioning in complex urban environments. Summary of the Invention

[0007] The purpose of the present invention is to address the shortcomings of the above-mentioned background technology and the inherent problems of insufficient observation volume and small line-of-sight range of single GPS positioning, and to provide a GPS / AOA / SINS combined navigation method based on federated Kalman filtering to solve the technical problems that the accuracy of single GPS positioning is not high and the conventional federated Kalman filtering cannot effectively suppress the divergence of the combined navigation filtering results, thereby achieving the purpose of high-precision positioning and high-reliability positioning in complex urban environments.

[0008] To achieve the above-mentioned purpose, the present invention adopts the following technical solutions: the positioning error of the GPS / AOA / SINS integrated navigation system and the improvement problem of GPS positioning alone are analyzed and a mathematical model is established. Based on the federated Kalman filter and fault detection and fault handling means, a method for suppressing positioning abnormal divergence is proposed.

[0009] In an improved federated Kalman filter integrated navigation system, SINS serves as the reference system for the main filter and two sub-filters. The output signal of sub-filter 1 is the fusion result of SINS and GPS, while the output signal of sub-filter 2 is the fusion result of SINS and AOA. Before the sub-filter state estimates enter the main filter for fusion, they must be subjected to fault detection and processing. Fault detection utilizes a combination of residual chi-square detection and sliding window averaging based on the innovation sequence. Once a sub-filter fault is determined, it indicates that the state estimate for that sub-filter is incorrect. The ratio of the threshold in the residual chi-square detection method to the fault detection function is used to adaptively adjust the faulty sub-filter. The processed result is then fed into the main filter for fusion, and the fusion result is further fused with the time-update value of the faulty sub-filter. This structure improves the federated Kalman filter by utilizing useful information contained in abnormal measurements to reduce the impact of abnormal errors on filtering. By reintegrating the global state estimate with the time-update value of the faulty sub-filter, the filtering accuracy and fault tolerance of the integrated navigation system are improved, enabling a certain degree of real-time positioning.

[0010] The state equation of the integrated navigation system is shared by all sub-filters. Based on the analysis of the error sources of the SINS, the error of the SINS navigation parameters is used as the state of the system. The navigation coordinate system is the local east-north-sky coordinate system, and the 15-dimensional state variables are selected to be expressed as follows:

[0011]

[0012] Among them, φ E 、φ N 、φ U They represent the platform attitude angle errors in the east, north and sky directions in the navigation coordinate system, δv E ,δv N ,δv U It represents the velocity error in the east, north and sky directions in the navigation coordinate system. δL, δλ and δh represent the latitude, longitude and elevation errors in the navigation coordinate system respectively. is the gyroscope drift error along the x-axis, y-axis, and z-axis of the carrier coordinate system, and is assumed to be a random constant drift. is the accelerometer bias error and is assumed to be a random constant bias with zero bias.

[0013] The state equation of the integrated navigation system is as follows:

[0014]

[0015] In formula (2), F(t) is the state transfer matrix, G(t) is the control matrix, and W(t) is the random noise of the gyroscope, which is assumed to be Gaussian white noise.

[0016] W(t)=[ε gx ε gy ε gz ε ax ε ay ε az ] T (3),

[0017] Among them, ε gx , ε gy , ε gz Respectively represent the components of the gyroscope Gaussian white noise along the x-axis, y-axis, and z-axis of the carrier coordinate system; ε ax , ε ay , ε az They represent the components of the accelerometer Gaussian white noise along the x-axis, y-axis, and z-axis of the carrier coordinate system respectively.

[0018] The SINS / GPS and SINS / AOA sub-filters are combined to correct the attitude, velocity and position errors of the SINS error model.

[0019] Sub-filter 1 takes the difference between the speed and position information output by GPS navigation and the speed and position information output by SINS navigation, and obtains the measurement equation of sub-filter 1 as follows:

[0020]

[0021] H1(t) is the measurement matrix:

[0022]

[0023] In formula (5), V SINSE 、V SINSN 、V SINSU 、L SINS ,λ SINS 、h SINS are the speed and position information measured by SINS, V GPSE 、V GPSN 、V GPSU 、L GPS ,λ GPS 、h GPS are the speed and position information measured by GPS, V1(t)=[v Vgpse v Vgpsn v Vgpsu v gl v gλ v gh ] T, where v Vgpse 、v Vgpsn 、v Vgpsu They are the GPS velocity measurement noise in the east, north and sky directions in the navigation coordinate system, v gl 、v gλ 、v gh They are the GPS latitude, accuracy and elevation position measurement noise in the navigation coordinate system.

[0024] Subfilter 2 is the difference between the position given by the 5G base station arrival angle positioning and the position given by the SINS system. The measurement equation of subfilter 2 is:

[0025]

[0026] H2(t) is the measurement matrix:

[0027] H2(t)=[0 3x6 I 3x3 0 3x6 ] (7)

[0028] In formula (6), L SINS ,λ SINS , h SINS is the position information measured by SINS, L AOA ,λ AOA , h AOA is the position information of the 5G base station arrival angle positioning, V2(t)=[v aoae v aoan v aoau ] T , where v aoae 、v aoan 、v aoau They are the position measurement noise of the 5G base station arrival angle positioning along the east, north and sky directions in the navigation coordinate system.

[0029] In this method, the federated Kalman filter adopts a reset structure. This structure has good fault isolation capabilities for the sub-filters of the federated Kalman filter, but poor fault isolation capabilities for the main filter. The global filter after fusion has high accuracy, and the accuracy of the sub-filters is also improved due to the feedback reset.

[0030] The state equations and measurement equations of the SINS / GPS / AOA integrated navigation model are discretized, and the resulting system model is shown below:

[0031] X(k)=F(k,k-1)X(k-1)+G(k-1)W(k-1) (8)

[0032] Z i (k)=H i(k)X(k)+V i (k)i=1,2 (9)

[0033] Among them, X(k) is the state vector of the integrated navigation system, F(k,k-1) is the first-order transfer matrix of the system, G(k) is the noise matrix of the system, and W(k) is the noise vector of the system. The covariance matrix of W(k) is Q(k), V i The covariance matrix of (k) is R i (k).

[0034] The main filter fusion equation is:

[0035]

[0036] The main filter then feeds back information to each sub-filter. This process distributes the global optimal estimation information among the sub-filters:

[0037]

[0038] Where i=1,2,...,N is the subscript of the subfilter; P i , Q i are the prediction covariance matrix of the sub-filter and the process noise covariance matrix, is the state estimate of the sub-filter; P g , Q g are the global prediction covariance matrix and the process noise covariance matrix, is the global state estimate; since the main filter has no information allocation and no filtering is performed, its estimate Take it as m is the subscript of the main filter. i (0<β i ≤1) represents the information allocation coefficient of the i-th sub-filter, which needs to satisfy the information conservation principle shown in formula (12). The information allocation coefficient of the main filter β m Equal to 0.

[0039]

[0040] While GPS / AOA / SINS fusion can achieve high-precision positioning, the GPS subsystem is prone to positioning anomalies and even satellite invisibility in complex environments, resulting in a decrease in the positioning accuracy of the integrated navigation system. The state estimates output by the federated Kalman subfilters with feedback, after being fused with the main filter, can further contaminate other subfilters through feedback, leading to unreliable or even divergent positioning results. Therefore, this invention incorporates a fault detection and processing unit between the subfilters and the main filter to process faults in real time, thereby ensuring the accuracy of the global state estimate.

[0041] Chi-squared fault detection is often used in navigation systems. Residual chi-squared fault detection, which leverages measurement residuals (i.e., innovations), can effectively detect and isolate sudden faults in certain sub-filters. This method also offers a simple algorithm. However, due to the threshold settings, there is a certain probability of missed detections and false detections, so it must be used in conjunction with other fault detection methods.

[0042] In Kalman filtering, the residual is expressed as:

[0043] r k =Z k -H k X k|k-1 (13)

[0044] In formula (13), Z k is the measurement information of the filter at time k, X k|k-1 is the one-step prediction information of the filter.

[0045] When the navigation system model is accurate, the measurement information Z at time k k When there is no abnormality, the residual r of the Kalman filter is k is Gaussian white noise with a mean of 0, as shown in formula (14):

[0046] E{r k}=0 (14)

[0047] Its variance is:

[0048]

[0049] When a fault occurs in the system, the mean of the residual is no longer 0.

[0050] E{r k}=μ,E{(r k -μ)(r k -μ) T}=A k (16)

[0051] The fault detection function is defined as follows:

[0052]

[0053] Among them, λ k ~χ 2 (m), subject to the chi-square with m degrees of freedom (χ 2 ) distribution, m is the measurement value Z k dimension.

[0054] When no fault occurs, H k Xk|k-1 =Z k It is Z k The best prediction estimate, r k Theoretically small; but when a sudden failure occurs, Z k The mutation makes the residual r k There will be a large deviation, λ k Therefore, it is necessary to set up corresponding detection rules to detect the occurrence of faults.

[0055] The detection rule is: when λ k >T d When λ k ≤T d When , it is determined that the system is working normally. d is the fault detection threshold, T d The selection of reflects the fault detection capability. Under a given false alarm probability P(λ k >T d )=α, T d It can be obtained from formula (18):

[0056]

[0057] The theoretical residual variance is defined as Equation (15) based on the sliding window averaging method of the new interest sequence. The actual residual variance can be obtained by sliding window averaging the residual sequence.

[0058] The standard sliding window average of the residual sequence is defined as follows:

[0059]

[0060] Among them, c r Estimated residual covariance, which represents the result of averaging the variances of the first M residual vectors, r i represents the residual at time i, i0 = k - M + 1. M is determined empirically and based on specific circumstances, and is generally chosen between 8 and 10. The sliding window averaging method primarily smooths the residual, eliminating occasional anomalies. However, due to the limited window length, the residual estimate after the fault has resolved can be affected by the fault value, widening the actual fault determination region. This effect persists until the window leaves the fault region.

[0061] Define the theoretical residual covariance A k and the estimated residual covariance c r The relationship between them is as follows:

[0062]

[0063] Among them, A kIt is a 6-dimensional theoretical residual covariance matrix, whose diagonal matrix corresponds to the velocity and position covariance respectively. Since the covariance values ​​of the two are too different, this invention adds noise to the GPS three-axis velocity. If the position covariance is used to find the matrix trace, no obvious fault phenomenon will occur. Therefore, when performing the trace calculation, A needs to be added. k Perform rank reduction processing and perform rank calculation only on the matrix position corresponding to the velocity.

[0064] AOC k The size of reflects the stability of the observation value at the current moment. When the measurement noise is accurate, A k with c r Approximately equal, AOC k Approximately equal to 1; when the measurement noise is inaccurate and the observed value is abnormal, c r Mutation will occur, c r Deviation A k , AOC k It will be much greater than 1. Therefore, whether the measured value is abnormal can be determined by AOC k If the value is close to 1 (±0.5), it is considered that the measured value is normal. If it is much larger than 1, it is considered that the measured value is abnormal and the filter is faulty and needs to be processed.

[0065] It is easy to miss detection and make false detection by only using one fault identification method. Once the missed outliers enter the main filter, they will affect the fusion result, reduce the fusion filter accuracy, and pollute other sub-filters.

[0066] The two methods are combined for fault judgment. According to the characteristics of the two methods, the fault thresholds of different detection methods are first given, T d As the threshold of the residual chi-square test, set the upper limit of the sliding window average to C max =1.5, the lower limit is C min =0.5; then give the joint fault detection result judgment, when the joint detection result output is 1, it is considered that the sub-filter has a fault, and when the joint detection result output is 0, it is considered that the sub-filter has no fault; the specific joint fault judgment rule is: when λ k >T d When the residual chi-square test result is 1, AOC k Greater than C max or AOC k Less than C min When λ is , the sliding window average method determines the result to be 1, and the joint detection result is 1; when λ k <T d When the residual chi-square test result is 0, AOC k Greater than C max or AOC k Less than Cmin When λ is , the sliding window average method determines the result to be 1, and the joint detection result is 0; k >T d When the residual chi-square test result is 1, AOC k Less than C max and greater than C min When λ is 0, the sliding window average method determines the result to be 0, and the joint detection result is 0; when λ k <T d When the residual chi-square test result is 0, AOC k Less than C max and greater than C min When the sliding window average method determines that the result is 0, the joint detection result is 0.

[0067] Therefore, the two methods are combined to judge the fault, reduce the probability of missed detection, thereby reducing the impact of outliers and improving the accuracy of fusion filtering.

[0068] After fault detection, the faulty filter needs to be processed. Conventional methods isolate the faulty filter and exclude its output state estimate from the main filter for fusion, or use only the first-order state prediction of the faulty sub-filter. This completely discards the measurement information of the sub-filter and prevents it from being properly utilized. We propose an algorithm that maximizes the use of useful information in the measurement values.

[0069] The federated Kalman filter performs the classic Kalman filter time update and measurement update process as follows:

[0070] One-step prediction of the state:

[0071]

[0072] State Estimation:

[0073]

[0074] Filter gain matrix:

[0075]

[0076] One-step forecast error variance matrix:

[0077]

[0078] Estimated error variance matrix:

[0079] P i,k =(IK i,k H i,k )P i,k|k-1 (25)

[0080] It can be seen from formula (22) that the state estimation result is related to the measured value and predicted value at time k, and the filter gain matrix K is adaptively adjusted. k The degree of confidence in the measured values ​​and state predictions can be adjusted. k The availability is large, K k The larger the value, the larger the Z k The availability of K k The smaller the gain, the less likely it is to have an impact on the filtering result. Therefore, when the measured value is abnormal, the filter gain can be adaptively adjusted to fully utilize the measured value, making the fusion result more stable and reliable.

[0081] Improved filter gain matrix:

[0082]

[0083] Among them, a k is the adaptive coefficient, a k ∈(0,1). k Set the following rules:

[0084]

[0085] When the joint fault detection result is 1, it is considered that the sub-filter has a fault, and the K k Make adaptive adjustments to Due to λ k When there is a mutation, the value will increase significantly, taking λ k and residual chi-square detection threshold T d The ratio of a to b adjusts the filter gain. When the measured value is abnormal, the trust in the abnormal measured value is greatly reduced, making the filtering result more accurate and more stable. When the joint fault detection result is 0, it is considered that the sub-filter has no fault at this time, so let a k =1.

[0086] Adaptively adjusting the filter gain prevents divergence in the fusion filtering results when filtering anomalies occur, significantly reducing positioning errors at the anomaly point. After the anomaly is resolved, the system quickly converges to a fault-free state. However, the positioning error at the anomaly point is higher than when only the k-time prediction value is considered. The convergence speed of only considering the k-time prediction value is not as fast as adaptively adjusting the filter gain. Combining the advantages and disadvantages of both, after the main filter is fused, it is further fused with the time-updated value of the fault sub-filter, which both reduces positioning error and accelerates convergence.

[0087] P e =a k P 1,k|k-1 +(1-a k )P g(27)

[0088]

[0089] P 1,k|k-1 is the one-step prediction error variance matrix of the fault sub-filter, reflecting the quality of the update time. The smaller it is, the more accurate the state estimation result is. is the first-order state prediction value of the fault sub-filter. Here we study the situation when GPS fails, so the fault sub-filter is sub-filter 1; P e is the global error estimate covariance at the time of failure, X e is the global state estimation value at the time of the fault, which is the weighted fusion result of the fault sub-filter time update value and the global state estimation value after fault processing. e , X e As the new global error estimate covariance and state estimate continue to enter the FKF for feedback reset.

[0090] The present invention adopts the above technical solution and has the following beneficial effects:

[0091] (1) When GPS positioning is used alone, the positioning accuracy is not high. The present invention proposes a GPS / AOA / SINS combined navigation and positioning method based on a federated Kalman filter, which makes up for the shortcomings of GPS positioning alone and reduces the root mean square error of positioning to about 1.6 meters, greatly improving the positioning accuracy.

[0092] (2) The present invention adds a fault detection and processor between the sub-filter and the main filter. In view of the fact that conventional single detection means are prone to false detection and missed detection, the present invention uses the residual chi-square detection and the sliding window averaging method based on the new information sequence to make judgments together, thereby reducing the possibility of missed detection, reducing the impact of outliers, and improving the fault detection rate.

[0093] (3) The present invention makes full use of the useful information in the abnormal measurement information obtained by fault detection, adaptively adjusts the gain matrix of the fault sub-filter through the ratio of the threshold and the detection function in the residual chi-square detection, and reasonably allocates the degree of trust in the measurement value and the state value.

[0094] (4) In view of the characteristics that the positioning divergence is small and the convergence speed is slow when only the first-order state prediction value of the fault sub-filter is used, and the positioning divergence is large and the convergence speed is fast when only adaptive processing is used, the global state estimation value after adaptive processing is further combined with the time update value of the fault sub-filter, while meeting the requirements of reducing the error at the abnormal part and accelerating the convergence speed, meeting a certain degree of real-time processing, reducing the positioning divergence caused by the abnormal measurement value, improving positioning stability, and ensuring the accuracy of the global state estimation. BRIEF DESCRIPTION OF THE DRAWINGS

[0095] Figure 1 It is a structural diagram of the combined navigation system of the present invention.

[0096] Figure 2 This is a comparison chart of the RMSE of positioning of the GPS alone and the integrated navigation system of the present invention.

[0097] Figure 3 This is a position error curve diagram of the integrated navigation system when there is no fault in the present invention.

[0098] Figure 4 This is a speed error curve diagram of the integrated navigation system when there is no fault in the present invention.

[0099] Figure 5 This is a position error curve diagram of the integrated navigation system when there is a fault in the present invention.

[0100] Figure 6 This is a speed error curve diagram of the integrated navigation system when there is a fault in the present invention.

[0101] Figure 7 This is a position error curve diagram of the improved federated filtering integrated navigation system of the present invention.

[0102] Figure 8 This is a speed error curve diagram of the improved federated filtering integrated navigation system of the present invention.

[0103] Figure 9 This is a mean square error curve diagram of the faulty, fault-free and improved federated Kalman filter integrated navigation systems of the present invention.

[0104] Figure 10 This is a comparison chart of four different fault handling methods for improving the federated Kalman filter in the present invention. DETAILED DESCRIPTION

[0105] The technical solution of the invention is described in detail below with reference to the accompanying drawings.

[0106] This method uses simulation experiments to verify the superiority of the SINS / GPS / AOA combined navigation system, and further verifies the improvement of the reliability of the combined navigation positioning by the improved federated Kalman filter. The target object is simulated to move in a uniform straight line, with a movement time of 300s. The simulated trajectory of the moving object is used as the true value during its movement. The initial position is 32.081° north latitude, 118.771° east longitude, and 40m elevation. The data update frequency of SINS is 100Hz, the data update frequency of GPS is 1Hz, and the fusion filter period of the main filter is 1s. The residual chi-square detection false alarm rate is set to 0.05, and the corresponding detection threshold is 12.592. The SINS error sources are set as follows: the gyroscope constant drift is 0.03° / h, the angle random walk is 0.001° / The accelerometer has a random constant bias of 1×10-4 g, the random walk speed is 5μg / The initial alignment error of the strapdown inertial navigation system is 0.5′ in water roll angle, 0.5′ in pitch angle, 20′ in heading angle, 1m in initial platform alignment error, 1m in initial platform alignment error, 3m in initial platform alignment error, 0.1m / s ...

[0107] To validate the effectiveness of this method, a GPS positioning subsystem failure was simulated. An artificial interference of 0.8 times the normal observation noise was introduced into the GPS velocity observations in the northeast celestial direction between 130 and 149 seconds. A Monte Carlo simulation was performed 2500 times, and different fault handling algorithms were compared. The experiments confirmed that integrated GPS / AOA / SINS navigation can improve the positioning performance of a standalone GPS system. The improved federated Kalman filter effectively reduces the impact of the faulty subfilter. By rationally utilizing the useful information in the anomalous observations, the filtering accuracy of the fused positioning system is improved, ensuring the proper operation of the integrated navigation system.

[0108] Figure 1 The combined navigation system of the present invention, Figure 2 This is a comparison of the RMSE values ​​for GPS positioning alone and the GPS / AOA / SINS integrated navigation system. As can be seen from the figure, GPS positioning alone performs poorly, with an overall mean RMSE of approximately 16 meters. When the GPS signal is severely obstructed, the maximum mean square error can reach 24 meters, failing to meet real-time positioning requirements. Initially, the RMSE value for the GPS / AOA / SINS integrated navigation system is high at approximately 3 meters due to the initial SINS alignment error. Subsequently, the RMSE gradually converges, stabilizing at a mean RMSE of approximately 1.5 meters. The positioning curve is relatively smooth, and the overall RMSE value is significantly lower than that for GPS positioning alone, meeting daily positioning requirements. Experiments demonstrate that the GPS / AOA / SINS integrated navigation system can improve positioning performance compared to GPS positioning alone, significantly improving positioning accuracy and demonstrating its superiority.

[0109] Figure 3 、 Figure 4The following are the position and velocity error curves of the integrated navigation system under normal conditions. The initial positioning error is 1 meter in latitude and longitude, and 3 meters in elevation. This is due to an initial alignment error in the strapdown inertial navigation system. Over time, the position error gradually converges, stabilizing to approximately 0.5 meters in latitude and longitude, and 0.25 meters in elevation. Similarly, due to the initial alignment of the SINS, the velocity error is initially larger in the northeast celestial direction, at 0.1 meters. There are slight fluctuations within the first 50 seconds, after which the curve gradually stabilizes. After stabilization, the northeast celestial errors are approximately 0.01, 0.02, and 0.01 meters, respectively. These positioning errors are all below the meter level. The designed GPS / AOA / SINS integrated navigation and positioning system can improve the performance of GPS positioning alone and meet basic positioning requirements.

[0110] Figure 5 、 Figure 6 The following are graphs of the mean position and velocity errors when the integrated navigation system malfunctions. The mean error reflects the overall stability of the errors. The graph shows that, initially, the velocity and latitude errors are large due to the initial alignment of the inertial navigation system. The mean position and velocity errors converge after 50 seconds. Within the 130-149 second interval, significant fluctuations in the mean position and velocity errors indicate unstable fusion filtering and abnormal filtering results. This is because the variance of the actual measurement noise increases to 0.8 times the initial measurement noise within this interval. Because the mean errors are balanced, the mean error amplitude within the fault interval is small. After the fault is resolved after 149 seconds, both velocity and position errors gradually converge to normal levels, demonstrating that conventional federated Kalman filtering has a certain degree of adaptability to faults.

[0111] Figure 7 、 Figure 8 These are the mean position and velocity error curves of the integrated navigation system after the improved federated Kalman filter. Figure 5 Figure 6 It can be seen that within the fault occurrence range of 130-149 seconds, the mean position and velocity errors of the integrated navigation system based on the improved federated Kalman filter did not fluctuate significantly, indicating that fault detection can effectively detect abnormalities in the output state values ​​of the sub-filters, and the fault handling algorithm can effectively reduce the impact of abnormal values ​​on the fusion filtering results and reduce contamination to other sub-filters. Figure 3 、 Figure 4It can be seen that the position and velocity error curves of the integrated navigation system when there is no fault are basically similar to those of the improved federated Kalman filter integrated navigation system. Except for a slight increase in the mean error at the fault location, which creates a sharp point after the fault ends, the overall mean error curve is stable and converges quickly. Simulations show that the improved federated Kalman filter can enhance the stability and reliability of the GPS / AOA / SINS integrated navigation system.

[0112] Figure 9 The following are the RMSE error curves of the combined navigation system with and without faults, and with the improved federated Kalman filter. As can be seen from the figure, the RMSE of the GPS / SINS / AOA combined navigation system converges at 1.6m when there is no fault. The fault causes the positioning results of the combined navigation system to diverge at 130s, and the mean square error deviates far from the normal value, diverging to about 5m at 150s. After the fault ends, the fusion filter results converge slowly due to contamination by the fault information. The improved federated Kalman filter fuses the adaptively processed fusion positioning results with the time-updated value of the fault sub-filter again, greatly reducing the filter divergence caused by outliers. The maximum RMSE is about 1.8m, and after the fault ends, the mean square error quickly converges to the normal positioning accuracy. This shows that the improved federated Kalman filter proposed in this paper can greatly improve the positioning accuracy of the combined navigation system, enhance the adaptability of the combined navigation system to sudden faults, and enable the system to still operate normally in a reduced performance manner in the presence of a fault.

[0113] Figure 10 This is a comparison of the RMSE of four different fault handling methods using the improved federated Kalman filter. These methods are: excluding the fault sub-filter from the main filter fusion, using the first-order state prediction of the fault sub-filter for fusion, adaptive fault handling, and combining adaptive handling with time updates of the fault sub-filter. The first two methods are commonly used in other papers for fault detection and isolation, while the latter two are the algorithms proposed in this paper. Due to the poor performance of the third algorithm in fault suppression, the fourth method further improves on it.

[0114] Neither the methods that do not use the fault sub-filter output nor the methods that use first-order state predictions utilize information from abnormal measurements. The method that does not use the fault sub-filter output requires fusion with the state output estimates of other sub-filters and reconstruction of the fault sub-filter. However, since the INS / AOA sub-filter can only correct position information, it also exhibits divergence when a fault occurs. The method that uses first-order state predictions is significantly affected by the state estimate at the previous moment and less affected by the fault, resulting in better fault amelioration. The RMSE of the adaptive fault processing method is between the previous two methods, but its convergence speed is much faster than that of the method using first-order state predictions. In summary, to achieve both high accuracy and fast convergence, the results of the adaptive processing are further fused with the time-updated results of the fault sub-filter. The figure clearly shows that the method combining adaptive processing with the time-updated fault sub-filter achieves the lowest mean squared error, with the lowest mean squared error at 150 seconds, and significantly improves convergence speed. The experimental results show that the method proposed in this paper can handle the integrated navigation system faults in real time, effectively compensate for the abnormal errors in the integrated navigation process, reduce the loss of system filtering accuracy during faults, and improve the fusion positioning accuracy of the integrated navigation and positioning system.

[0115] Improve the performance of four different fault handling methods of the federated Kalman filter through quantitative analysis:

[0116] Table 1 Statistics of four fault handling methods Unit / m

[0117]

[0118] Table 1 shows that the adaptive processing method with fault sub-filter time update achieves the lowest mean and maximum RMSE values. The maximum RMSE of the integrated navigation system during a fault is 4.952. After fault processing, the maximum RMSE decreases by 58.16%, 61.53%, 60.70%, and 61.89%, respectively. The mean RMSE for the fault-free state is 1.6185, and the mean RMSE for the fault-involved state is 2.4857. After processing, the mean RMSE decreases by 31.47%, 32.67%, 32.70%, and 32.86%, respectively. Quantitative analysis further demonstrates that the improved federated Kalman filter algorithm proposed in this invention has advantages in enhancing GPS / AOA / SINS positioning reliability and can improve the filtering accuracy of the integrated navigation system during faults.

Claims

1. A GPS / AOA / SINS integrated navigation method based on federated Kalman filtering, characterized in that: The first sub-filter is used to observe the difference between the speed and position output by GPS navigation and the speed and position output by SINS navigation, and the state estimation at the next moment is performed based on the current global state estimation value and global prediction covariance matrix assigned by the main filter; The second sub-filter is used to observe the difference between the AOA positioning position and the position output by the SINS navigation, and the state estimation at the next moment is performed based on the current global state estimation value and the global prediction covariance matrix assigned by the main filter; The main filter is used to fuse the state estimation value and prediction covariance matrix output by the first sub-filter after chi-square fault detection and processing, and the state estimation value and prediction covariance matrix output by the second sub-filter, and the first-order state prediction value of the fault sub-filter is fused with the global state estimation value and global prediction covariance matrix at the current moment and then assigned to the two sub-filters. e =a k P 1,k|k-1 +(1-a k )P g , Among them, X e 、P e is the estimated value of the global state at the current moment Global prediction covariance matrix P g The correction value after incorporating the first-order state prediction value of the fault sub-filter, P 1,k|k-1 is the one-step prediction error variance matrix of the fault sub-filter, is the first-order state prediction value of the fault sub-filter, a k is the adaptive coefficient of the Kalman filter gain matrix for the k-th step prediction, λ k is the chi-square fault detection value, T d is the chi-square fault detection threshold.

2. The GPS / AOA / SINS integrated navigation method based on federated Kalman filtering according to claim 1, characterized in that: The main filter fuses the state estimation value and the prediction covariance matrix output by the first sub-filter after chi-square fault detection and processing as follows: Among them, Q g is the process noise covariance matrix, P i , Q i are the prediction covariance matrix and process noise covariance matrix of the i-th sub-filter, respectively. is the state estimate of the i-th sub-filter, N=2.

3. The GPS / AOA / SINS integrated navigation method based on federated Kalman filtering according to claim 2, characterized in that: The expression for assigning the first-order state prediction value of the fault sub-filter, the global state estimation value at the current moment, and the global prediction covariance matrix to the two sub-filters is: Among them, β i is the information allocation coefficient of the i-th sub-filter.

4. The GPS / AOA / SINS integrated navigation method based on federated Kalman filtering according to any one of claims 1 to 3, characterized in that: The method for performing chi-square fault detection on the state estimation value and prediction covariance matrix of the sub-filter sub-output is to combine the chi-square detection judgment result and the result of sliding window averaging of the innovation sequence to determine whether there is a fault: When the chi-square test result indicates that there is a fault and the result of sliding window averaging of the innovation sequence indicates that there is a fault, the sub-filter is judged to be faulty; When the chi-square test result is no fault and the result of sliding window averaging of the innovation sequence is faulty, the sub-filter is judged to be no fault; When the chi-square test result indicates that there is a fault and the result of sliding window averaging of the innovation sequence indicates that there is no fault, the sub-filter is judged to be fault-free; When the chi-square detection result is that there is no fault and the result of sliding window averaging of the innovation sequence is that there is no fault, it is determined that the sub-filter has no fault.

5. The GPS / AOA / SINS integrated navigation method based on federated Kalman filtering according to claim 4, characterized in that: The Kalman filter gain matrix of the main filter predicting the global state estimate at the current moment is updated according to the chi-square fault detection result. Among them, K k is the Kalman gain matrix predicted at step k, P k|k-1 H is the covariance matrix of the current measurement moment predicted based on the optimal estimation result of the covariance matrix of the previous moment, k is the measurement matrix predicted at step k, R k is the covariance matrix of the measurement noise matrix for the k-th step prediction.

6. The GPS / AOA / SINS integrated navigation method based on federated Kalman filtering according to claim 4, characterized in that: The criterion for the chi-square test result to be a fault is λ k >T d The criterion for the chi-square test result to be fault-free is λ k <T d The result of sliding window averaging of the new information sequence is the fault criterion AOC k Greater than C max or AOC k Less than C min The result of sliding window averaging of the new information sequence is the fault-free criterion AOC k Less than C max and greater than C min , C max 、C min are the upper and lower limits of the sliding window average, AOC k Represents the 6-dimensional theoretical residual covariance matrix A k and the estimated residual covariance c r The relationship between Tr(C r ) represents the estimated residual covariance c r Perform trace operation, Tr(A k (1:3,1:3)) represents the 6-dimensional theoretical residual covariance matrix A k Perform trace operation on the velocity covariance diagonal matrix in .

Images