Cooperative state estimation method and system based on Kalman filtering and deep learning

Abstract

The invention discloses a cooperative state estimation method and system based on Kalman filtering and deep learning, and relates to the technical field of cluster control, and the method comprises the steps: obtaining a state value of an intelligent agent at a (k-1) th sampling moment and a control value of the intelligent agent from the (k-1) th sampling moment to a kth sampling moment; according to the obtained state value and control value, a predictive state value and predictive uncertainty of the intelligent agent at the kth sampling moment are obtained by using a deterministic kinematics model and a process noise neural network; obtaining observation data of the intelligent agent on a neighbor intelligent agent, calculating a structured residual error based on the prediction state value, the observation data and the prediction state value of the neighbor intelligent agent, and inputting the prediction state value, the prediction uncertainty, the observation data and the structured residual error of the intelligent agent at the kth sampling moment into a posterior neural network to obtain a predicted state value of the intelligent agent at the kth sampling moment; and obtaining a posterior state value and posterior uncertainty of the intelligent agent at the kth sampling moment. According to the method, the state estimation precision and the robustness to environmental interference can be remarkably improved.

Description

Technical Field

[0001] This invention relates to the field of cluster control technology, and in particular to a cooperative state estimation method and system based on Kalman filtering and deep learning. Background Technology

[0002] State estimation is a fundamental core task in multi-agent swarm systems (such as robots and drones) for achieving perception, localization, navigation, and control. Its goal is to infer the dynamic state of the system—which cannot be directly measured—from noisy observation data. In multi-agent swarm systems, agents collaborate by sharing local information to perform distributed, high-precision, real-time estimation of the group's state. This is a prerequisite for completing complex tasks such as formation, search, and cooperative transportation, and has significant theoretical and practical value.

[0003] Traditional state estimation methods are primarily model-driven, with Kalman filtering (KF) being a prime example. Under the assumption of a linear Gaussian system, KF provides the optimal minimum mean square error estimate. To handle nonlinear systems, researchers have developed a series of variants such as Extended Kalman Filter (EKF) and Unscented Kalman Filter (UKF). These methods predict and update states based on explicit physical or kinematic models, offering significant advantages such as high computational efficiency, strong interpretability, and the ability to quantify uncertainty. However, the performance of these methods heavily depends on the accuracy of the model. In complex real-world applications, systems often exhibit strong nonlinearity and non-Gaussian noise characteristics, and suffer from sensor aliasing due to sensor limitations or environmental interference. Model-driven methods often experience a significant decrease in estimation accuracy and reduced robustness when facing these challenges due to model mismatch.

[0004] In recent years, with breakthroughs in deep learning technology, data-driven methods have provided new approaches to state estimation. Models represented by recurrent neural networks (RNNs), long short-term memory networks (LSTMs), and attention mechanisms can directly learn complex spatiotemporal dynamic relationships and observation models from large-scale historical data, demonstrating powerful nonlinear fitting capabilities. In particular, frameworks based on variational inference and deep generative models can learn the posterior distribution of states. These methods reduce the dependence on precise explicit models and show potential in scenarios where models are unknown or overly complex. However, data-driven methods typically have the following inherent limitations: First, deep learning models are black-box models, lacking interpretability in their decision-making processes and making it difficult to incorporate known prior physical knowledge; second, performance is severely limited by the scale and quality of training data, and generalization ability often drops sharply when data is scarce or out-of-distribution situations exist; furthermore, they may produce state outputs that violate physical laws, posing security risks in practical deployments. Summary of the Invention

[0005] To address some or all of the technical problems existing in the prior art, this invention provides a collaborative state estimation method and system based on Kalman filtering and deep learning.

[0006] The technical solution of the present invention is as follows: Firstly, a collaborative state estimation method based on Kalman filtering and deep learning is provided. This method is applied to a multi-agent cluster and is executed distributed across each agent. The method includes: Obtain the agent's state value at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time; Based on the acquired state value and control value, the ideal state value of the agent at the k-th sampling time is estimated using a pre-set deterministic kinematic model, and the mean and standard deviation of the process noise are estimated using a pre-trained process noise neural network. Based on the ideal state value and the mean and standard deviation of the process noise, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are calculated. The system acquires the observation data of the agent on its neighboring agents. Based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of the agent itself sent by the neighboring agents at the k-th sampling time, it calculates the structured residual that incorporates the prior of geometric relations. The agent's predicted state value, prediction uncertainty, observation data, and structured residual at the k-th sampling time are input into a pre-trained posterior neural network to obtain the agent's posterior state value and posterior uncertainty at the k-th sampling time output by the posterior neural network. Among them, the neighboring intelligent agents are other intelligent agents within the perception range of the intelligent agent. Only when the intelligent agent detects the existence of a neighboring intelligent agent within its perception range will it conduct observations and communicate data with the neighboring intelligent agents.

[0007] Furthermore, in some implementations, the state values ​​of the agent include: the x-axis coordinate of the agent in a two-dimensional world coordinate system, the y-axis coordinate of the agent in a two-dimensional world coordinate system, and the orientation angle of the agent; The control values ​​of the agent include: the agent's linear velocity and angular velocity.

[0008] Furthermore, in some embodiments, the deterministic kinematic model is represented as: ; in, This represents the ideal x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the (k-1)-th sampling time. This represents the average linear velocity of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. This represents the average angular velocity of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. This represents the orientation angle of the i-th agent at the (k-1)-th sampling time. This represents the time interval between two adjacent sampling times. Indicates the angular velocity threshold. This represents the ideal y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the y-coordinate of the i-th agent in the two-dimensional world coordinate system at the (k-1)-th sampling time. This represents the ideal orientation angle of the i-th agent at the k-th sampling time. This represents the calculation of the cosine function. This indicates the calculation of the sine function.

[0009] Furthermore, in some implementations, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are determined by the following formula: ; ; in, This represents the predicted state value of the i-th agent at the k-th sampling time. Represents a deterministic kinematic model. This represents the state value of the i-th agent at the (k-1)-th sampling time. This represents the control value of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. Represents the mean vector of process noise. This represents the prediction uncertainty of the i-th agent at the k-th sampling time. This represents the standard deviation vector of process noise.

[0010] Furthermore, in some implementations, the observation data of an agent over its neighboring agents includes the relative distance and relative azimuth angle between the agent and its neighboring agents.

[0011] Furthermore, in some implementations, the structured residuals that incorporate prior geometric relationships are calculated in the following manner: Based on the predicted state value of the agent at the k-th sampling time and the predicted state value of itself sent by the neighboring agent at the k-th sampling time, calculate the expected distance and expected azimuth angle between the agent and the neighboring agent. Based on the observation data acquired by the agent at the k-th sampling time, as well as the expected distance and expected azimuth angle between the agent and its neighboring agents, the structured residual that incorporates prior geometric relationships is calculated.

[0012] Furthermore, in some implementations, the expected distance between the agent and its neighboring agents is calculated using the following formula: ; The desired azimuth angle between the agent and its neighboring agents is calculated using the following formula: ; Structured residuals are calculated using the following formula: ; in, This represents the expected distance between the i-th agent and the j-th agent at the k-th sampling time. This represents the predicted x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted x-axis coordinate of the j-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the j-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the expected azimuth angle between the i-th agent and the j-th agent at the k-th sampling time. This represents the predicted orientation angle of the i-th agent at the k-th sampling time. This represents the structured residual between the i-th agent and the j-th agent at the k-th sampling time. This represents the observation data of the i-th agent on the j-th agent at the k-th sampling time. This indicates the arctangent function operation in the four quadrants, and the superscript T indicates the transpose operation.

[0013] Furthermore, in some embodiments, the process noise neural network and the posterior neural network are trained in the following manner: Step 311: Obtain the training data set, which includes: the agent's state value at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time, the agent's observation data of its neighboring agents at the kth sampling time, and the agent's true state value at the kth sampling time. Step 312: Based on the current process noise neural network and the current posterior neural network, obtain the predicted state value and prediction uncertainty of the agent at the k-th sampling time for each training data, as well as the posterior state value and posterior uncertainty of the agent at the k-th sampling time. Step 313: Input the posterior state value and posterior uncertainty of the agent at the k-th sampling time into the observation neural network, and obtain the predicted observation data and standard deviation of the predicted observation data of the agent to its neighbor agents at the k-th sampling time. Step 314: Based on the observation data of the agent to its neighboring agents at the k-th sampling time, the agent's true state value at the k-th sampling time, the posterior state value and posterior uncertainty of the agent at the k-th sampling time, and the predicted observation data and standard deviation of the predicted observation data of the agent to its neighboring agents at the k-th sampling time, calculate the preset loss function. Step 315: Determine whether the preset training stopping condition has been met. If not, update the parameters of the process noise neural network, the posterior neural network, and the observation neural network respectively using the preset loss function, and dynamically adjust the weights corresponding to each loss in the preset loss function based on the gradient of the preset loss function, and return to step 312 to continue iterative optimization. If yes, use the current process noise neural network, the current posterior neural network, and the current observation neural network as the neural networks that have completed training.

[0014] Furthermore, in some implementations, the loss function is expressed as: ; in, Represents the loss function. Indicates the reconstruction loss. Indicates the transfer of KL losses. This indicates that monitoring KL's losses, This represents the weights corresponding to the KL loss transition. This represents the weights corresponding to the supervised KL loss; The reconstruction loss is expressed as: ; The loss from transferring KL is expressed as: ; The KL loss under supervision is expressed as: ; Indicates the amount of training data. Indicates the training data index. This represents the observation data of an agent regarding its neighboring agents in the b-th training data set. and Let represent the predicted observation data and the standard deviation of the predicted observation data of the agent corresponding to the observation neural network outputting the b-th training data regarding its neighboring agents, respectively. This represents the posterior state value of the agent corresponding to the b-th training data point. This represents the posterior uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, This represents the predicted state value of the agent corresponding to the b-th training data. This represents the prediction uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, express and KL divergence between them This represents the true state value of the agent in the b-th training data. The standard deviation of the true state value. The mean is variance is Gaussian distribution, express and The KL divergence between them.

[0015] Secondly, a collaborative state estimation system based on Kalman filtering and deep learning is also provided. This system is applied to a multi-agent cluster, with the system deployed on each agent in the cluster. The system includes: The data acquisition module is used to acquire the state value of the agent at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time, as well as the observation data of the agent on its neighboring agents; The kinematic prediction module is equipped with a deterministic kinematic model and a process noise neural network. The kinematic prediction module is used to estimate the ideal state value of the agent at the k-th sampling time using the deterministic kinematic model based on the acquired state value and control value, and to estimate the mean and standard deviation of the process noise using the process noise neural network. Based on the ideal state value and the mean and standard deviation of the process noise, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are calculated. The communication module is used to exchange predicted state values ​​with neighboring agents. The neighboring agents are other agents within the agent's perception range. The agent only observes and communicates with the neighboring agents when it detects the presence of a neighboring agent within its perception range. The residual calculation module is used to calculate the structured residual that incorporates the prior knowledge of geometric relationships based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of itself sent by neighboring agents at the k-th sampling time. The posterior estimation module is equipped with a posterior neural network. The posterior estimation module is used to input the predicted state value, prediction uncertainty, observation data and structured residual of the agent at the k-th sampling time into the posterior neural network to obtain the posterior state value and posterior uncertainty of the agent at the k-th sampling time as output by the posterior neural network.

[0016] The main advantages of the technical solution of this invention are as follows: The collaborative state estimation method and system based on Kalman filtering and deep learning of this invention combines the structured prediction of Kalman filtering with neural networks, and uses a deterministic kinematic model for explicit prediction, which can ensure the interpretability and physical rationality of the final result. By using neural networks to dynamically estimate process noise and learn complex update mappings, the estimation accuracy and robustness to environmental interference can be significantly improved. This overcomes the accuracy degradation problem caused by model mismatch in strong nonlinear and non-Gaussian noise environments of traditional filtering methods, and avoids the problems of poor interpretability, weak generalization ability and large amount of training data required by pure data-driven methods. At the same time, the distributed execution mode is adopted, and different agents only share state values, which can reduce the communication load between different agents and improve computational efficiency. Attached Figure Description

[0017] The accompanying drawings, which are included to provide a further understanding of embodiments of the invention and constitute a part of this invention, illustrate exemplary embodiments of the invention and, together with their description, serve to explain the invention and do not constitute an undue limitation thereof. In the drawings: Figure 1 A flowchart illustrating a collaborative state estimation method based on Kalman filtering and deep learning, provided in an embodiment of the present invention; Figure 2 This is a schematic diagram of a collaborative state estimation system based on Kalman filtering and deep learning, provided as an embodiment of the present invention. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the technical solutions of this invention will be clearly and completely described below in conjunction with specific embodiments and corresponding drawings. Obviously, the described embodiments are only a part of the embodiments of this invention, and not all of them. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0019] The technical solutions provided by the embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0020] refer to Figure 1 In a first aspect, embodiments of the present invention provide a cooperative state estimation method based on Kalman filtering and deep learning, applied to a multi-agent cluster. This method is executed in a distributed manner on each agent and includes the following steps: Step 1: Obtain the state value of the agent at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time; Step 2: Based on the acquired state value and control value, estimate the ideal state value of the agent at the k-th sampling time using a preset deterministic kinematic model, and estimate the mean and standard deviation of the process noise using a pre-trained process noise neural network. Based on the ideal state value and the mean and standard deviation of the process noise, calculate the predicted state value and prediction uncertainty of the agent at the k-th sampling time. Step 3: Obtain the observation data of the agent on its neighboring agents. Based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of itself sent by the neighboring agents at the k-th sampling time, calculate the structured residual that incorporates the prior of geometric relations. Input the agent's predicted state value, prediction uncertainty, observation data, and structured residual at the k-th sampling time into a pre-trained posterior neural network to obtain the agent's posterior state value and posterior uncertainty at the k-th sampling time output by the posterior neural network.

[0021] In this embodiment of the invention, a deterministic kinematics model is used to estimate the ideal state value of the agent at the k-th sampling time based on the state value of the agent at the (k-1)-th sampling time and the control values ​​from the (k-1)-th sampling time to the k-th sampling time. A process noise neural network is used to estimate the mean and standard deviation of the process noise based on the state value of the agent at the (k-1)-th sampling time and the control values ​​from the (k-1)-th sampling time to the k-th sampling time.

[0022] In this embodiment of the invention, the input to the process noise neural network is the state value of the agent at the (k-1)th sampling time and the control value from the (k-1)th sampling time to the kth sampling time, and the output is the mean of the process noise and the logarithm of the standard deviation of the process noise.

[0023] In this embodiment of the invention, the predicted state value of the agent at the k-th sampling time is the sum of the ideal state value at the k-th sampling time and the mean of the process noise, and the prediction uncertainty of the agent at the k-th sampling time is the standard deviation of the process noise.

[0024] In this embodiment of the invention, the neighboring intelligent agents are other intelligent agents within the perception range of the intelligent agent. Only when the intelligent agent detects the existence of a neighboring intelligent agent within its perception range will it observe and communicate data with the neighboring intelligent agent.

[0025] In this embodiment of the invention, the input of the posterior neural network is the predicted state value, prediction uncertainty, observation data and structured residual of the agent at the k-th sampling time, and the output is the posterior state value and posterior uncertainty of the agent at the k-th sampling time.

[0026] In this embodiment of the invention, the posterior state value of the agent at the k-th sampling time is the final estimated state value of the agent at the k-th sampling time.

[0027] In this embodiment of the invention, each agent in the multi-agent cluster executes the above method in a distributed manner to obtain its own posterior state value and posterior uncertainty.

[0028] In this embodiment of the invention, the intelligent agent is a robot or a drone.

[0029] The collaborative state estimation method based on Kalman filtering and deep learning provided in this invention combines the structured prediction of Kalman filtering with neural networks. By using a deterministic kinematic model for explicit prediction, it ensures the interpretability and physical rationality of the final result. By using neural networks to dynamically estimate process noise and learn complex update mappings, it can significantly improve estimation accuracy and robustness to environmental interference. It overcomes the accuracy degradation problem caused by model mismatch in strong nonlinear and non-Gaussian noise environments of traditional filtering methods, and avoids the problems of poor interpretability, weak generalization ability, and the need for a large amount of training data in pure data-driven methods. At the same time, the distributed execution mode, where different agents only share state values, can reduce the communication load between different agents and improve computational efficiency.

[0030] Furthermore, in this embodiment of the invention, the state values ​​of the agent include: the x-axis coordinate of the agent in the two-dimensional world coordinate system, the y-axis coordinate of the agent in the two-dimensional world coordinate system, and the orientation angle of the agent; the control values ​​of the agent include: the linear velocity and angular velocity of the agent.

[0031] The orientation angle of the agent is defined as the angle between the agent's forward direction and the positive x-axis of the two-dimensional world coordinate system.

[0032] The two-dimensional world coordinate system is set according to the actual situation.

[0033] Furthermore, based on the aforementioned state and control values ​​of the agent, the deterministic kinematic model is expressed as: ; in, This represents the ideal x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the (k-1)-th sampling time. This represents the average linear velocity of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. This represents the average angular velocity of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. This represents the orientation angle of the i-th agent at the (k-1)-th sampling time. This represents the time interval between two adjacent sampling times. Indicates the angular velocity threshold. This represents the ideal y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the y-coordinate of the i-th agent in the two-dimensional world coordinate system at the (k-1)-th sampling time. This represents the ideal orientation angle of the i-th agent at the k-th sampling time. This represents the calculation of the cosine function. This indicates the calculation of the sine function.

[0034] In this embodiment of the invention, the angular velocity threshold is set according to actual needs, for example, to 0.001 rad / s. The angular velocity threshold is used to determine the motion mode of the agent. When the angular velocity is less than the angular velocity threshold, the motion of the agent is approximated as linear motion; otherwise, the motion of the agent is regarded as circular motion.

[0035] In this embodiment of the invention, the orientation angle of the agent is defined as the angle between the agent's forward direction and the positive x-axis of the two-dimensional world coordinate system, with counterclockwise being positive, and the orientation angle is normalized to... This ensures that the angle representation is unique and continuous.

[0036] In this embodiment of the invention, the angular velocity of the intelligent agent is positive when rotating counterclockwise.

[0037] In this embodiment of the invention, the deterministic kinematics model described above describes the motion law of the agent in a two-dimensional plane under ideal, noise-free, and completely accurate conditions. By using this deterministic kinematics model to calculate the ideal state value of the agent, physical priors can be provided for the subsequent hybrid filtering of the fused neural network, ensuring the physical rationality and interpretability of the state prediction.

[0038] Furthermore, in this embodiment of the invention, the mean and standard deviation of the process noise are estimated using a pre-trained process noise neural network.

[0039] Specifically, the output of the process noise neural network is represented as: ; in, Represents the mean vector of process noise. Represents the standard deviation vector of process noise. Represents a neural network for process noise. This represents the state value of the i-th agent at the (k-1)-th sampling time. , This represents the control value of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. , The superscript T indicates a logarithmic operation, and the superscript T indicates a transpose operation.

[0040] in, , This represents the mean of process noise along the x-axis. This represents the mean process noise along the y-axis. This represents the mean of process noise along the orientation angle dimension; in, , This represents the standard deviation of process noise along the x-axis. This represents the standard deviation of process noise along the y-axis. This represents the standard deviation of process noise in the angular dimension.

[0041] In this embodiment of the invention, the mean value of the process noise is estimated based on a process noise neural network. and standard deviation Process noise can be expressed as having a mean of variance is The Gaussian distribution of is specifically represented as: ; in, Represents the process noise vector. The mean is variance is Gaussian distribution, Indicated by It is a diagonal matrix with diagonal elements.

[0042] in, , This represents process noise along the x-axis. This represents the process noise along the y-axis. This represents process noise in the angular dimension.

[0043] In this embodiment of the invention, the process noise neural network employs a multilayer perceptron, and the parameters of the process noise neural network are obtained through training.

[0044] In this embodiment of the invention, by utilizing neural networks to dynamically estimate process noise related to the environment and motion state, it is possible to adaptively compensate for model mismatch and environmental disturbances, thereby significantly improving the accuracy and robustness of state estimation.

[0045] Furthermore, in this embodiment of the invention, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are determined by the following formula: ; ; in, This represents the predicted state value of the i-th agent at the k-th sampling time. Represents a deterministic kinematic model. This represents the prediction uncertainty of the i-th agent at the k-th sampling time.

[0046] Based on the above settings, , This represents the ideal state value of the i-th agent at the k-th sampling time.

[0047] Based on the above settings, , This represents the predicted x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. Let represent the predicted orientation angle of the i-th agent at the k-th sampling time.

[0048] In this embodiment of the invention, based on the above settings, the predicted state distribution of the agent at the k-th sampling time can be expressed as: ; in, Indicates that in the known and In the case of The probability distribution, The mean is variance is Gaussian distribution, Indicated by It is a diagonal matrix with diagonal elements.

[0049] In this embodiment of the invention, the ideal prediction of the deterministic kinematic model is combined with the non-zero mean process noise of the neural network dynamic estimation to determine the predicted state value and the prediction uncertainty, and an adaptive predicted state distribution is constructed. This achieves active compensation for system deviation and accurate quantification of prediction uncertainty, and can provide more reliable and informative prior estimates for subsequent update steps.

[0050] Furthermore, in this embodiment of the invention, the observation data of the intelligent agent on its neighboring intelligent agents includes: the relative distance and relative azimuth angle between the intelligent agent and its neighboring intelligent agents.

[0051] In this embodiment of the invention, the relative distance can be measured using a UWB sensor mounted on the agent, and the relative azimuth angle can be measured using a vision sensor mounted on the agent.

[0052] Furthermore, based on the observation data set above, and based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of itself sent by neighboring agents at the k-th sampling time, the structured residual that incorporates the prior knowledge of geometric relationships is calculated in the following manner: Based on the predicted state value of the agent at the k-th sampling time and the predicted state value of itself sent by the neighboring agent at the k-th sampling time, calculate the expected distance and expected azimuth angle between the agent and the neighboring agent. Based on the observation data acquired by the agent at the k-th sampling time, as well as the expected distance and expected azimuth angle between the agent and its neighboring agents, the structured residual that incorporates prior geometric relationships is calculated.

[0053] In this embodiment of the invention, taking the j-th intelligent agent as a neighboring intelligent agent as an example, the expected distance between the intelligent agent and its neighboring intelligent agents is calculated using the following formula: ; Taking the j-th intelligent agent as a neighboring intelligent agent as an example, the expected azimuth angle between the intelligent agent and its neighboring intelligent agents is calculated using the following formula: ; Taking the j-th agent as an example, the structured residual, which incorporates prior geometric relationships, is calculated using the following formula: ; in, This represents the expected distance between the i-th agent and the j-th agent at the k-th sampling time. This represents the predicted x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted x-axis coordinate of the j-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the j-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the expected azimuth angle between the i-th agent and the j-th agent at the k-th sampling time. This represents the predicted orientation angle of the i-th agent at the k-th sampling time. This represents the structured residual between the i-th agent and the j-th agent at the k-th sampling time. This represents the observation data of the i-th agent on the j-th agent at the k-th sampling time. This represents the arctangent function operation in the four quadrants.

[0054] in, , This represents the relative distance between the i-th agent and the j-th agent at the k-th sampling time. This represents the relative azimuth angle between the i-th agent and the j-th agent at the k-th sampling time.

[0055] In this embodiment of the invention, an agent can obtain a corresponding observation data with a neighboring agent. When an agent has multiple neighboring agents, it can obtain multiple observation data. At this time, the multiple observation data are concatenated and processed as the input of the posterior neural network.

[0056] In this embodiment of the invention, an agent and a neighboring agent can calculate a corresponding structured residual. When an agent has multiple neighboring agents, multiple structured residuals can be calculated. At this time, the calculated multiple structured residuals are concatenated and processed as the input of the posterior neural network.

[0057] Furthermore, in this embodiment of the invention, the agent's posterior state value and posterior uncertainty are estimated using a pre-trained posterior neural network.

[0058] Specifically, the output of the posterior neural network is represented as: ; in, This represents the posterior state value of the i-th agent at the k-th sampling time. This represents the posterior uncertainty of the i-th agent at the k-th sampling time. This represents a posterior neural network. This represents the predicted state value of the i-th agent at the k-th sampling time. This represents the prediction uncertainty of the i-th agent at the k-th sampling time. This represents the observation data of the i-th agent on its neighboring agents at the k-th sampling time. This represents the structured residual acquired by the i-th agent at the k-th sampling time.

[0059] Based on the above settings, , This represents the posterior x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the posterior y-axis coordinate of the i-th agent at the k-th sampling time in the two-dimensional world coordinate system. Let represent the posterior orientation angle of the i-th agent at the k-th sampling time.

[0060] In this embodiment of the invention, the posterior neural network employs a multilayer perceptron, and the parameters of the posterior neural network are obtained through training.

[0061] In this embodiment of the invention, based on the above settings, the posterior state of the agent at the k-th sampling time can also be expressed as: ; in, The mean is variance is Gaussian distribution, Indicated by It is a diagonal matrix with diagonal elements.

[0062] In this embodiment of the invention, by introducing structured residuals that incorporate prior geometric relationships, the neural network can explicitly utilize physical relationships to correct errors, achieving efficient collaboration and probabilistic fusion of multi-source information, thereby significantly improving the accuracy, robustness, and interpretability of state estimation.

[0063] Furthermore, in this embodiment of the invention, the process noise neural network and the posterior neural network are trained together, specifically through the following training method: Step 311: Obtain the training data set, which includes: the agent's state value at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time, the agent's observation data of its neighboring agents at the kth sampling time, and the agent's true state value at the kth sampling time. Step 312: Based on the current process noise neural network and the current posterior neural network, obtain the predicted state value and prediction uncertainty of the agent at the k-th sampling time for each training data, as well as the posterior state value and posterior uncertainty of the agent at the k-th sampling time. Step 313: Input the posterior state value and posterior uncertainty of the agent at the k-th sampling time into the observation neural network, and obtain the predicted observation data and standard deviation of the predicted observation data of the agent to its neighbor agents at the k-th sampling time. Step 314: Based on the observation data of the agent to its neighboring agents at the k-th sampling time, the agent's true state value at the k-th sampling time, the posterior state value and posterior uncertainty of the agent at the k-th sampling time, and the predicted observation data and standard deviation of the predicted observation data of the agent to its neighboring agents at the k-th sampling time, calculate the preset loss function. Step 315: Determine whether the preset training stopping condition has been met. If not, update the parameters of the process noise neural network, the posterior neural network, and the observation neural network respectively using the preset loss function, and dynamically adjust the weights corresponding to each loss in the preset loss function based on the gradient of the preset loss function, and return to step 312 to continue iterative optimization. If yes, use the current process noise neural network, the current posterior neural network, and the current observation neural network as the neural networks that have completed training.

[0064] In this embodiment of the invention, at the start of training, the parameters of the process noise neural network, the posterior neural network, and the observation neural network are all initialized parameters. During the training process, the parameters of the neural networks are continuously updated and optimized.

[0065] In this embodiment of the invention, the observation neural network employs a multilayer perceptron.

[0066] In this embodiment of the invention, the training dataset is obtained from historical data.

[0067] In this embodiment of the invention, the predicted state value and prediction uncertainty of the agent corresponding to each training data at the k-th sampling time are obtained in accordance with the method described in step 2, and the posterior state value and posterior uncertainty of the agent corresponding to each training data at the k-th sampling time are obtained in accordance with the method described in step 3.

[0068] In this embodiment of the invention, the training stop condition is set according to actual needs, for example, it is set to the number of training iterations reaching a set number.

[0069] In this embodiment of the invention, the parameters of the neural network are updated and optimized by calculating the gradient of the preset loss function with respect to the parameters of the neural network using the gradient descent method.

[0070] Furthermore, in this embodiment of the invention, the process noise neural network and the posterior neural network employ the following loss function during training: ; in, Represents the loss function. Indicates the reconstruction loss. Indicates the transfer of KL losses. This indicates that monitoring KL's losses, This represents the weights corresponding to the KL loss transition. This represents the weight corresponding to the supervised KL loss.

[0071] In this embodiment of the invention, the weights corresponding to the KL loss are transferred. Weights corresponding to supervised KL loss During training, the network dynamically adjusts its parameters based on the gradient of the preset loss function and gradient descent, updating synchronously with the network parameters through backpropagation. Specifically, adding a regularization term to the weights can prevent excessive weight decay.

[0072] In this embodiment of the invention, the reconstruction loss is expressed as: ; in, Indicates the amount of training data. Indicates the training data index. This represents the observation data of an agent regarding its neighboring agents in the b-th training data set. and Let represent the predicted observation data and the standard deviation of the predicted observation data of the agent output by the observation neural network corresponding to the b-th training data to its neighboring agents, respectively.

[0073] In this embodiment of the invention, the transfer KL loss is expressed as: ; in, Indicates the amount of training data. Indicates the training data index. This represents the posterior state value of the agent corresponding to the b-th training data point. This represents the posterior uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, This represents the predicted state value of the agent corresponding to the b-th training data. This represents the prediction uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, express and The KL divergence between them.

[0074] In this embodiment of the invention, the supervised KL loss is expressed as: ; in, Indicates the amount of training data. Indicates the training data index. This represents the posterior state value of the agent corresponding to the b-th training data point. This represents the posterior uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, This represents the true state value of the agent in the b-th training data. The standard deviation of the true state value. The mean is variance is Gaussian distribution, express and The KL divergence between them.

[0075] The standard deviation of the true state value is a preset hyperparameter, and the specific value can be set according to actual needs, for example, 0.1.

[0076] In this embodiment of the invention, the reconstruction loss is used to maximize the likelihood of the observed data, ensuring that the posterior state estimate can accurately predict the actual reading of the sensor; the transition KL loss is used to constrain the consistency between the posterior distribution and the prior prediction distribution, so that the state estimate conforms to the prior physical model of Bayesian filtering; and the supervised KL loss is used to make the posterior distribution approximate the true state, thereby improving the accuracy of the state estimate.

[0077] refer to Figure 2 Secondly, embodiments of the present invention also provide a collaborative state estimation system based on Kalman filtering and deep learning, the system comprising: The data acquisition module 100 is used to acquire the state value of the agent at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time, as well as the observation data of the agent on its neighboring agents; The kinematic prediction module 200 is equipped with a deterministic kinematic model and a process noise neural network. The kinematic prediction module 200 is used to estimate the ideal state value of the agent at the k-th sampling time using the deterministic kinematic model based on the acquired state value and control value, and to estimate the mean and standard deviation of the process noise using the process noise neural network. Based on the ideal state value and the mean and standard deviation of the process noise, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are calculated. The communication module 300 is used to exchange predicted state values ​​with neighboring intelligent agents. The neighboring intelligent agents are other intelligent agents within the perception range of the intelligent agent. The intelligent agent will observe and communicate data with the neighboring intelligent agents only when it detects the existence of a neighboring intelligent agent within its perception range. The residual calculation module 400 is used to calculate the structured residual that incorporates the prior knowledge of geometric relationships based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of itself sent by the neighboring agents at the k-th sampling time. The posterior estimation module 500 is equipped with a posterior neural network. The posterior estimation module 500 is used to input the predicted state value, prediction uncertainty, observation data and structured residual of the agent at the k-th sampling time into the posterior neural network to obtain the posterior state value and posterior uncertainty of the agent at the k-th sampling time as output by the posterior neural network.

[0078] In this embodiment of the invention, the system is applied to a multi-agent cluster, and the system is deployed on each agent in the multi-agent cluster.

[0079] Furthermore, in this embodiment of the invention, the system further includes: an observation neural network module, which is configured with an observation neural network. The observation neural network module is used to map the acquired posterior state value and posterior uncertainty of the agent at the k-th sampling time into the predicted observation data and predicted observation data standard deviation of the agent to its neighbor agents at the k-th sampling time during the training phase of the process noise neural network and the posterior neural network, so as to realize the parameter update of the neural network.

[0080] It should be noted that during the deployment phase of this system, the observation neural network module is inactive.

[0081] In this embodiment of the invention, each of the above modules is a device corresponding to the above method steps. The specific working principle and beneficial effects of each module can be found in the above method, and will not be repeated here.

[0082] It should be noted that, in this document, relational terms such as “first” and “second” are used merely to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms “comprising,” “including,” or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus.

[0083] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A collaborative state estimation method based on Kalman filtering and deep learning, characterized in that, The method is applied to a multi-agent cluster, and is executed in a distributed manner on each agent. The method includes: Obtain the agent's state value at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time; Based on the acquired state value and control value, the ideal state value of the agent at the k-th sampling time is estimated using a pre-set deterministic kinematic model, and the mean and standard deviation of the process noise are estimated using a pre-trained process noise neural network. Based on the ideal state value and the mean and standard deviation of the process noise, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are calculated. The system acquires the observation data of the agent on its neighboring agents. Based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of the agent itself sent by the neighboring agents at the k-th sampling time, it calculates the structured residual that incorporates the prior of geometric relations. The agent's predicted state value, prediction uncertainty, observation data, and structured residual at the k-th sampling time are input into a pre-trained posterior neural network to obtain the agent's posterior state value and posterior uncertainty at the k-th sampling time output by the posterior neural network. Among them, the neighboring intelligent agents are other intelligent agents within the perception range of the intelligent agent. Only when the intelligent agent detects the existence of a neighboring intelligent agent within its perception range will it conduct observations and communicate data with the neighboring intelligent agents.

2. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 1, characterized in that, The state values ​​of an agent include: the agent's x-axis coordinate in a two-dimensional world coordinate system, the agent's y-axis coordinate in a two-dimensional world coordinate system, and the agent's orientation angle; The control values ​​of the agent include: the agent's linear velocity and angular velocity.

3. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 2, characterized in that, The deterministic kinematic model is expressed as follows: ； in, This represents the ideal x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the (k-1)-th sampling time. This represents the average linear velocity of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. This represents the average angular velocity of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. This represents the orientation angle of the i-th agent at the (k-1)-th sampling time. This represents the time interval between two adjacent sampling times. Indicates the angular velocity threshold. This represents the ideal y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the y-coordinate of the i-th agent in the two-dimensional world coordinate system at the (k-1)-th sampling time. This represents the ideal orientation angle of the i-th agent at the k-th sampling time. This represents the calculation of the cosine function. This indicates the calculation of the sine function.

4. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 3, characterized in that, The predicted state value and prediction uncertainty of the agent at the k-th sampling time are determined by the following formula: ； ； in, This represents the predicted state value of the i-th agent at the k-th sampling time. Represents a deterministic kinematic model. This represents the state value of the i-th agent at the (k-1)-th sampling time. This represents the control value of the i-th agent from the (k-1)-th sampling time to the k-th sampling time. Represents the mean vector of process noise. This represents the prediction uncertainty of the i-th agent at the k-th sampling time. This represents the standard deviation vector of process noise.

5. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 4, characterized in that, The observation data of an agent on its neighboring agents includes: the relative distance and relative azimuth angle between the agent and its neighboring agents.

6. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 5, characterized in that, The structured residuals, which incorporate prior geometric relationships, are calculated in the following manner: Based on the predicted state value of the agent at the k-th sampling time and the predicted state value of itself sent by the neighboring agent at the k-th sampling time, calculate the expected distance and expected azimuth angle between the agent and the neighboring agent. Based on the observation data acquired by the agent at the k-th sampling time, as well as the expected distance and expected azimuth angle between the agent and its neighboring agents, the structured residual that incorporates prior geometric relationships is calculated.

7. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 6, characterized in that, The expected distance between an agent and its neighboring agents is calculated using the following formula: ； The desired azimuth angle between the agent and its neighboring agents is calculated using the following formula: ； Structured residuals are calculated using the following formula: ； in, This represents the expected distance between the i-th agent and the j-th agent at the k-th sampling time. This represents the predicted x-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted x-axis coordinate of the j-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the i-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the predicted y-axis coordinate of the j-th agent in the two-dimensional world coordinate system at the k-th sampling time. This represents the expected azimuth angle between the i-th agent and the j-th agent at the k-th sampling time. This represents the predicted orientation angle of the i-th agent at the k-th sampling time. This represents the structured residual between the i-th agent and the j-th agent at the k-th sampling time. This represents the observation data of the i-th agent on the j-th agent at the k-th sampling time. This indicates the arctangent function operation in the four quadrants, and the superscript T indicates the transpose operation.

8. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 1, characterized in that, The process noise neural network and the posterior neural network are trained in the following manner: Step 311: Obtain the training data set, which includes: the agent's state value at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time, the agent's observation data of its neighboring agents at the kth sampling time, and the agent's true state value at the kth sampling time. Step 312: Based on the current process noise neural network and the current posterior neural network, obtain the predicted state value and prediction uncertainty of the agent at the k-th sampling time for each training data, as well as the posterior state value and posterior uncertainty of the agent at the k-th sampling time. Step 313: Input the posterior state value and posterior uncertainty of the agent at the k-th sampling time into the observation neural network, and obtain the predicted observation data and standard deviation of the predicted observation data of the agent to its neighbor agents at the k-th sampling time. Step 314: Based on the observation data of the agent to its neighboring agents at the k-th sampling time, the agent's true state value at the k-th sampling time, the posterior state value and posterior uncertainty of the agent at the k-th sampling time, and the predicted observation data and standard deviation of the predicted observation data of the agent to its neighboring agents at the k-th sampling time, calculate the preset loss function. Step 315: Determine whether the preset training stopping condition has been met. If not, update the parameters of the process noise neural network, the posterior neural network, and the observation neural network respectively using the preset loss function, and dynamically adjust the weights corresponding to each loss in the preset loss function based on the gradient of the preset loss function, and return to step 312 to continue iterative optimization. If yes, use the current process noise neural network, the current posterior neural network, and the current observation neural network as the neural networks that have completed training.

9. The cooperative state estimation method based on Kalman filtering and deep learning according to claim 8, characterized in that, The loss function is expressed as: ； in, Represents the loss function. Indicates the reconstruction loss. Indicates the transfer of KL losses. This indicates that monitoring KL's losses, This represents the weights corresponding to the KL loss transition. This represents the weights corresponding to the supervised KL loss; The reconstruction loss is expressed as: ； The loss from transferring KL is expressed as: ； The KL loss under supervision is expressed as: ； Indicates the amount of training data. Indicates the training data index. This represents the observation data of an agent regarding its neighboring agents in the b-th training data set. and Let represent the predicted observation data and the standard deviation of the predicted observation data of the agent corresponding to the observation neural network outputting the b-th training data regarding its neighboring agents, respectively. This represents the posterior state value of the agent corresponding to the b-th training data point. This represents the posterior uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, This represents the predicted state value of the agent corresponding to the b-th training data. This represents the prediction uncertainty of the agent corresponding to the b-th training data. The mean is variance is Gaussian distribution, express and KL divergence between them This represents the true state value of the agent in the b-th training data. The standard deviation of the true state value. The mean is variance is Gaussian distribution, express and The KL divergence between them.

10. A cooperative state estimation system based on Kalman filtering and deep learning, characterized in that, The system is applied to a multi-agent cluster, and the system is deployed on each agent in the multi-agent cluster. The system includes: The data acquisition module is used to acquire the state value of the agent at the (k-1)th sampling time and the control values ​​from the (k-1)th sampling time to the kth sampling time, as well as the observation data of the agent on its neighboring agents; The kinematic prediction module is equipped with a deterministic kinematic model and a process noise neural network. The kinematic prediction module is used to estimate the ideal state value of the agent at the k-th sampling time using the deterministic kinematic model based on the acquired state value and control value, and to estimate the mean and standard deviation of the process noise using the process noise neural network. Based on the ideal state value and the mean and standard deviation of the process noise, the predicted state value and prediction uncertainty of the agent at the k-th sampling time are calculated. The communication module is used to exchange predicted state values ​​with neighboring agents. The neighboring agents are other agents within the agent's perception range. The agent only observes and communicates with the neighboring agents when it detects the presence of a neighboring agent within its perception range. The residual calculation module is used to calculate the structured residual that incorporates the prior knowledge of geometric relationships based on the agent's predicted state value at the k-th sampling time, the observation data, and the predicted state value of itself sent by neighboring agents at the k-th sampling time. The posterior estimation module is equipped with a posterior neural network. The posterior estimation module is used to input the predicted state value, prediction uncertainty, observation data and structured residual of the agent at the k-th sampling time into the posterior neural network to obtain the posterior state value and posterior uncertainty of the agent at the k-th sampling time as output by the posterior neural network.

Images