Self-adaptive federated Kalman filtering positioning algorithm

Abstract

The invention relates to the technical field of data processing, in particular to a self-adaptive federated Kalman filtering positioning algorithm, which comprises the following steps of: constructing a kinematics system model based on the motion characteristics of an indoor mobile robot; based on parameters generated by the system model, local state estimation of the distributed sensor is fused into a global optimal solution through recursive calculation, and a federated Kalman filtering model is constructed; an information sharing algorithm is utilized to dynamically optimize an interaction strategy between sub-filters, the weight of a sensor is adaptively adjusted, a covariance transmission path is constrained, and finally a multi-sensor fusion system with adaptability and expandability is constructed, so that an indoor mobile robot can work more accurately and robustly.

Description

Technical Field

[0001] The present invention relates to the technical field of data processing, and particularly to an adaptive federated Kalman filtering positioning algorithm. Background Art

[0002] In the traditional federated Kalman filtering algorithm, a fixed information allocation method is adopted. When the local sub-filter is interfered, the algorithm cannot adjust in time, resulting in reduced positioning accuracy and poor fault tolerance. And in extreme abnormal situations, such as local sub-filter failures or extreme error messages, it will cause the information fusion process to diverge, ultimately leading to positioning failure. Summary of the Invention

[0003] The purpose of the present invention is to provide an adaptive federated Kalman filtering positioning algorithm to solve the problems raised in the above background art.

[0004] To solve the above technical problems, the present invention provides the following technical solutions:

[0005] An adaptive federated Kalman filtering positioning algorithm, characterized in that: the method includes:

[0006] Step 1, constructing a kinematic system model based on the motion characteristics of an indoor mobile robot;

[0007] Step 2, based on the parameters generated by the system model, fusing the local state estimates of distributed sensors through recursive calculation into a global optimal solution, and constructing a federated Kalman filtering model;

[0008] Step 3, using an information sharing algorithm to adaptively adjust the sensor weights and constrain the covariance transfer path by dynamically optimizing the interaction strategy between sub-filters, and finally constructing a multi-sensor fusion system with adaptability and scalability.

[0009] Preferably, Step 1 includes: system modeling:

[0010] In a multi-sensor fusion system, the system model, the federated Kalman filter model, and the information sharing algorithm form a progressive collaborative architecture. As the mathematical foundation framework, the system model provides a strict parametric representation for the system dynamics and observation mechanism by defining the state transition matrix, the observation matrix, and the noise covariance matrix, laying the theoretical foundation for state estimation. The federated Kalman filter model performs the multi-source data fusion function within this framework. Based on the parameters generated by the system model, it fuses the local state estimates of distributed sensors into a global optimal solution through recursive calculation, significantly enhancing the robustness and fault tolerance of pose estimation. The information sharing algorithm dynamically optimizes the interaction strategy between sub-filters, adaptively adjusts the sensor weights, and constrains the covariance transfer path, thus achieving a balance between computational efficiency and estimation accuracy. The three form a closed-loop collaboration link: the system model provides parameter support for the federated filter, the federated filter generates the global state through the fusion mechanism, and the information sharing algorithm reversely optimizes the parameters of the fusion process, ultimately constructing an adaptive and scalable multi-sensor fusion system.

[0011] System model of indoor mobile robot:

[0012]

[0013] In the formula: Φ k+1|k is the system matrix, H k is the measurement matrix; the process noise is W k , and the observation noise is V k ;

[0014] Among them, a simple kinematic model is adopted. The parameters are:

[0015]

[0016] Preferably, step 2 includes: the federated Kalman filter model;

[0017] The traditional Kalman filter algorithm (FKF) is a two-stage decentralized filtering algorithm, consisting of several sub-filters and a global filter. During the filtering process, each sub-filter independently performs time update and measurement update, and the global filter fuses the results of each sub-filter together to obtain the global optimal estimate or a conservative sub-optimal estimate.

[0018] The FKF program includes five steps: initial state setting, time update, measurement update, information fusion, and information distribution.

[0019] S201. The initial error covariance matrix and the system noise covariance matrix of each local filter can be determined by the initial value of the global filter, and the starting value of the global filter can be set manually. The initial state is set as:

[0020]

[0021] Where: P i,0 is the state error covariance matrix of the i-th local sub-filter in the initial state; β i is the information sharing factor of the i-th local sub-filter in the initial state; P g,0 is the state error covariance matrix of the global main filter in the initial state;

[0022] S202. Time update: Independently perform a priori prediction on the state quantities and state error covariances of each local sub-filter and the main filter, that is, the one-step prediction value:

[0023]

[0024] Where: is the a priori state estimate of the i-th local sub-filter at the (k + 1)-th moment; is the posterior state estimate of the i-th local sub-filter at the k-th moment; Φ i,k+1|k is the system matrix of the i-th local sub-filter at the (k + 1)-th moment;

[0025]

[0026] Where: P i,k+1|k is the a priori state error covariance of the i-th local sub-filter at the (k + 1)-th moment; P i,k is the posterior state error covariance of the i-th local sub-filter at the k-th moment; Q i,k is the covariance matrix of the process noise.

[0027] S203. Observation update: It performs a posteriori prediction on the state quantities and state error covariances, and then corrects the a priori state estimate value and state error covariance according to the observation value, so as to obtain the posterior state estimate and state error covariance:

[0028]

[0029] Where: K i,k+1 is the Kalman gain coefficient of the i-th local sub-filter at the (k + 1)-th moment; H i,k+1 is the observation matrix of the i-th local sub-filter at the (k + 1)-th moment; R i,k+1 is the covariance of the observation noise.

[0030]

[0031] Where: is the posterior state estimate value of the i-th local sub-filter at the (k + 1)-th moment; Z i,k+1 is the observation value of the i-th sub-filter at the (k + 1)-th moment.

[0032] P i,k+1 =(I - K i,k+1 H i,k+1 )P i,k+1|k , i = 1, …, N, m.

[0033] S204. Information fusion: It is the core of the FKF algorithm, which fuses the local estimation information of each local sub-filter to obtain the global optimal estimation or a conservative sub-optimal estimation value:

[0034]

[0035] In the formula: P m,k+1 is the error covariance matrix of the main filter at the (k + 1)-th moment;

[0036]

[0037]

[0038] In the formula: Q m,k+1 is the system noise covariance matrix of the main filter at the (k + 1)-th moment.

[0039] S205. Information distribution: It feeds back the global optimal state estimation value the state error covariance matrix P g,k+1 and the system noise covariance matrix Q g,k+1 to each local sub-filter through the distribution factor β i :

[0040]

[0041] In the formula: β i represents the information sharing factor of the i-th local sub-filter at the (k + 1)-th moment, and β i > 0, satisfying the following equation:

[0042] Preferably, step 3 includes: an information sharing algorithm;

[0043] S301. An adaptive feed-forward sharing factor algorithm;

[0044] The global solution of the FKF algorithm is obtained by fusing the solutions of each local sub-filter. The information fusion of each local sub-filter can be regarded as the feed-forward process of the FKF algorithm. In this paper, an adaptive algorithm based on the prediction residual of the local sub-filter is adopted to obtain the information sharing factor for the information fusion of the feed-forward of each local sub-filter, as shown in the following formula:

[0045]

[0046] where: d is a constant that needs to be given manually; is the normalized prediction residual of the i-th local sub-filter observation at the k-th moment, and is:

[0047]

[0048] where: is the residual of the i-th local sub-filter at the k-th moment; H i,k is the observation matrix of the i-th local sub-filter at the k-th moment; the observation noise covariance matrix of the i-th local sub-filter at the k-th moment is R i,k ; P i,k is the state error covariance matrix of the i-th local sub-filter at the k-th moment; Z i,k is the observation value of the i-th local sub-filter at the k-th moment; is the prior state estimate of the i-th local sub-filter at the k-th moment;

[0049] where represents the error between the actual observation and the predicted observation. If increases, then will increase, and correspondingly, α i,k will decrease, indicating a decrease in the credibility of the local sub-filter data; conversely, if decreases, then will decrease, and correspondingly, α i,k will increase, indicating an increase in the credibility of the local sub-filter data. is the error covariance matrix between the theoretical observation and the predicted observation, and its trace represents the frequency of the observation error change. If the trace increases, it indicates an increase in the observation error change rate, and correspondingly will decrease, indicating an increase in the credibility of the local sub-filter data; conversely, if the trace decreases, it indicates that the observation error changes slowly, and correspondingly will increase, indicating a decrease in the credibility of the local sub-filter data.

[0050] According to the information sharing principle, the feedforward information sharing factors of each local sub-filter are normalized, and the feedforward information sharing factor is rewritten as:

[0051]

[0052] where: α' i,k is the normalized feedforward information sharing factor of the i-th local sub-filter at the k-th moment;

[0053] According to Figure 1 shown in the structural diagram of the adaptive feedforward information sharing algorithm in, the information fusion process based on the feedforward adaptive information sharing factor is:

[0054]

[0055] From the perspective of abnormal disturbances and fault tolerance, the adaptive feedforward information sharing algorithm can overcome the problem of information contamination in the fusion process by the global filter. When one of the local sub-sensors works under poor conditions, the state residual of this local sub-sensor will be large, which makes the proportion of the feedforward information sharing factor of this sensor in information fusion become smaller, thus achieving the optimal fusion effect.

[0056] S302. Adaptive negative feedback sharing factor algorithm;

[0057] The feedback information fusion structures of the traditional FKF algorithm are mainly non-reset structure, zero-reset structure, fusion reset structure, and readjustment structure. In this paper, the fusion reset structure is adopted, that is, the global filter needs to feedback the allocation information after each fusion calculation, and each local sub-filter needs to wait for the feedback information of the global filter before working. The fusion reset structure can further improve the fault tolerance rate and calculation accuracy of the whole system. However, the traditional fusion reset structure sets the feedback information sharing factor as a constant value, which cannot reflect the performance changes of local sensors in real time. And during the information propagation process, faults will also spread through the same path, resulting in a decrease in fault tolerance. Therefore, to reduce the influence of improper selection of the allocation coefficient on data fusion, this paper adopts an algorithm based on the error covariance matrix that changes in real time of local sub-filters to determine the feedback information sharing factor.

[0058] Figure 2 is the structural diagram of the adaptive negative feedback information sharing algorithm, which uses the trace of the error covariance matrix to calculate the information sharing factor in the feedback process. The information sharing factor can be expressed as:

[0059]

[0060] where: P i,k is the state error covariance matrix of the i-th local sub-filter at the k-th moment.

[0061] S303. Local filter fault detector;

[0062] The adaptive feedforward information sharing algorithm can already achieve a good fusion effect. However, in extreme abnormal situations, such as local sub-filter faults or extreme error messages, the information sharing factor of the local sub-filter is close to 0, which will lead to an infinite matrix in the information fusion process, the fusion process diverges, and finally the positioning fails. To solve the above problems, this paper adds a fault detector to detect the fault signal in real time during the information fusion process;

[0063] Figure 3It is the structural diagram of local sub-filter fault detection. The judgment criterion for local sub-filter failure can be expressed as:

[0064]

[0065] In the formula: is the residual of the i-th local sub-filter at the k-th moment; h is the outlier threshold set artificially; if the local sub-filter receives data, τ i,k is true, otherwise it is false.

[0066] When a fault signal occurs, the fault detector discards the data of the local sub-filter, enabling the global filter to obtain more stable and reliable fusion information.

[0067] Compared with the prior art, the beneficial effects achieved by the present invention are:

[0068] The adaptive federated Kalman filtering algorithm of the present invention not only makes up for the deficiency of the low fault tolerance of the traditional FKF algorithm, but also meets the requirements of the distributed structure and information conservation of the FKF algorithm. In the process of information fusion and information distribution, the AFKF algorithm provides an adaptively changing information sharing factor for the local sub-filter, improving the estimation accuracy and fault tolerance rate of the filter, and adding a fault detector to detect the fault signal in real time during the information fusion process, so that the indoor mobile robot can work more accurately and robustly. Description of the Drawings

[0069] The drawings are used to provide a further understanding of the present invention and constitute a part of the specification. Together with the embodiments of the present invention, they are used to explain the present invention and do not constitute a limitation to the present invention. In the drawings:

[0070] Figure 1 is the structural diagram of the adaptive feedforward information sharing algorithm of the present invention;

[0071] Figure 2 is the structural diagram of the adaptive negative feedback information sharing algorithm of the present invention;

[0072] Figure 3 is the structural diagram of local sub-filter fault detection of the present invention;

[0073] Figure 4 is the structural diagram of the adaptive federated filtering algorithm of the present invention;

[0074] Figure 5 is the real test environment diagram of the indoor mobile robot of the present invention;

[0075] Figure 6 is the comparison diagram of the sub-filter, traditional FKF and the proposed AFKF of the present invention;

[0076] Figure 7It is the comparison chart of the X - direction error between the sub - filter of the present invention and the proposed algorithm;

[0077] Figure 8 It is the comparison chart of the Y - direction error between the sub - filter of the present invention and the proposed algorithm;

[0078] Figure 9 It is the comparison chart of the X - direction error between the traditional FKF of the present invention and the proposed algorithm;

[0079] Figure 10 It is the comparison chart of the Y - direction error between the traditional FKF of the present invention and the proposed algorithm. Specific implementation manner

[0080] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0081] Please refer to Figures 1 - 4 , the present invention provides a technical solution:

[0082] Embodiment 1:

[0083] The structure of the adaptive federated Kalman filtering algorithm proposed in this paper is as Figure 4 shown. It not only makes up for the deficiency of the low fault - tolerance ability of the traditional FKF algorithm, but also meets the requirements of the distributed structure and information conservation of the FKF algorithm. In the process of information fusion and information distribution, the AFKF algorithm provides an adaptively changing information sharing factor for local sub - filters, improving the estimation accuracy and fault - tolerance rate of the filter. And, in this paper, a fault detector is added to detect the fault signal in real - time during the information fusion process, so that the indoor mobile robot can work more accurately and robustly.

[0084] Experimental analysis:

[0085] This paper conducts experimental comparison and analysis based on a real indoor environment. The indoor mobile robot platform used in the experiment is a self - developed differential - wheel mobile robot. Taking the indoor mobile robot as the test object, a test experimental platform is built in the indoor environment as Figure 5 shown, where the indoor environment is on the 18th floor of Building B in the Optoelectronic Science and Technology Park, with a size of approximately 25m × 37m.

[0086] In Figure 5In the actual indoor environment, based on the pre-built map, the positioning pseudo-ground truth provided by the Cartographer algorithm is used as the reference trajectory to judge the robustness and high precision of the proposed AFKF algorithm, and a comparative analysis is carried out with other AFKF algorithms. Let the initial observation noise Initial measurement noise The experimental comparison results are as Figure 6 — Figure 10 shown;

[0087] Figure 6 Figures are the comparison diagrams of the wheel encoder / IMU local sub-filter, the laser odometry local sub-filter, the traditional FKF algorithm, and the proposed AFKF algorithm. Since the algorithm verification is carried out in a simulation environment without extra external noise interference, the positioning information comparison diagrams almost overlap. In Figure 7 , area A is the time period when the working performance of the wheel encoder / IMU local sub-filter is poor. The positioning error of the wheel encoder / IMU local sub-filter quickly reaches 17 cm at two time points, which will cause the mobile robot positioning to go wrong; in Figure 8 , area B is the time period when the performance of the laser odometry local sub-filter is poor. Within 500 time points, the positioning error of the laser odometry local sub-filter reaches 5 cm. From Figure 7 and Figure 8 , it can be seen that the AFKF algorithm proposed by the present invention can achieve global optimal fusion, and the performance of the global filter is not affected by the abnormal signals of the wheel encoder / IMU or the laser odometry local sub-filter. Therefore, during the whole experiment process, even under the unstable conditions of the wheel encoder / IMU or the laser odometry local sub-filter, the global filter can work normally.

[0088] In addition, Figure 9 and Figure 10 are the error comparison diagrams of the traditional FKF algorithm and the AFKF algorithm proposed by the present invention. In the time period C-J in the figure, the proposed AFKF algorithm has better stability than the traditional FKF algorithm. The results show that the proposed AFKF algorithm can adaptively change the information sharing factor of each local sub-filter and effectively improve the accuracy and tolerance of the whole system in a dynamic environment.

[0089] Embodiment 2:

[0090] The computer-readable storage medium of this embodiment stores a computer program, and when the program is executed by a processor, it implements the steps in an adaptive federated Kalman filter positioning algorithm of Embodiment 1.

[0091] The computer-readable storage medium of this embodiment can be the internal storage unit of the terminal, such as the hard disk or memory of the terminal; the computer-readable storage medium of this embodiment can also be the external storage device of the terminal, such as the plug-in hard disk, smart memory card, secure digital card, flash card, etc. equipped on the terminal; further, the computer-readable storage medium can also include both the internal storage unit and the external storage device of the terminal.

[0092] The computer-readable storage medium of this embodiment is used to store computer programs and other programs and data required by the terminal, and the computer-readable storage medium can also be used to temporarily store the data that has been output or will be output.

[0093] Embodiment 3:

[0094] The computer device of this embodiment includes a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the program, it implements the steps in an adaptive federated Kalman filter positioning algorithm of Embodiment 1.

[0095] In this embodiment, the processor can be a central processing unit, or other general-purpose processors, digital signal processors, application-specific integrated circuits, off-the-shelf programmable gate arrays, or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor can be a microprocessor or the processor can also be any conventional processor, etc.; the memory can include a read-only memory and a random access memory, and provide instructions and data to the processor. A part of the memory can also include a non-volatile random access memory. For example, the memory can also store information about the device type.

[0096] Those skilled in the art should understand that the content disclosed in the embodiment can be provided as a method, a system, or a computer program product. Therefore, this solution can be in the form of a hardware embodiment, a software embodiment, or an embodiment combining software and hardware aspects. Moreover, this solution can be in the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage and optical storage, etc.) containing computer-usable program code.

[0097] This solution is described with reference to the flowcharts and / or block diagrams of the method and computer program product according to the embodiments of this solution. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combination of the flows and / or blocks in the flowchart and / or block diagram can be implemented by computer program instructions; these computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate for implementation in the processFigure 1 a process or multiple processes and / or party schematic Figure 1 a device for the functions specified in a box or multiple boxes.

[0098] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory produce a manufactured article including an instruction device, and the instruction device implements the process Figure 1 a process or multiple processes and / or party schematic Figure 1 the functions specified in a box or multiple boxes.

[0099] These computer program instructions can also be loaded onto a computer or other programmable data processing device, so that a series of operation steps are executed on the computer or other programmable device to generate a computer-implemented process. Thus, the instructions executed on the computer or other programmable device provide steps for implementing the functions specified in a process Figure 1 a process or multiple processes and / or party schematic Figure 1 the functions specified in a box or multiple boxes.

[0100] Those of ordinary skill in the art can understand that all or part of the processes of implementing the methods in the above embodiments can be completed by instructing relevant hardware through a computer program. The program can be stored in a computer-readable storage medium. When the program is executed, it can include the processes of the embodiments of the above methods. Among them, the storage medium can be a magnetic disk, an optical disk, a read-only memory (ROM), or a random access memory (RAM), etc.

[0101] Finally, it should be noted that the above are only the preferred embodiments of the present invention and are not used to limit the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements for some of the technical features. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

Claims

1. An adaptive federated Kalman filter positioning algorithm, characterized by: The method includes: Step 1: Construct a kinematic system model based on the motion characteristics of an indoor mobile robot; Step 2: Based on the parameters generated by the system model, fuse the local state estimates of distributed sensors into a global optimal solution through recursive calculation, and construct a federated Kalman filter model; Step 3: Use an information sharing algorithm to adaptively adjust the sensor weights and constrain the covariance transfer path by dynamically optimizing the interaction strategy between sub-filters, and finally construct a multi-sensor fusion system with adaptability and scalability.

2. The adaptive federated Kalman filter positioning algorithm according to claim 1, wherein The said Step 1 includes: Obtain the process noise W of the mobile robot during movement k and the observation noise V k , and obtain the system model of the mobile robot according to the formula: Among them, Φ k+1|k represents the system matrix, and H k represents the measurement matrix; in, 3. The adaptive federated Kalman filter positioning algorithm according to claim 1, wherein: The said Step 2 includes: initial state setting, time update, measurement update, information fusion, and information distribution; S201: Initial state setting: The initial error covariance matrix and system noise covariance matrix of each local filter can be determined by the initial values of the global filter, and the starting value of the global filter can be set manually; Based on this, the initial state is set as follows: i = 1, …, N, m; Among them, P i,0 is the state error covariance matrix of the i-th local sub-filter in the initial state; β i is the information sharing factor of the i-th local sub-filter in the initial state; P g,0 is the state error covariance matrix of the global main filter at the initial state. S202. Time update: Perform a prior prediction on the state variables and state error covariances of each local sub-filter and the main filter separately, i.e., the one-step prediction value: i = 1, …, N, m; Among them, is the prior state estimate of the i-th local sub-filter at the (k + 1)-th moment; is the posterior state estimate of the i-th local sub-filter at the k-th moment; Φ i,k+1|k is the system matrix of the i-th local sub-filter at the (k + 1)-th moment; i=1,…,N,m; Among them, P i,k+1|k is the prior state error covariance of the i-th local sub-filter at the k+1th time; P i,k is the posterior state error covariance of the i-th local sub-filter at the k-th moment; Q i,k is the covariance matrix of the process noise; S203: Observation update: Perform posterior prediction on the state quantity and state error covariance, and then correct the prior state estimate value and state error covariance according to the observation value to obtain the posterior state estimate and state error covariance: i = 1, …, N, m; Among them, K i,k+1 is the Kalman gain coefficient of the i-th local sub-filter at the k+1th time; H i,k+1 is the observation matrix of the i-th local sub-filter at the k+1th time; R i,k+1 is the covariance of the observation noise; i = 1, …, N, m; wherein, is the posterior state estimation value of the i-th local sub-filter at the (k + 1)-th moment; Z i,k+1 is the observation value of the i-th sub-filter at the (k + 1)-th moment; P i,k+1 = (I - K i,k+1 H i,k+1 )P i,k+1|k , i = 1, …, N, m; where, P i,k+1 is the posterior state error covariance of the i-th local sub-filter at the (k + 1)-th moment; S204. Information fusion: Fuse the local estimation information of each local sub-filter to obtain the globally optimal estimation or a conservative sub-optimal estimation value: Among them, P m,k+1 is the error covariance matrix of the main filter at the k+1th moment; Among them, Q m,k+1 is the system noise covariance matrix of the main filter at the k+1th moment; S205. Information distribution: Feed the globally optimal state estimate value state error covariance matrix P g,k+1 and system noise covariance matrix Q g,k+1 back to each local sub-filter through the distribution factor β i : i = 1, …, N, m; i=1,…,N,m; i = 1, …, N, m; where β i represents the information sharing factor of the i-th local sub-filter at the (k + 1)-th moment, and β i > 0, satisfying 4. The adaptive federated Kalman filter positioning algorithm according to claim 1, wherein: The said Step 3 includes: S301: Adopt an adaptive algorithm based on the prediction residuals of local sub-filters, and obtain the information sharing factor for the feedforward information fusion of each local sub-filter according to the formula: Where d is a constant, is the normalized prediction residual of the i-th local sub-filter observation at the k-th time, and wherein, is the residual of the i-th local sub-filter at the k-th moment; H i,k is the observation matrix of the i-th local sub-filter at the k-th moment; the observation noise covariance matrix of the i-th local sub-filter at the k-th moment is R i,k ; P i,k is the state error covariance matrix of the i-th local sub-filter at the k-th moment; Z i,k is the observation value of the i-th local sub-filter at the k-th moment; is the prior state estimate of the i-th local sub-filter at the k-th moment; According to the information sharing principle, normalize the feedforward information sharing factors of each local sub-filter, and the feedforward information sharing factor is rewritten as: where α′ i,k is the feedforward information sharing factor of the i-th local sub-filter after normalization at the k-th moment; S302: Use the trace of the error covariance matrix to calculate the information sharing factor in the feedback process, and the information sharing factor can be expressed as: where, P i,k is the state error covariance matrix of the i-th local sub-filter at the k-th moment; S303: Add a fault detector to detect the fault signal in real time during the information fusion process: Obtain the local sub-filter in the fault detection module. According to the formula, the judgment criterion for the failure of the local sub-filter is: Among them, is the residual of the i-th local sub-filter at the k-th moment; h is an artificially set outlier threshold; if the local sub-filter receives data, τ i,k is true, otherwise it is false; When a fault signal occurs, the fault detector discards the data of the local sub-filter, enabling the global filter to obtain more stable and reliable fusion information. Based on this, a multi-sensor fusion system with adaptability and scalability is constructed.

5. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by the processor, it implements the steps in an adaptive federated Kalman filter positioning algorithm as described in any one of claims 1-4.

6. A computer device, comprising a memory, a processor, and a computer program stored on the memory and running on the processor, characterized in that, When the processor executes the program, it implements the steps in an adaptive federated Kalman filter positioning algorithm as described in any one of claims 1-4.

Images