CN113422593B - Filtering method, filter, computer-readable storage medium, processor, and FPGA - Google Patents
Filtering method, filter, computer-readable storage medium, processor, and FPGA Download PDFInfo
- Publication number
- CN113422593B CN113422593B CN202110759746.0A CN202110759746A CN113422593B CN 113422593 B CN113422593 B CN 113422593B CN 202110759746 A CN202110759746 A CN 202110759746A CN 113422593 B CN113422593 B CN 113422593B
- Authority
- CN
- China
- Prior art keywords
- estimated value
- state
- covariance
- error
- value
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Classifications
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
- H03H17/0248—Filters characterised by a particular frequency response or filtering method
- H03H17/0255—Filters based on statistics
- H03H17/0257—KALMAN filters
-
- H—ELECTRICITY
- H03—ELECTRONIC CIRCUITRY
- H03H—IMPEDANCE NETWORKS, e.g. RESONANT CIRCUITS; RESONATORS
- H03H17/00—Networks using digital techniques
- H03H17/02—Frequency selective networks
- H03H17/0223—Computation saving measures; Accelerating measures
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Computer Hardware Design (AREA)
- Mathematical Physics (AREA)
- Probability & Statistics with Applications (AREA)
- Computing Systems (AREA)
- Theoretical Computer Science (AREA)
- Navigation (AREA)
- Feedback Control In General (AREA)
Abstract
The application provides a filtering method, a filter, a computer readable storage medium, a processor and an FPGA, wherein the filtering method comprises the following steps: step S101, obtaining a measured value of a system state parameter at the current moment; step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; step S103, covariance of prior error is obtained through calculation according to covariance of posterior error at the previous moment; step S104, calculating Kalman gain according to covariance of prior error, wherein the Kalman gain is a weighting matrix of residual error at the current moment, and the residual error at the current moment is the residual error of the measured value and the first estimated value; step S105, correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value; step S106, the covariance of the posterior error at the current moment is obtained through calculation according to the covariance of the prior error and the Kalman gain, step S102 and step S103 are synchronously executed, and step S105 and step S106 are synchronously executed.
Description
Technical Field
The application relates to the technical field of filtering, in particular to a filtering method, a filter, a computer readable storage medium, a processor and an FPGA.
Background
In engineering practice, a general processor is often adopted in the past to implement the kalman filtering algorithm, such as a singlechip, a DSP processor or an ASIC, but the above methods have great limitations. If the Kalman filter is realized by adopting the DSP processor, the realization is simple, the flexibility is good, but the operation speed is slower, and the requirements are difficult to meet in the occasion with higher real-time requirements. In order to meet the requirement of system instantaneity, the Kalman filtering is realized by using the ASIC, and the ASIC is adopted to realize the purposes of high speed, high reliability, poor universality, incapability of modifying after the design is finished and high development cost. Aiming at the defects, the characteristics of high parallelism and high execution speed of the FPGA (Field Programmable GATE ARRAY ) are fully utilized, and in recent years, extensive research on realizing a Kalman filtering algorithm in the FPGA is started.
The above information disclosed in the background section is only for enhancement of understanding of the background art from the technology described herein and, therefore, may contain some information that does not form the prior art that is already known in the country to a person of ordinary skill in the art.
Disclosure of Invention
The application mainly aims to provide a filtering method, a filter, a computer readable storage medium, a processor and an FPGA, so as to solve the problem of low efficiency of a Kalman filtering algorithm implementation method in the prior art.
According to an aspect of an embodiment of the present invention, there is provided a filtering method including: step S101, obtaining a measured value of a system state parameter at the current moment; step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; step S103, covariance of prior error is obtained through calculation according to covariance of posterior error at the previous moment; step S104, calculating Kalman gain according to covariance of the prior error, wherein the Kalman gain is a weighting matrix of a residual error at the current moment, and the residual error at the current moment is a residual error between the measured value and the first estimated value; step S105, correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value; and step S106, calculating the covariance of the posterior error at the current time according to the covariance of the prior error and the Kalman gain, wherein the step S102 and the step S103 are synchronously executed, and the step S105 and the step S106 are synchronously executed.
Optionally, calculating the first estimated value according to the estimated value of the system state at the previous moment includes: acquiring an estimated value and a control quantity of the system state at the previous moment; and calculating to obtain the first estimated value according to the estimated value of the system state at the previous moment and the control quantity of the system state at the previous moment.
Optionally, the covariance of the a priori error is calculated according to the covariance of the a priori error at the previous moment, and the method further includes: acquiring covariance of the posterior error at the previous moment; and calculating covariance of the prior error according to covariance of the posterior error at the previous moment.
Optionally, correcting the first estimated value according to the measured value and the kalman gain to obtain a second estimated value, including: calculating the current moment residual error according to the measured value and the first estimated value; and correcting the first estimated value according to the residual error at the current moment and the Kalman gain to obtain a second estimated value.
Optionally, the matrix calculation in steps S101 to S105 is performed by sequential phase multiplication, which is a method of sequentially inputting the matrix to the multiplier for calculation, or simultaneous phase multiplication, which is a method of simultaneously inputting the matrix to the multiplier for calculation.
Optionally, the filtering method controls the execution of steps S101 to S105 through a finite state machine, where the finite state machine includes four states, the finite state machine enters a first state, performs step S101, the finite state machine enters a second state, performs step S102, the finite state machine enters a third state, performs step S103, and the finite state machine enters a fourth state, and performs steps S104 and S105.
According to another aspect of an embodiment of the present invention, there is also provided a filter including: the acquisition unit is used for acquiring the measured value of the system state parameter at the current moment; the first calculation unit is used for calculating a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; the second calculation unit is used for calculating covariance of the prior error according to covariance of the posterior error at the previous moment; the third calculation unit is used for calculating a Kalman gain according to the covariance of the prior error, wherein the Kalman gain is a weighting matrix of a current moment residual error, and the current moment residual error is the residual error of the measured value and the first estimated value; the fourth calculation unit is used for correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value; and a fifth calculation unit, configured to calculate, according to the covariance of the prior error and the kalman gain, a covariance of the posterior error at the current time, and perform parallel calculation on the second estimated value and the covariance of the posterior error, where the first calculation unit and the first calculation unit operate synchronously, and the fourth calculation unit and the fifth calculation unit operate synchronously.
According to still another aspect of the embodiments of the present invention, there is also provided a computer-readable storage medium including a stored program, wherein the program performs any one of the methods.
According to yet another aspect of the embodiments of the present invention, there is further provided a processor, where the processor is configured to execute a program, where the program executes any one of the methods.
According to yet another aspect of the embodiment of the present invention, there is also provided an FPGA, including a filter, where the filter is configured to perform any one of the methods.
In the embodiment of the present invention, in the filtering method, first, step S101, a measured value of a system state parameter at a current moment is obtained; step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; step S103, calculating covariance of prior error according to covariance of posterior error of previous moment, and step S104, calculating Kalman gain according to covariance of prior error, wherein the Kalman gain is a weighting matrix of residual error of current moment, and the residual error of current moment is residual error of the measured value and the first estimated value; step S105, correcting the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; finally, step S106, calculating the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain, wherein step S102 and step S103 are synchronously executed, and step S105 and step S106 are synchronously executed. The step S102 and the step S103 of the filtering method are synchronously executed, and the step S105 and the step S106 are synchronously executed, namely, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the posterior error is synchronously calculated.
Drawings
The accompanying drawings, which are included to provide a further understanding of the application and are incorporated in and constitute a part of this specification, illustrate embodiments of the application and together with the description serve to explain the application. In the drawings:
FIG. 1 shows a flow chart of a filtering method according to an embodiment of the application;
FIG. 2 illustrates a logic diagram of a finite state machine according to an embodiment of the present application;
Fig. 3 shows a schematic diagram of a filter according to an embodiment of the application.
Detailed Description
It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the application. Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.
It is noted that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of exemplary embodiments according to the present application. As used herein, the singular is also intended to include the plural unless the context clearly indicates otherwise, and furthermore, it is to be understood that the terms "comprises" and/or "comprising" when used in this specification are taken to specify the presence of stated features, steps, operations, devices, components, and/or combinations thereof.
It will be understood that when an element such as a layer, film, region, or substrate is referred to as being "on" another element, it can be directly on the other element or intervening elements may also be present. Furthermore, in the description and in the claims, when an element is described as being "connected" to another element, the element may be "directly connected" to the other element or "connected" to the other element through a third element.
As described in the background, the prior art kalman filter algorithm is inefficient to implement, and in order to solve the above problem, in an exemplary embodiment of the present application, a filtering method, a filter, a computer readable storage medium, a processor, and an FPGA are provided.
According to an embodiment of the present application, there is provided a filtering method.
Fig. 1 is a flowchart of a filtering method according to an embodiment of the present application. As shown in fig. 1, the method comprises the steps of:
step S101, obtaining a measured value of a system state parameter at the current moment;
step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment;
step S103, covariance of prior error is obtained through calculation according to covariance of posterior error at the previous moment;
Step S104, calculating a Kalman gain according to the covariance of the prior error, wherein the Kalman gain is a weighting matrix of a residual error at the current moment, and the residual error at the current moment is the residual error of the measured value and the first estimated value;
Step S105, correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value;
step S106, according to the covariance of the prior error and the Kalman gain, calculating to obtain the covariance of the posterior error at the current time,
Wherein, the step S102 and the step S103 are synchronously executed, and the step S105 and the step S106 are synchronously executed.
In the filtering method, firstly, step S101, obtaining a measured value of a system state parameter at the current moment; step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; step S103, calculating covariance of prior error according to covariance of posterior error of previous moment, and step S104, calculating Kalman gain according to covariance of prior error, wherein the Kalman gain is a weighting matrix of residual error of current moment, and the residual error of current moment is residual error of the measured value and the first estimated value; step S105, correcting the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; finally, step S106, calculating the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain, wherein step S102 and step S103 are synchronously executed, and step S105 and step S106 are synchronously executed. The step S102 and the step S103 of the filtering method are synchronously executed, and the step S105 and the step S106 are synchronously executed, namely, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the posterior error is synchronously calculated.
It should be noted that the steps illustrated in the flowcharts of the figures may be performed in a computer system such as a set of computer executable instructions, and that although a logical order is illustrated in the flowcharts, in some cases the steps illustrated or described may be performed in an order other than that illustrated herein.
In one embodiment of the present application, calculating the first estimated value according to the estimated value of the system state at the previous time includes: acquiring an estimated value and a control quantity of the system state at the previous moment; the first estimated value is calculated based on the estimated value of the system state at the previous time and the control amount of the system state at the previous time. Specifically, the calculation formula of the first estimated value is thatWherein/>For the first estimate,/>For the estimated value of the system state at the previous time, u k-1 is the control quantity of the system state at the previous time, A is a state transition matrix, B is a matrix for converting the input into the state, and both A and B can be obtained according to the monitoring data of the system state, for example, the system state is the state of the moving object, the state of the moving object at the k-1 time is known, and the calculation formula of the state of the moving object at the k time is/>Wherein p k and p k-1 are the position states of the moving object at the k moment and the k-1 moment respectively, v k and v k-1 are the speed states of the moving object at the k moment and the k-1 moment respectively, Δk is the time difference between the k moment and the k-1 moment, and u k is the acceleration of the moving object at the k moment.
In one embodiment of the present application, the covariance of the prior error is obtained by calculating the covariance of the posterior error at the previous moment, and the method further includes: acquiring covariance of the posterior error at the previous moment; and calculating covariance of the prior error according to covariance of the posterior error at the previous moment. Specifically, the above-mentioned prior error covariance is calculated by the formulaWherein/>The covariance of the prior error at the current moment, the covariance of the posterior error at the previous moment of P k-1, and Q is the covariance of the process noise.
In one embodiment of the present application, correcting the first estimated value according to the measured value and the kalman gain to obtain a second estimated value includes: calculating the current moment residual error according to the measured value and the first estimated value; and correcting the first estimated value according to the residual error at the current moment and the Kalman gain to obtain a second estimated value. Specifically, the calculation formula of the Kalman gain is as followsWherein K k is Kalman gain, R is covariance of measurement noise, H is a conversion matrix from state variable to observation, and represents a relation connecting state and observation, and the calculation formula of the second estimated value is/>Wherein/>For the second estimate, z k is a measure of the system state,/>And the residual is the current moment.
In one embodiment of the present application, the matrix calculation in the step S101 to the step S105 is performed by sequential phase multiplication, which is a method of sequentially inputting the matrix to the multiplier for calculation, or simultaneous phase multiplication, which is a method of simultaneously inputting the matrix to the multiplier for calculation. Specifically, the calculation matrices A, B and C are multiplied by taking as an example, the matrices a, B, and C are stored in ROM1, ROM2, and ROM3, respectively, if the two matrices are multiplied sequentially, first, before the two matrices are multiplied, the multiplexer selects the switch MUX to gate the ROM1 and the ROM2, and sequentially reads out the data, first, the first two matrices are multiplied, the result is stored in ROMTEMP, then, the MUM gates ROMTEMP and the ROM3 are combined with the same resource to complete the three matrix multiplications, if the same resource is used in the same time, the output data of the ROM1, the ROM2, and the ROM3, the MUM inputs the multiplier and the adder are configured according to the solution requirement, all the processes are performed simultaneously, and the sequential multiplication method requires more time, and the same time multiplication will use more resource.
In one embodiment of the present application, the covariance of the posterior error at the current time is calculated according to the covariance of the prior error and the kalman gain, specifically, a calculation formula of the covariance of the posterior error at the current time isWherein I is an identity matrix.
In an embodiment of the present application, the filtering method controls the execution of steps S101 to S105 through a finite state machine, where the finite state machine includes four states, the finite state machine enters a first state, step S101 is executed, the finite state machine enters a second state, step S102 is executed, the finite state machine enters a third state, step S103 is executed, the finite state machine enters a fourth state, and steps S104 and S105 are executed. Specifically, using a finite state machine to control the transfer of each step, the steps can be roughly divided into four states according to the logical relationship between each step: a first state S0, a second state S1, a third state S2 and a fourth state S3, wherein the S0 is initialized, and a first estimated value is calculated when the S1 state is enteredCovariance/>, of prior error at current momentThen enter S2 state, calculate Kalman gain K k, finally enter S3 state, calculate the second estimated value/>And covariance P k of the posterior error at the current time.
It should be noted that, as shown in fig. 2, the state of calculating the K value is not set by the flag signal enable and the finish, and the initial values of the enable and the finish are both 0, when the state of S2 is entered, the state of calculating the K value is started when the enable is set to 1, after the calculation of the K value is completed, the finish is set to 1, the state of S3 is entered, the initial values of the enable and the finish are reset to 0, the next state is ready for interaction, and the next state machine is entered to continue the following calculation, and finally the filtering calculation is completed, wherein the K value can be the value to be calculated in any state machine state.
The embodiment of the application also provides a filter, and the filter of the embodiment of the application can be used for executing the filtering method provided by the embodiment of the application. The filter provided by the embodiment of the application is described below.
Fig. 3 is a schematic diagram of a filter according to an embodiment of the application. As shown in fig. 3, the apparatus includes:
an acquiring unit 10, configured to acquire a measured value of a system state parameter at a current time;
a first calculating unit 20, configured to calculate a first estimated value according to the estimated value of the system state at the previous time, where the first estimated value is an estimated value of the system state at the current time;
A second calculating unit 30, configured to calculate a covariance of the prior error according to the covariance of the posterior error at the previous moment;
a third calculation unit 40, configured to calculate a kalman gain according to the covariance of the prior error, where the kalman gain is a weighting matrix of a current time residual, and the current time residual is a residual between the measured value and the first estimated value;
A fourth calculation unit 50, configured to correct the first estimated value according to the measured value and the kalman gain, so as to obtain a second estimated value;
A fifth calculation unit 60 for obtaining a covariance of the posterior error at the current time based on the covariance of the prior error and the kalman gain calculation, performing parallel calculation of the second estimated value and the covariance of the posterior error,
Wherein the first computing unit and the first computing unit operate synchronously, and the fourth computing unit and the fifth computing unit operate synchronously.
In the above filter, the acquiring unit executes step S101 to acquire a measured value of a system state parameter at the current moment; step S102 is executed by the first calculating unit, and a first estimated value is calculated according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; the second calculating unit performs step S103 to calculate the covariance of the prior error according to the covariance of the posterior error at the previous time, the third calculating unit performs step S104 to calculate the kalman gain according to the covariance of the prior error, the kalman gain is the weighting matrix of the residual error at the current time, and the residual error at the current time is the residual error between the measured value and the first estimated value; the fourth calculation unit executes step S105 to correct the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; and a fifth calculation unit executing step S106, wherein the first calculation unit and the second calculation unit execute synchronous execution, and the fourth calculation unit and the fifth calculation unit execute synchronous execution, so as to obtain the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain. The step S102 and the step S103 of the filter are synchronously executed, and the step S105 and the step S106 are synchronously executed, that is, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the prior error is synchronously calculated.
In one embodiment of the present application, the first calculating unit includes a first obtaining module and a first calculating module, where the first obtaining module is configured to obtain an estimated value and a control amount of the system state at a previous time; the first calculation module is configured to calculate the first estimated value according to an estimated value of the system state at a previous time and a control amount of the system state at a previous time. Specifically, the calculation formula of the first estimated value is thatWherein/>For the first estimate,/>For the estimated value of the system state at the previous time, u k-1 is the control quantity of the system state at the previous time, A is a state transition matrix, B is a matrix for converting the input into the state, and both A and B can be obtained according to the monitoring data of the system state, for example, the system state is the state of the moving object, the state of the moving object at the k-1 time is known, and the calculation formula of the state of the moving object at the k time is/>Wherein p k and p k-1 are the position states of the moving object at the k moment and the k-1 moment respectively, v k and v k-1 are the speed states of the moving object at the k moment and the k-1 moment respectively, Δk is the time difference between the k moment and the k-1 moment, and u k is the acceleration of the moving object at the k moment.
In an embodiment of the present application, the second calculating unit includes a second obtaining module and a second calculating module, where the second obtaining module is configured to obtain a covariance of the posterior error at a previous time; the second calculating module is configured to calculate a covariance of the prior error according to the covariance of the posterior error at a previous time. Specifically, the above-mentioned prior error covariance is calculated by the formulaWherein/>The covariance of the prior error at the current moment, the covariance of the posterior error at the previous moment of P k-1, and Q is the covariance of the process noise.
In an embodiment of the present application, the fourth calculating unit includes a third calculating module and a fourth calculating module, where the third calculating module is configured to calculate the current time residual according to the measured value and the first estimated value; the fourth calculation module is configured to correct the first estimated value according to the current time residual error and the kalman gain, so as to obtain a second estimated value. Specifically, the calculation formula of the Kalman gain is as followsWherein K k is Kalman gain, R is covariance of measurement noise, H is a conversion matrix from state variable to observation, and represents a relation connecting state and observation, and the calculation formula of the second estimated value is/>Wherein/>For the second estimate, z k is a measure of the system state,/>And the residual is the current moment.
In one embodiment of the present application, the matrix calculation in the step S101 to the step S105 is performed by sequential phase multiplication, which is a method of sequentially inputting the matrix to the multiplier for calculation, or simultaneous phase multiplication, which is a method of simultaneously inputting the matrix to the multiplier for calculation. Specifically, the calculation matrices A, B and C are multiplied by taking as an example, the matrices a, B, and C are stored in ROM1, ROM2, and ROM3, respectively, if the two matrices are multiplied sequentially, first, before the two matrices are multiplied, the multiplexer selects the switch MUX to gate the ROM1 and the ROM2, and sequentially reads out the data, first, the first two matrices are multiplied, the result is stored in ROMTEMP, then, the MUM gates ROMTEMP and the ROM3 are combined with the same resource to complete the three matrix multiplications, if the same resource is used in the same time, the output data of the ROM1, the ROM2, and the ROM3, the MUM inputs the multiplier and the adder are configured according to the solution requirement, all the processes are performed simultaneously, and the sequential multiplication method requires more time, and the same time multiplication will use more resource.
In one embodiment of the present application, the fifth calculation unit is configured to calculate the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain, and specifically, a calculation formula of the covariance of the posterior error at the current time isWherein I is an identity matrix.
In an embodiment of the present application, the filtering method controls the execution of steps S101 to S105 through a finite state machine, where the finite state machine includes four states, the finite state machine enters a first state, step S101 is executed, the finite state machine enters a second state, step S102 is executed, the finite state machine enters a third state, step S103 is executed, the finite state machine enters a fourth state, and steps S104 and S105 are executed. Specifically, using a finite state machine to control the transfer of each step, the steps can be roughly divided into four states according to the logical relationship between each step: a first state S0, a second state S1, a third state S2 and a fourth state S3, wherein the S0 is initialized, and a first estimated value is calculated when the S1 state is enteredCovariance/>, of prior error at current momentThen enter S2 state, calculate Kalman gain K k, finally enter S3 state, calculate the second estimated value/>And covariance P k of the posterior error at the current time.
It should be noted that, as shown in fig. 2, the state of calculating the K value is not set by the flag signal enable and the finish, and the initial values of the enable and the finish are both 0, when the state of S2 is entered, the state of calculating the K value is started when the enable is set to 1, after the calculation of the K value is completed, the finish is set to 1, the state of S3 is entered, the initial values of the enable and the finish are reset to 0, the next state is ready for interaction, and the next state machine is entered to continue the following calculation, and finally the filtering calculation is completed, wherein the K value can be the value to be calculated in any state machine state.
The embodiment of the application also provides an FPGA which comprises a filter, wherein the filter is used for executing any one of the methods.
The FPGA includes a filter, and the acquiring unit executes step S101 to acquire a measured value of a system state parameter at a current time; step S102 is executed by the first calculating unit, and a first estimated value is calculated according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; the second calculating unit performs step S103 to calculate the covariance of the prior error according to the covariance of the posterior error at the previous time, the third calculating unit performs step S104 to calculate the kalman gain according to the covariance of the prior error, the kalman gain is the weighting matrix of the residual error at the current time, and the residual error at the current time is the residual error between the measured value and the first estimated value; the fourth calculation unit executes step S105 to correct the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; and a fifth calculation unit executing step S106, wherein the first calculation unit and the second calculation unit execute synchronous execution, and the fourth calculation unit and the fifth calculation unit execute synchronous execution, so as to obtain the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain. The step S102 and the step S103 of the filter are synchronously executed, and the step S105 and the step S106 are synchronously executed, that is, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the prior error is synchronously calculated.
The filter includes a processor and a memory, the acquisition unit, the first calculation unit, the second calculation unit, the third calculation unit, the fourth calculation unit, the fifth calculation unit, and the like are stored in the memory as program units, and the processor executes the program units stored in the memory to realize corresponding functions.
The processor includes a kernel, and the kernel fetches the corresponding program unit from the memory. The kernel can be provided with one or more than one kernel, and the problem of low efficiency of the implementation method of the Kalman filtering algorithm in the prior art is solved by adjusting kernel parameters.
The memory may include volatile memory, random Access Memory (RAM), and/or nonvolatile memory, such as Read Only Memory (ROM) or flash memory (flash RAM), among other forms in computer readable media, the memory including at least one memory chip.
An embodiment of the present invention provides a computer-readable storage medium having stored thereon a program which, when executed by a processor, implements the above-described method.
The embodiment of the invention provides a processor, which is used for running a program, wherein the method is executed when the program runs.
The embodiment of the invention provides equipment, which comprises a processor, a memory and a program stored in the memory and capable of running on the processor, wherein the processor realizes at least the following steps when executing the program:
step S101, obtaining a measured value of a system state parameter at the current moment;
step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment;
step S103, covariance of prior error is obtained through calculation according to covariance of posterior error at the previous moment;
Step S104, calculating a Kalman gain according to the covariance of the prior error, wherein the Kalman gain is a weighting matrix of a residual error at the current moment, and the residual error at the current moment is the residual error of the measured value and the first estimated value;
Step S105, correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value;
step S106, according to the covariance of the prior error and the Kalman gain, calculating to obtain the covariance of the posterior error at the current time,
Wherein, the step S102 and the step S103 are synchronously executed, and the step S105 and the step S106 are synchronously executed.
The device herein may be a server, PC, PAD, cell phone, etc.
The application also provides a computer program product adapted to perform, when executed on a data processing device, a program initialized with at least the following method steps:
step S101, obtaining a measured value of a system state parameter at the current moment;
step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment;
step S103, covariance of prior error is obtained through calculation according to covariance of posterior error at the previous moment;
Step S104, calculating a Kalman gain according to the covariance of the prior error, wherein the Kalman gain is a weighting matrix of a residual error at the current moment, and the residual error at the current moment is the residual error of the measured value and the first estimated value;
Step S105, correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value;
step S106, according to the covariance of the prior error and the Kalman gain, calculating to obtain the covariance of the posterior error at the current time,
Wherein, the step S102 and the step S103 are synchronously executed, and the step S105 and the step S106 are synchronously executed.
In the foregoing embodiments of the present invention, the descriptions of the embodiments are emphasized, and for a portion of this disclosure that is not described in detail in this embodiment, reference is made to the related descriptions of other embodiments.
In the several embodiments provided in the present application, it should be understood that the disclosed technology may be implemented in other manners. The above-described embodiments of the apparatus are merely exemplary, and the division of the units may be a logic function division, and there may be another division manner when actually implemented, for example, a plurality of units or components may be combined or may be integrated into another system, or some features may be omitted or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be through some interfaces, units or modules, or may be in electrical or other forms.
The units described above as separate components may or may not be physically separate, and components shown as units may or may not be physical units, may be located in one place, or may be distributed over a plurality of units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.
In addition, each functional unit in the embodiments of the present invention may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.
The integrated units described above, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer-readable storage medium. Based on such understanding, the technical solution of the present invention may be embodied in essence or a part contributing to the prior art or all or part of the technical solution in the form of a software product stored in a computer readable storage medium, comprising several instructions for causing a computer device (which may be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the above-mentioned methods of the various embodiments of the present invention. And the aforementioned computer-readable storage medium includes: a usb disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a removable hard disk, a magnetic disk, or an optical disk, or other various media capable of storing program codes.
From the above description, it can be seen that the above embodiments of the present application achieve the following technical effects:
1) In the filtering method of the present application, firstly, step S101, obtaining a measured value of a system state parameter at the current moment; step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; step S103, calculating covariance of prior error according to covariance of posterior error of previous moment, and step S104, calculating Kalman gain according to covariance of prior error, wherein the Kalman gain is a weighting matrix of residual error of current moment, and the residual error of current moment is residual error of the measured value and the first estimated value; step S105, correcting the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; finally, step S106, calculating the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain, wherein step S102 and step S103 are synchronously executed, and step S105 and step S106 are synchronously executed. The step S102 and the step S103 of the filtering method are synchronously executed, and the step S105 and the step S106 are synchronously executed, namely, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the posterior error is synchronously calculated.
2) In the filter of the present application, the acquiring unit executes step S101 to acquire the measured value of the system state parameter at the current moment; step S102 is executed by the first calculating unit, and a first estimated value is calculated according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; the second calculating unit performs step S103 to calculate the covariance of the prior error according to the covariance of the posterior error at the previous time, the third calculating unit performs step S104 to calculate the kalman gain according to the covariance of the prior error, the kalman gain is the weighting matrix of the residual error at the current time, and the residual error at the current time is the residual error between the measured value and the first estimated value; the fourth calculation unit executes step S105 to correct the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; and a fifth calculation unit executing step S106, wherein the first calculation unit and the second calculation unit execute synchronous execution, and the fourth calculation unit and the fifth calculation unit execute synchronous execution, so as to obtain the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain. The step S102 and the step S103 of the filter are synchronously executed, and the step S105 and the step S106 are synchronously executed, that is, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the prior error is synchronously calculated.
3) The FPGA comprises a filter, an acquisition unit executes step S101 to acquire a measured value of a system state parameter at the current moment; step S102 is executed by the first calculating unit, and a first estimated value is calculated according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment; the second calculating unit performs step S103 to calculate the covariance of the prior error according to the covariance of the posterior error at the previous time, the third calculating unit performs step S104 to calculate the kalman gain according to the covariance of the prior error, the kalman gain is the weighting matrix of the residual error at the current time, and the residual error at the current time is the residual error between the measured value and the first estimated value; the fourth calculation unit executes step S105 to correct the first estimated value according to the measured value and the kalman gain to obtain a second estimated value; and a fifth calculation unit executing step S106, wherein the first calculation unit and the second calculation unit execute synchronous execution, and the fourth calculation unit and the fifth calculation unit execute synchronous execution, so as to obtain the covariance of the posterior error at the current time according to the covariance of the prior error and the kalman gain. The step S102 and the step S103 of the filter are synchronously executed, and the step S105 and the step S106 are synchronously executed, that is, the covariance of the first estimated value and the prior error is synchronously calculated, and the covariance of the second estimated value and the prior error is synchronously calculated.
The above description is only of the preferred embodiments of the present application and is not intended to limit the present application, but various modifications and variations can be made to the present application by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present application should be included in the protection scope of the present application.
Claims (7)
1. A method of filtering comprising:
step S101, obtaining a measured value of a system state parameter at the current moment;
step S102, calculating to obtain a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment;
step S103, covariance of prior error is obtained through calculation according to covariance of posterior error at the previous moment;
step S104, calculating Kalman gain according to covariance of the prior error, wherein the Kalman gain is a weighting matrix of a residual error at the current moment, and the residual error at the current moment is a residual error between the measured value and the first estimated value;
Step S105, correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value;
step S106, according to the covariance of the prior error and the Kalman gain, calculating to obtain the covariance of the posterior error at the current time,
Wherein the step S102 and the step S103 are performed synchronously, and the step S105 and the step S106 are performed synchronously; the matrix calculation process in the steps S101 to S105 is performed by using a sequential phase multiplication or a simultaneous multiplication method, where the sequential phase multiplication is a method of sequentially inputting the matrix into the multiplier to perform calculation, and the simultaneous phase multiplication is a method of simultaneously inputting the matrix into the multiplier to perform calculation; the filtering method controls execution of steps S101 to S105 through a finite state machine, the finite state machine includes four states, the finite state machine enters a first state, step S101 is executed, the finite state machine enters a second state, step S102 is executed, the finite state machine enters a third state, step S103 is executed, the finite state machine enters a fourth state, step S104 and step S105 are executed, the first estimated value is corrected according to the measured value and the kalman gain, and a second estimated value is obtained, and the method includes: calculating the current moment residual error according to the measured value and the first estimated value; and correcting the first estimated value according to the residual error at the current moment and the Kalman gain to obtain a second estimated value, wherein the state of calculating the K value is not set by a flag signal enable and a finish, the state of calculating the K value is not set by the flag signal enable and the finish, the initial values of the enable and the finish are all 0, when the state of S2 is entered, the state of calculating the K value is started when the enable is set to 1, after the calculation of the K value is completed, the finish is set to 1 to enter the S3 state, the initial values of the enable and the finish are reset to 0, the interaction of the next state is ready, the next state machine is entered, the following calculation is continued, and finally the filtering calculation is completed, wherein the K value is the value required to be calculated under any state machine state.
2. The method of claim 1, wherein calculating a first estimate from an estimate of the system state at a previous time instant comprises:
Acquiring an estimated value and a control quantity of the system state at the previous moment;
And calculating to obtain the first estimated value according to the estimated value of the system state at the previous moment and the control quantity of the system state at the previous moment.
3. The method of claim 1, wherein the covariance of the a priori error is calculated from covariance of a posterior error at a previous time, further comprising:
Acquiring covariance of the posterior error at the previous moment;
and calculating covariance of the prior error according to covariance of the posterior error at the previous moment.
4. A filter based on the filtering method of claim 1, comprising:
the acquisition unit is used for acquiring the measured value of the system state parameter at the current moment;
The first calculation unit is used for calculating a first estimated value according to the estimated value of the system state at the previous moment, wherein the first estimated value is the estimated value of the system state at the current moment;
The second calculation unit is used for calculating covariance of the prior error according to covariance of the posterior error at the previous moment;
the third calculation unit is used for calculating a Kalman gain according to the covariance of the prior error, wherein the Kalman gain is a weighting matrix of a current moment residual error, and the current moment residual error is the residual error of the measured value and the first estimated value;
the fourth calculation unit is used for correcting the first estimated value according to the measured value and the Kalman gain to obtain a second estimated value;
A fifth calculation unit for calculating the covariance of the posterior error at the current time according to the covariance of the prior error and the Kalman gain, wherein the second estimation value and the covariance of the posterior error are calculated in parallel,
The first computing unit and the first computing unit run synchronously, and the fourth computing unit and the fifth computing unit run synchronously; the matrix calculation process in the steps S101 to S105 is performed by using a sequential phase multiplication or a simultaneous multiplication method, where the sequential phase multiplication is a method of sequentially inputting the matrix into the multiplier to perform calculation, and the simultaneous phase multiplication is a method of simultaneously inputting the matrix into the multiplier to perform calculation; the filtering method comprises the steps of controlling execution of steps S101 to S105 through a finite state machine, wherein the finite state machine comprises four states, the finite state machine enters a first state, the step S101 is executed, the finite state machine enters a second state, the step S102 is executed, the finite state machine enters a third state, the step S103 is executed, the finite state machine enters a fourth state, the step S104 and the step S105 are executed, and the fourth computing unit comprises a third computing module and a fourth computing module, wherein the third computing module is used for computing the current time residual according to the measured value and the first estimated value; the fourth calculation module is configured to correct the first estimation value according to the residual error at the current time and the kalman gain to obtain a second estimation value, and not set a state of calculating a K value by using a flag signal enable and a flag signal finish, where initial values of the enable and the finish are both 0, when the state is entered into the S2 state, the enable is set to 1, the state of calculating the K value is started, after the calculation of the K value is completed, the finish is set to 1, the state is entered into the S3 state, initial values of the enable and the finish are reset to 0, interaction in the next state is ready, and the next calculation is continued by entering an original state machine, and finally the filtering calculation is completed, where the K value is a value to be calculated in any state of the state machine.
5. A computer readable storage medium, characterized in that the computer readable storage medium comprises a stored program, wherein the program performs the method of any one of claims 1 to 3.
6. A processor for running a program, wherein the program when run performs the method of any one of claims 1 to 3.
7. An FPGA comprising a filter, characterized in that the filter is adapted to perform the method of any of claims 1 to 3.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110759746.0A CN113422593B (en) | 2021-07-05 | 2021-07-05 | Filtering method, filter, computer-readable storage medium, processor, and FPGA |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110759746.0A CN113422593B (en) | 2021-07-05 | 2021-07-05 | Filtering method, filter, computer-readable storage medium, processor, and FPGA |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113422593A CN113422593A (en) | 2021-09-21 |
| CN113422593B true CN113422593B (en) | 2024-04-26 |
Family
ID=77721519
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110759746.0A Active CN113422593B (en) | 2021-07-05 | 2021-07-05 | Filtering method, filter, computer-readable storage medium, processor, and FPGA |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113422593B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114563045B (en) * | 2022-02-28 | 2023-06-27 | 南京天溯自动化控制系统有限公司 | Detection method and device for hospital logistics system, electronic equipment and medium |
| CN118227951A (en) * | 2024-03-14 | 2024-06-21 | 长沙理工大学 | A bridge structure health monitoring system and method based on Kalman filter circuit |
Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101777887A (en) * | 2010-01-08 | 2010-07-14 | 西安电子科技大学 | FPGA (Field Programmable Gata Array)-based unscented kalman filter system and parallel implementation method |
| CN103941195A (en) * | 2014-05-05 | 2014-07-23 | 山东大学 | Method for battery SOC estimation based on small model error criterion expanding Kalman filter |
| CN105929340A (en) * | 2016-06-30 | 2016-09-07 | 四川普力科技有限公司 | Method for estimating battery SOC based on ARIMA |
| CN106026919A (en) * | 2016-05-16 | 2016-10-12 | 南京理工大学 | Time-keeping compensation method for high-precision crystal oscillator |
| CN109376332A (en) * | 2018-10-30 | 2019-02-22 | 南京大学 | An Arbitrary Order Kalman Filter System |
| US10274318B1 (en) * | 2014-09-30 | 2019-04-30 | Amazon Technologies, Inc. | Nine-axis quaternion sensor fusion using modified kalman filter |
| CN109902568A (en) * | 2019-01-22 | 2019-06-18 | 长江大学 | A kind of adaptive Kalman filter algorithm |
| CN110174907A (en) * | 2019-04-02 | 2019-08-27 | 诺力智能装备股份有限公司 | A kind of human body target follower method based on adaptive Kalman filter |
| CN110602647A (en) * | 2019-09-11 | 2019-12-20 | 江南大学 | Indoor fusion positioning method based on extended Kalman filtering and particle filtering |
| CN111666137A (en) * | 2020-04-26 | 2020-09-15 | 广州文远知行科技有限公司 | Data annotation method and device, computer equipment and storage medium |
| CN112671373A (en) * | 2020-12-21 | 2021-04-16 | 北京科技大学 | Kalman filtering self-adaptive filtering algorithm based on error control |
| CN113054948A (en) * | 2020-11-27 | 2021-06-29 | 江苏亨通太赫兹技术有限公司 | Parallel Kalman filtering system and method |
Family Cites Families (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US8854594B2 (en) * | 2010-08-31 | 2014-10-07 | Cast Group Of Companies Inc. | System and method for tracking |
| US10054446B2 (en) * | 2015-11-17 | 2018-08-21 | Truemotion, Inc. | Methods and systems for combining sensor data to measure vehicle movement |
-
2021
- 2021-07-05 CN CN202110759746.0A patent/CN113422593B/en active Active
Patent Citations (12)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101777887A (en) * | 2010-01-08 | 2010-07-14 | 西安电子科技大学 | FPGA (Field Programmable Gata Array)-based unscented kalman filter system and parallel implementation method |
| CN103941195A (en) * | 2014-05-05 | 2014-07-23 | 山东大学 | Method for battery SOC estimation based on small model error criterion expanding Kalman filter |
| US10274318B1 (en) * | 2014-09-30 | 2019-04-30 | Amazon Technologies, Inc. | Nine-axis quaternion sensor fusion using modified kalman filter |
| CN106026919A (en) * | 2016-05-16 | 2016-10-12 | 南京理工大学 | Time-keeping compensation method for high-precision crystal oscillator |
| CN105929340A (en) * | 2016-06-30 | 2016-09-07 | 四川普力科技有限公司 | Method for estimating battery SOC based on ARIMA |
| CN109376332A (en) * | 2018-10-30 | 2019-02-22 | 南京大学 | An Arbitrary Order Kalman Filter System |
| CN109902568A (en) * | 2019-01-22 | 2019-06-18 | 长江大学 | A kind of adaptive Kalman filter algorithm |
| CN110174907A (en) * | 2019-04-02 | 2019-08-27 | 诺力智能装备股份有限公司 | A kind of human body target follower method based on adaptive Kalman filter |
| CN110602647A (en) * | 2019-09-11 | 2019-12-20 | 江南大学 | Indoor fusion positioning method based on extended Kalman filtering and particle filtering |
| CN111666137A (en) * | 2020-04-26 | 2020-09-15 | 广州文远知行科技有限公司 | Data annotation method and device, computer equipment and storage medium |
| CN113054948A (en) * | 2020-11-27 | 2021-06-29 | 江苏亨通太赫兹技术有限公司 | Parallel Kalman filtering system and method |
| CN112671373A (en) * | 2020-12-21 | 2021-04-16 | 北京科技大学 | Kalman filtering self-adaptive filtering algorithm based on error control |
Non-Patent Citations (3)
| Title |
|---|
| FPGA-Based Implementation of Kalman Filter for Real-Time Estimation of Tire Velocity and Acceleration;Fargham Sandhu等;《IEEE Sensors Journal》;20170901;第17卷(第17期);5749-5758 * |
| 基于FPGA的数字保护中卡尔曼滤波的实现;阮建国等;《电力系统保护与控制》;20091116;第37卷(第22期);121-125 * |
| 基于动态自适应卡尔曼滤波的空中目标跟踪;周琳;《实验室研究与探索》;20101115;第29卷(第11期);385-389 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113422593A (en) | 2021-09-21 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113422593B (en) | Filtering method, filter, computer-readable storage medium, processor, and FPGA | |
| JP6991983B2 (en) | How and systems to train machine learning systems | |
| WO2019057173A1 (en) | Sensor data processing method and apparatus | |
| WO2020119188A1 (en) | Program detection method, apparatus and device, and readable storage medium | |
| JP7247628B2 (en) | Creation device, creation system, creation method and creation program | |
| CN108733347B (en) | Data processing method and device | |
| CN111708931B (en) | Big data acquisition method based on mobile internet and artificial intelligence cloud service platform | |
| CN111641572A (en) | Noise power evaluation method and device and storage medium | |
| CN109213965B (en) | System capacity prediction method, computer readable storage medium and terminal device | |
| CN110018419B (en) | SOC and SOH joint estimation method and device of lithium ion battery and terminal equipment | |
| CN118377990A (en) | Post-processing method to improve the displacement solution accuracy of intrinsic hybrid finite element method | |
| CN113110982A (en) | Data access layer verification method and device, storage medium and electronic device | |
| US20200219239A1 (en) | System and method for performing a convolution operation | |
| CN113705795B (en) | Convolution processing method, convolution processing device, convolution neural network accelerator and storage medium | |
| Soares et al. | A configurable pruning Gaussian image filter for energy-efficient edge detection | |
| CN111860758B (en) | Deep learning model operation method and device, electronic equipment and medium | |
| CN108416164A (en) | Network on three-dimensional chip temperature rebuilding system based on finite population temperature sensor | |
| CN112819095A (en) | Feature point matching method and device, intelligent terminal and computer readable storage medium | |
| RU62469U1 (en) | ADAPTIVE WAVELET CONVERSION CALCULATION DEVICE | |
| CN106294143A (en) | The adjustment method of the depositor of chip and device | |
| CN105278923B (en) | A kind of signal sources number detection hardware circuit and its implementation based on AIC information criterions | |
| CN116361967B (en) | Three-dimensional model order reduction method and device for engineering machinery and electronic equipment | |
| CN107180014B (en) | A kind of quick sinc interpolation methods and system | |
| CN111614078A (en) | Method, device, equipment and storage medium for analyzing small interference stability of power system | |
| CN111340349A (en) | Product reliability evaluation method, apparatus, computer equipment and storage medium |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |