Constrained one-step ahead measurement particle filtering method, device, system and medium

Abstract

The application provides a particle filtering method, device, system and medium for constraining one-step advanced measurement, proposes a constraint and given dynamic model fusion strategy, constructs a prediction distribution center value movement criterion to obtain a constraint auxiliary dynamic model, improves dynamic modeling accuracy, takes the dynamic model with the constraint as a proposal distribution function, improves particle filter sampling efficiency, constructs a cost function of the constraint auxiliary dynamic model to measure the inconsistency degree of the given dynamic evolution model and the constraint, and then evaluates the importance degree of particles, updates the particle weight at the current moment by using the constraint information at the current moment and the next moment and the measurement, so as to improve state estimation accuracy, and under the condition that the dynamic evolution model meets the constraint condition, the application can be degraded into a traditional sequential importance resampling particle filter, and is suitable for a wide range of applications.

Description

Technical Field

[0001] The present invention belongs to the technical field of signal processing, and in particular relates to a particle filtering method, device, system and medium for constraining one-step advance measurement. Background Art

[0002] Constrained dynamic stochastic systems are widely present in both civil and military fields. For example, in ground target tracking by airborne and satellite-borne radars, vehicles are constrained by roads, terrain, and topography. Another example is that aircraft attitude estimation requires the sum of the squares of its state components (quaternions) to be 1. Although these constraints limit the system's state space, they also imply the evolutionary laws and value ranges of the states. From an information fusion perspective, effectively utilizing these constraints can significantly improve system filtering accuracy, significantly benefiting system analysis, identification, and control. Consequently, constrained filtering algorithms have garnered widespread attention in recent years.

[0003] However, existing constrained filtering algorithms only use constraints at the current moment. In fact, future constraints are also effective. Taking the final state constraint as an example, in an attack radar system, in order to prevent the radar array from being attacked, people use the prior knowledge of the radar position to improve the accuracy of anti-radiation missile motion modeling, thereby obtaining better target tracking results to achieve missile interception missions. In other words, future constraint information is effective additional information for dynamic random systems. It lacks constraint information at the current and subsequent moments, and the filtering accuracy is relatively low. Summary of the Invention

[0004] In response to the problems existing in the prior art, the present invention provides a particle filtering method, device, system and medium for constraining one-step advance measurement, which can improve the accuracy of filtering.

[0005] The present invention is achieved through the following technical solutions:

[0006] The particle filtering method for constrained one-step-ahead measurement is characterized by comprising the following steps:

[0007] By integrating the inequality constraint information with the given dynamic model, the center value of the state prediction distribution is moved to the feasible region to obtain a constraint-assisted dynamic model.

[0008] The Bayesian formula is used to derive the decomposition structure of the state posterior density function under measurement and constraint conditions, including the state posterior distribution under the current moment constraint conditions and the current moment state distribution under the next moment constraint conditions;

[0009] Under the current constraints, the given dynamic model and the constraint-assisted dynamic model are selected as the proposed distribution function one by one, and particles that meet the constraints are sampled and retained from the given dynamic model; for particles that do not meet the constraints, the corresponding constraint-assisted dynamic model is constructed and calculated, and particles that meet the constraints are sampled and retained;

[0010] The method of constructing the moving cost function compensates for the influence of the dynamic model correction on the particle distribution, calculates the measurement likelihood function and the cost function to update the particle weight, and obtains the state posterior distribution under the current time constraint conditions;

[0011] Under the constraints of the next moment, it is determined whether the center value of the predicted distribution of each constrained particle meets the constraints of the next moment. For the center value that exceeds the feasible region, the corresponding particle constraint auxiliary dynamic model method is constructed to ensure that the particle trajectory meets the constraint information of two consecutive moments, and the moving cost function method is used to update the current particle weight.

[0012] By fusing the constraints of the current moment and the constraints of the next moment to construct a dynamic model, the state posterior density function of the constraint one-step ahead measurement is obtained, and the state estimation result is output.

[0013] Furthermore, the constraint-assisted dynamic model is obtained by constraining and correcting a given dynamic model, and under the current constraint conditions, includes the following steps:

[0014] The likelihood function based on the constraint is p(C k |x k ):

[0015]

[0016] If the state x k Satisfy constraint C k When , the likelihood function value is 1; otherwise, it is 0;

[0017] The dynamic equation is x k =f(x k-1 )+w k , w k is process noise, with mean 0 and covariance Q k The Gaussian distribution of the probability density function is expressed as p(x k |x k-1 );

[0018] The predicted state set is usually clustered around the center value of the predicted distribution. The predicted center value is moved to the constraint range by constructing the moving criterion function. The center value of the unconstrained state predicted distribution is The center value that satisfies the constraint is u obtained by solving the moving criterion function k ,like If the constraints are satisfied, Otherwise k Solve the optimization function using constrained optimization methods:

[0019]

[0020] It is used to find a point with the largest prediction probability density in the feasible region as the new prediction center value, effectively integrating the given dynamic model information and constraints;

[0021] The obtained constraint-assisted dynamic model is x k =u k +w k .

[0022] 3. The particle filtering method with one-step-ahead measurement constraints according to claim 2, wherein the decomposition structure of the state posterior density function is derived using the Bayesian formula under measurement and constraint conditions, comprising the following steps:

[0023] Measurement equation y k =h(x k )+v k , its probability density function expression is p(y k |x k ), using the Bayesian formula to decompose the state posterior density function under measurement and constraint conditions, using the known measurement sequence y 1:k and constraint sequence C 1:k+1 , calculate the state posterior density function:

[0024]

[0025] Among them, p(x k |y 1:k ,C 1:k ) is the state posterior density function under the current constraint, p(C k+1 |x k ) is the state x k The predicted state satisfies the likelihood function of the next moment constraint.

[0026] Furthermore, a particle filter algorithm for sampling particles by using the given dynamic model and the constraint-assisted dynamic model as the proposed distribution function one by one includes the following steps:

[0027] The weighted particle set at the previous moment is is the i-th particle, N is the number of particles, is the normalized weight of the ith particle and The sum of the particle weights is 1, that is According to the particle weight definition, calculate the particles The non-normalized weights of:

[0028]

[0029] It is known that the typical bootstrap particle filter takes the dynamic model as the proposed distribution. On this basis, the present invention incorporates the constraints into the proposed distribution, and the proposed distribution function q(xk |x k-1 ,y k ,C k )=p(x k |x k-1 ,C k ), using the Bayesian formula to derive, the recommended distribution function of the i-th particle is

[0030] Will Sampling is performed as a proposed distribution function, and particles that meet the constraints are retained. For particles that do not meet the constraints, the constraint-assisted dynamic model of the i-th particle is obtained by constructing and solving the movement criterion function, which is used as the proposed distribution.

[0031] According to claim 2, the modified dynamic model is x k =u k +w k , sampled from this dynamic model, i.e. Particles that satisfy the constraints are retained.

[0032] Furthermore, the cost function and the measurement likelihood function are calculated to update the particle weights. The steps are as follows:

[0033] For a given dynamic model as a constraint particle obtained from the proposed distribution, the non-normalized weight of the i-th particle is calculated as:

[0034] For the constrained particles obtained as the proposed distribution by the constraint-assisted dynamic model, the non-normalized weight of the i-th particle is calculated as:

[0035]

[0036] in, is the cost of the mobile dynamic model, which is used to measure the degree of inconsistency between the given dynamic evolution model and the constraints. The greater the inconsistency, the lower the importance of the particle. The state posterior density function based on the current constraint conditions is:

[0037]

[0038] Here, δ(·) is the Dirac delta function.

[0039] Furthermore, under the next moment constraint, the process of updating the particle weight at the current moment includes the following steps:

[0040] For constrained particles For example, the probability density function of the current state under the next moment constraint is:

[0041]

[0042] Modify the i-th dynamic model to obtain:

[0043]

[0044] Calculate the movement cost function Used to measure the inconsistency between particle evolution model and constraints.

[0045] Furthermore, the state estimation under the constraints of the current and next moments includes the following steps:

[0046]

[0047] Among them, the normalized weight and

[0048]

[0049] Calculate state estimation results

[0050] A device for constraining a one-step-ahead measurement particle filter, characterized by comprising:

[0051] The constraint-assisted dynamic model module is used to fuse the inequality constraint information with the given dynamic model, move the center value of the state prediction distribution to the feasible domain, and obtain the constraint-assisted dynamic model;

[0052] The constrained state decomposition module uses the Bayesian formula to derive the state posterior density function decomposition structure under measurement and constraint conditions, including the state posterior distribution under the current moment constraint conditions and the current moment state distribution under the next moment constraint conditions;

[0053] The proposed distribution function module is used to select the given dynamic model and the constraint-assisted dynamic model as the proposed distribution function in turn under the current moment constraints, sample and retain particles that meet the constraints from the given dynamic model; for particles that do not meet the constraints, construct and calculate the corresponding constraint-assisted dynamic model, sample and retain particles that meet the constraints;

[0054] The state estimation module under the current constraints is used to construct a method of moving cost function to compensate for the impact of dynamic model correction on particle distribution. The particle weights are updated through measurement and cost function to obtain the posterior distribution of the state under the current constraints.

[0055] The particle update module under the next moment constraint is used to determine whether the center value of the predicted distribution of each constrained particle meets the next moment constraint under the next moment constraint. For center values ​​that exceed the feasible region, the corresponding particle constraint auxiliary dynamic model method is constructed to ensure that the particle trajectory meets the constraint information of two consecutive moments, and the particle weight is updated using the moving cost function method.

[0056] The output state estimation module is used to construct a dynamic model by fusing the constraints of the current moment and the constraints of the next moment, obtain the state posterior density function of the constraint one step ahead measurement, and output the state estimation result.

[0057] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the particle filtering method with constrained one-step-ahead measurement are implemented.

[0058] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the particle filtering method with constrained one-step-ahead measurement.

[0059] Compared with the prior art, the present invention has the following beneficial technical effects:

[0060] The present invention provides a particle filtering method, device, system and medium for constrained one-step-ahead measurement. A constraint and a given dynamic model fusion strategy is adopted to construct a prediction distribution center value movement criterion to obtain a constraint-assisted dynamic model, thereby improving the accuracy of dynamic modeling. The dynamic model incorporating the constraints is used as a suggested distribution function, thereby improving the sampling efficiency of the particle filter. By constructing a cost function of the constraint-assisted dynamic model, the degree of inconsistency between the given dynamic evolution model and the constraints is measured, thereby evaluating the importance of the particles. The constraint information and measurements at the current and subsequent moments are used to update the particle weight at the current moment, thereby improving the accuracy of state estimation. When the dynamic evolution model meets the constraint conditions, it can be degenerated into a traditional sequence importance resampling particle filter, which has strong applicability. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] Figure 1 This is a flow chart of a particle filtering method for one-step advance measurement with inequality constraints according to the present invention;

[0062] Figure 2 This is a specific flow chart of the filtering method in an embodiment of the present invention;

[0063] Figure 3 Schematic diagram of the change of the center value of the particle prediction distribution at the current and next moments in an embodiment of the present invention;

[0064] Figure 4 This is a schematic diagram of a ship sailing in a canal according to an embodiment of the present invention;

[0065] Figure 5 100 Monte Carlo simulation position results of the constraint simulation experiment in the embodiment of the present invention;

[0066] Figure 6100 Monte Carlo simulation speed results of the constrained simulation experiment in the embodiment of the present invention. DETAILED DESCRIPTION

[0067] The present invention will be further described in detail below with reference to specific embodiments, which are intended to explain the present invention rather than to limit it.

[0068] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0069] It should be noted that the terms "first", "second", etc. in the description and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that the numbers used in this way can be interchanged where appropriate, so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having" and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product or device that includes a series of steps or units is not necessarily limited to those steps or units clearly listed, but may include other steps or units that are not clearly listed or inherent to these processes, methods, products or devices.

[0070] The present invention provides a particle filtering method for one-step advance measurement with inequality constraints, such as Figure 1 As shown, the following steps are included:

[0071] The inequality constraint is integrated with the given dynamic model, and the center value of the state prediction distribution is moved to the feasible region by constructing a moving criterion function to obtain a constraint-assisted dynamic model.

[0072] The Bayesian formula is used to derive the decomposition structure of the state posterior density function under measurement and constraint conditions, including the state posterior distribution under the current moment constraint conditions and the current state distribution under the next moment constraint conditions;

[0073] Under the current constraints, the given dynamic model and the constraint-assisted dynamic model are selected as the proposed distribution function one by one. The given dynamic model is used to sample and retain particles that meet the constraints. For particles that do not meet the constraints, the corresponding constraint-assisted dynamic model is constructed and calculated, and then particles that meet the constraints are sampled and retained.

[0074] The effect of dynamic model correction on particle distribution is compensated by constructing a moving cost function. The cost function and measurement likelihood function are calculated to update the particle weights. The weights of the constrained particles sampled based on the given dynamic model are updated through the measurement likelihood function. Finally, the posterior distribution of the state under the current constraint conditions is obtained.

[0075] Under the constraints of the next moment, determine whether the predicted distribution center value of each constrained particle in step 4 meets the constraints of the next moment. For the center value that exceeds the feasible region, construct a constraint-assisted dynamic model for the corresponding particle and use the movement cost function to update the particle weight to ensure that the particle trajectory meets the constraint information of two consecutive moments.

[0076] By fusing the constraints of the current moment and the constraints of the next moment to construct a dynamic model, the state posterior density function of the constraint one-step ahead measurement is obtained, and the state estimation result is output.

[0077] Furthermore, taking the current moment constraint as an example, the constraint auxiliary dynamic model is obtained by modifying the given dynamic model of the current constraint, including the following steps:

[0078] The likelihood function based on the constraint is p(C k |x k ):

[0079]

[0080] Among them, the state x k Satisfy constraint C k When , the likelihood function value is 1; otherwise, it is 0;

[0081] The dynamic equation is x k =f(x k-1 )+w k , its probability expression is p(x k |x k-1 );

[0082] The predicted state set is usually clustered around the center value of the predicted distribution, and the construction criterion function moves the center value within the constraint range; the center value of the unconstrained state predicted distribution is Get the moving criterion function and solve it to obtain the center value u that satisfies the constraints k , that is, find a point u in the constraint space k Make it with The Mahalanobis distance is the smallest. If If the constraints are satisfied, Otherwise k Solve the optimization function using constrained optimization methods:

[0083]

[0084] The constraint-assisted dynamic model is x k =u k +w k .

[0085] Furthermore, the Bayesian formula is used to derive the state posterior density function decomposition structure under the measurement and constraint conditions, including the following steps:

[0086] Based on the measurement equation y k =h(x k )+v k And the probability expression p(y k |x k ), using the Bayesian formula to decompose the state posterior density function under measurement and constraint conditions, using the known measurement sequence y 1:k and constraint sequence C 1:k+1 , calculate the state posterior density function:

[0087]

[0088] Furthermore, a particle filter algorithm that uses the dynamic model incorporating constraints as a suggestion distribution function for particle sampling includes the following steps:

[0089] The weighted particle set at the previous moment is N is the number of particles, and the sum of particle weights is 1, that is, Then calculate the particles The non-normalized weights of:

[0090]

[0091] Let the proposed distribution q(x k |x k-1 ,y k ,C k )=p(x k |x k-1 ,C k ), then the recommended distribution function of the i-th particle is Will The proposed distribution function is used for sampling, and particles that meet the constraints are retained. For particles that do not meet the constraints, the constraint-assisted dynamic model of the i-th particle is obtained by constructing and solving the movement criterion function, which is used as the proposed distribution.

[0092] The modified dynamic model is x k =u k +w k , are adopted from this dynamic model, and particles that satisfy the constraints are retained.

[0093] Furthermore, the cost function and the measurement likelihood function are calculated to update the particle weights, including the following steps:

[0094] For a given dynamic model as a constraint particle obtained from the proposed distribution, the non-normalized weight of the i-th particle is:

[0095] Sampling from the constraint-assisted dynamic model, i.e. Calculate the non-normalized weight of the i-th particle:

[0096]

[0097] in, The cost of the mobile dynamic model is to measure the degree of inconsistency between the given dynamic evolution model and the constraints. The greater the inconsistency, the lower the importance of the particle. The state posterior density function based on the current constraint is:

[0098]

[0099] Furthermore, under the next moment constraint, the process of updating the particle weight at the current moment includes the following steps:

[0100] For constrained particles For example, the state probability density function under the next moment constraint is:

[0101]

[0102] Modify the i-th dynamic model to obtain:

[0103]

[0104] Calculate the movement cost function Used to measure the impact of the moving prediction center value on the particle evolution model.

[0105] Furthermore, the state estimation under the constraints of the current and next moments includes the following steps:

[0106] Assume that the state posterior density function to be solved is:

[0107]

[0108] Among them, the normalized weight and

[0109]

[0110] Calculate state estimation results

[0111] The present invention provides a device for particle filtering with inequality constraints and one-step advance measurement, comprising:

[0112] The constraint-assisted dynamic model module is used to fuse the inequality constraint information with the given dynamic model, move the center value of the state prediction distribution to the feasible domain, and obtain the constraint-assisted dynamic model;

[0113] The constrained state decomposition module uses the Bayesian formula to derive the state posterior density function decomposition structure under measurement and constraint conditions, including the state posterior distribution under the current moment constraint conditions and the current moment state distribution under the next moment constraint conditions;

[0114] The proposed distribution function module is used to select the given dynamic model and the constraint-assisted dynamic model as the proposed distribution function in turn under the current moment constraints, sample and retain particles that meet the constraints from the given dynamic model; for particles that do not meet the constraints, construct and calculate the corresponding constraint-assisted dynamic model, sample and retain particles that meet the constraints;

[0115] The state estimation module under the current constraints is used to construct a motion cost function to compensate for the impact of dynamic model correction on particle distribution. The particle weights are updated through measurement and cost function to obtain the posterior distribution of the state under the current constraints.

[0116] The particle update module under the next moment constraint is used to determine whether the center value of the predicted distribution of each constrained particle meets the next moment constraint under the next moment constraint. For center values ​​that exceed the feasible region, the corresponding particle constraint auxiliary dynamic model method is constructed to ensure that the particle trajectory meets the constraint information of two consecutive moments, and the particle weight is updated using the moving cost function method.

[0117] The output state estimation module is used to construct a dynamic model by fusing the constraints of the current moment and the constraints of the next moment, obtain the state posterior density function of the constraint one step ahead measurement, and output the state estimation result.

[0118] A preferred embodiment provided by the present invention is as follows: Figure 2 As shown,

[0119] Step 1. Consider the scenario of a ship sailing in a canal, the target state vector Among them, (ξ k ,η k ) is the current location of the target, is the current moving speed of the target; the constraint condition is the ship channel.

[0120] Step 2. Give a dynamic random system model. According to the ship's motion characteristics, a typical uniform motion model is selected as the dynamic model, that is,

[0121] xk+1 =Fx k +w k ,w k ~N(0,Q k );

[0122] in, a ξ =50, a η =100, are the process noise intensities in the ξ and η directions respectively, and the sampling period T = 1 second.

[0123] Radar measurement model:

[0124]

[0125] Measurement noise variance R k =diag(10,10×10 -5 ).

[0126] Step 3. Initialization. Draw a sample set from the known probability distribution of q(x0) And the weights are set to equal weights

[0127] Step 4. k = 1, 2, 3..., take the given dynamic model as the proposed distribution, and Sampling in sequence;

[0128] Step 5. If Then the sampled particles satisfy the constraints; if Then calculate the mean of the predicted distribution at time k, that is

[0129] Step 6. Combine the constraints and the given dynamic model to propose a constraint-assisted dynamic model, and construct an optimization function to solve the predicted distribution center value that satisfies the constraints. Right now if Then, we can solve the above formula otherwise

[0130] Step 7. For particles that do not meet the constraints, the constraint-assisted dynamic model is used as the proposed distribution function, and then Sampling is performed to obtain particles

[0131] Step 8. Update the particle weight based on the movement cost function and measurement under the current moment constraints to obtain:

[0132] Step 9. Select particles that satisfy the constraints, i.e. Calculate the center value of its predicted distribution

[0133] Step 10. Construct optimization function if Solve the optimization function; otherwise,

[0134] Step 11. The cost function updates the particle weight at the next moment

[0135]

[0136] Step 12. Solve the posterior density function

[0137]

[0138] Among them, the normalized weight And output the estimated results

[0139] Step 13. Resampling. Obtain particle set through resampling strategy Recursively derive the posterior density function of the state at time k+1.

[0140] like Figure 3 As shown in the figure, the i-th particle is at the center of the predicted distribution of k and k+1. It can be seen that after moving the center of the predicted distribution, the probability of extracting constrained particles is increased, and the state evolution at continuous moments is more consistent with the actual situation.

[0141] The particle filtering method used in this application provides a new approach for constrained filters, rationally utilizing current and future constraints to improve filtering accuracy with minimal increase in computational complexity. One advantage of this application is that it effectively utilizes constraint information to correct a given dynamic model, making the state evolution law closer to actual physical conditions and greatly improving the accuracy of dynamic modeling. Another advantage is that it designs a cost function to measure the degree of inconsistency between the unconstrained auxiliary dynamic model and the constraints, and then uses the current and next moment constraints to update the particle weights and improve filtering accuracy.

[0142] like Figure 4 As shown in the figure, a simulation scenario of a ship navigating a canal is presented. The proposed solution is compared with several existing constrained particle filters, including the Constrained Assisted Particle Filter (CAPFA) and the Forward Filtering Backward Smoothing Constrained Particle Filter (FFBS). In addition, a classic unconstrained particle filter (SPF) is also selected for comparison. The root mean square error (RMSE) is used as a metric for filtering performance. 100 Monte Carlo simulations are conducted using 200 particles.

[0143] The simulation results are as follows Figure 5 and Figure 6As shown in the figure, it is clear that the PFCA method successfully incorporates constraints into the particle filter algorithm, significantly outperforming the unconstrained SPF. Compared to the CAPFA method, PFCA also exhibits certain performance advantages. This is due to the method's effective integration of current and next-time constraint information, rather than solely considering current-time constraints. PFCA outperforms FFBS because FFBS treats constraints as measurement information for smoothed estimation. Although it incorporates future-time constraint information, it does not modify the dynamic model, resulting in fewer valid samples and reduced estimation accuracy.

[0144] In addition, the methods described in this invention also compared computational complexity. As shown in Table 1, the unconstrained SPF method has the lowest computational complexity because it does not require processing of additional information. FFBS has the highest computational complexity, making it unsuitable for applications requiring high real-time performance. The filtering performance employed in this application outperforms CAPFA, demonstrating that this invention effectively improves filtering performance while also being highly applicable.

[0145] Table 1

[0146] filter SPF CAPFA FFBS PFAC Time (seconds) 0.0020 0.0115 0.1286 0.0131

[0147] In another embodiment of the present invention, a computer device is provided, comprising a processor and a memory, wherein the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or may be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA), or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions in the computer storage medium to implement the corresponding method flow or corresponding function; the processor described in the embodiment of the present invention can be used for the operation of a particle filtering method for one-step advance measurement with inequality constraints.

[0148] In another embodiment of the present invention, a storage medium is provided, specifically a computer-readable storage medium (Memory). The computer-readable storage medium is a memory device in a computer device, used to store programs and data. It is understood that the computer-readable storage medium herein may include both built-in storage media in the computer device and, of course, extended storage media supported by the computer device. The computer-readable storage medium provides storage space, which stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for being loaded and executed by a processor. These instructions may be one or more computer programs (including program code). It should be noted that the computer-readable storage medium herein may be a high-speed RAM memory or a non-volatile memory, such as at least one disk storage device. The processor may load and execute the one or more instructions stored in the computer-readable storage medium to implement the corresponding steps of the particle filtering method for one-step-ahead measurement with inequality constraints in the above-mentioned embodiment.

[0149] It will be understood by those skilled in the art that embodiments of the present invention may be provided as methods, systems, or computer program products. Thus, the present invention may take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware. Furthermore, the present invention may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to magnetic disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0150] The present invention is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present invention. It should be understood that each process and / or block in the flowcharts and / or block diagrams, as well as combinations of processes and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowcharts and / or block diagrams. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0151] These computer program instructions may 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 an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0152] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0153] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A particle filter method with one-step-ahead measurement constraint, characterized in that: The following steps are involved: By integrating the inequality constraint information with the given dynamic model, the center value of the state prediction distribution is moved to the feasible region to obtain a constraint-assisted dynamic model. The Bayesian formula is used to derive the decomposition structure of the state posterior density function under measurement and constraint conditions, including the state posterior distribution under the current moment constraint conditions and the current moment state distribution under the next moment constraint conditions; Under the current constraints, the given dynamic model and the constraint-assisted dynamic model are selected as the proposed distribution function one by one, and particles that meet the constraints are sampled and retained from the given dynamic model; for particles that do not meet the constraints, the corresponding constraint-assisted dynamic model is constructed and calculated, and particles that meet the constraints are sampled and retained; The method of constructing the moving cost function compensates for the influence of the dynamic model correction on the particle distribution, calculates the measurement likelihood function and the cost function to update the particle weight, and obtains the state posterior distribution under the current time constraint conditions; Under the constraints of the next moment, it is determined whether the center value of the predicted distribution of each constrained particle meets the constraints of the next moment. For the center value that exceeds the feasible region, the corresponding particle constraint auxiliary dynamic model method is constructed to ensure that the particle trajectory meets the constraint information of two consecutive moments, and the moving cost function method is used to update the current particle weight. By fusing the constraints of the current moment and the constraints of the next moment to construct a dynamic model, the state posterior density function of the constraint one-step ahead measurement is obtained, and the state estimation result is output.

2. The particle filtering method with one-step-ahead measurement constraint according to claim 1, characterized in that: The constraint-assisted dynamic model is obtained by constraining and correcting a given dynamic model, and under the current constraint conditions, includes the following steps: The likelihood function based on the constraint is p(C k |x k ): If the state x k Satisfy constraint C k When , the likelihood function value is 1; otherwise, it is 0; The dynamic equation is x k =f(x k-1 )+w k , w k is process noise, with mean 0 and covariance Q k The Gaussian distribution of the probability density function is expressed as p(x k |x k-1 ); The predicted state set is usually clustered around the center value of the predicted distribution. The predicted center value is moved to the constraint range by constructing the moving criterion function. The center value of the unconstrained state predicted distribution is The center value that satisfies the constraint is u obtained by solving the moving criterion function k ,like If the constraints are satisfied, Otherwise k Solve the optimization function using constrained optimization methods: It is used to find a point with the largest prediction probability density in the feasible region as the new prediction center value, effectively integrating the given dynamic model information and constraints; The obtained constraint-assisted dynamic model is x k =u k +w k .

3. The particle filtering method with one-step-ahead measurement constraint according to claim 2, characterized in that: The decomposition structure of the state posterior density function is derived using the Bayesian formula under measurement and constraint conditions, including the following steps: Measurement equation y k =h(x k )+v k , its probability density function expression is p(y k |x k ), using the Bayesian formula to decompose the state posterior density function under measurement and constraint conditions, using the known measurement sequence y 1:k and constraint sequence C 1:k+1 , calculate the state posterior density function: Among them, p(x k |y 1:k ,C 1:k ) is the state posterior density function under the current constraint, p(C k+1 |x k ) is the state x k The predicted state satisfies the likelihood function of the next moment constraint.

4. The particle filtering method with one-step-ahead measurement constraint according to claim 3, characterized in that: The particle filter algorithm that uses the given dynamic model and the constraint-assisted dynamic model as the proposed distribution function for particle sampling includes the following steps: The weighted particle set at the previous moment is is the i-th particle, N is the number of particles, is the normalized weight of the ith particle and The sum of the particle weights is 1, that is According to the particle weight definition, calculate the particles The non-normalized weights of: It is known that the typical bootstrap particle filter takes the dynamic model as the proposed distribution. On this basis, the present invention incorporates the constraints into the proposed distribution, and the proposed distribution function q(x k |x k-1 ,y k ,C k )=p(x k |x k-1 ,C k ), using the Bayesian formula to derive, the recommended distribution function of the i-th particle is Will Sampling is performed as a proposed distribution function, and particles that meet the constraints are retained. For particles that do not meet the constraints, the constraint-assisted dynamic model of the i-th particle is obtained by constructing and solving the movement criterion function, which is used as the proposed distribution. The modified dynamic model is x k =u k +w k , sampled from this dynamic model, i.e. Particles that satisfy the constraints are retained.

5. The particle filtering method with one-step-ahead measurement constraint according to claim 4, characterized in that: Calculate the cost function and measurement likelihood function to update the particle weights. The steps are as follows: For a given dynamic model as a constraint particle obtained from the proposed distribution, the non-normalized weight of the i-th particle is calculated as: For the constrained particles obtained as the proposed distribution by the constraint-assisted dynamic model, the non-normalized weight of the i-th particle is calculated as: in, is the cost of the mobile dynamic model, which is used to measure the degree of inconsistency between the given dynamic evolution model and the constraints. The greater the inconsistency, the lower the importance of the particle. The state posterior density function based on the current constraint conditions is: Here, δ(·) is the Dirac delta function.

6. The particle filtering method with one-step-ahead constraint according to claim 5, characterized in that: Under the next moment constraint, the process of updating the particle weight at the current moment includes the following steps: For constrained particles For example, the probability density function of the current state under the next moment constraint is: Modify the i-th dynamic model to obtain: Calculate the movement cost function Used to measure the inconsistency between particle evolution model and constraints.

7. The particle filtering method with one-step-ahead measurement constraint according to claim 6, characterized in that: The state estimation under the constraints of the current and next moments includes the following steps: Among them, the normalized weight and Calculate state estimation results 8. A device for constraining a particle filter with a one-step-ahead measurement, characterized in that A particle filtering method based on any one of claims 1 to 7 constrained one-step-ahead measurement, comprising: The constraint-assisted dynamic model module is used to fuse the inequality constraint information with the given dynamic model, move the center value of the state prediction distribution to the feasible domain, and obtain the constraint-assisted dynamic model; The constrained state decomposition module uses the Bayesian formula to derive the state posterior density function decomposition structure under measurement and constraint conditions, including the state posterior distribution under the current moment constraint conditions and the current moment state distribution under the next moment constraint conditions; The proposed distribution function module is used to select the given dynamic model and the constraint-assisted dynamic model as the proposed distribution function in turn under the current moment constraints, sample and retain particles that meet the constraints from the given dynamic model; for particles that do not meet the constraints, construct and calculate the corresponding constraint-assisted dynamic model, sample and retain particles that meet the constraints; The state estimation module under the current constraints is used to construct a method of moving cost function to compensate for the impact of dynamic model correction on particle distribution. The particle weights are updated through measurement and cost function to obtain the posterior distribution of the state under the current constraints. The particle update module under the next moment constraint is used to determine whether the center value of the predicted distribution of each constrained particle meets the next moment constraint under the next moment constraint. For center values ​​that exceed the feasible region, the corresponding particle constraint auxiliary dynamic model method is constructed to ensure that the particle trajectory meets the constraint information of two consecutive moments, and the particle weight is updated using the moving cost function method. The output state estimation module is used to construct a dynamic model by fusing the constraints of the current moment and the constraints of the next moment, obtain the state posterior density function of the constraint one step ahead measurement, and output the state estimation result.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein: When the processor executes the computer program, the steps of the particle filtering method with constrained one-step-ahead measurement as claimed in any one of claims 1 to 7 are implemented.

10. A computer-readable storage medium storing a computer program, characterized in that: When the computer program is executed by a processor, the steps of the particle filtering method with constrained one-step-ahead measurement as claimed in any one of claims 1 to 7 are implemented.

Images