CN104009735B - Implementation method of the Kalman filter in 300 series of PLC of S7 - Google Patents

Abstract

The invention discloses a realization method and specific realization steps of a Kalman filter in an S7-300 series PLC. In the S7-300 series PLC system, 16 data blocks are defined, 14 of which correspond to each matrix variable in the Kalman filter iterative formula, and the remaining two data blocks are used for temporary storage of matrix operations. The access operation of several variables in the Kalman filter iterative formula is transformed into the access operation of variables in the corresponding data block of the S7‑300 series PLC system. Using the command system of the PLC system, directly realize matrix addition and subtraction operations, multiplication operations, inverse operations and Kalman filter iterative calculations. The calculation time of the Kalman filter is relatively fixed, and each sampling period in practical applications is mostly a single iteration. By setting the alarm timer of the PLC system reasonably, the expanded Kalman filter function will not affect the normal scanning of the PLC system. functions and provide new means for its advanced applications.

Description

Realization method of Kalman filter in S7-300 series PLC

technical field

The invention belongs to the technical field of automation and relates to a realization method of a Kalman filter in an S7-300 series PLC.

Background technique

Programmable logic controller (PLC), industrial control microcomputer (mainly IPC), distributed control system (DCS)/field bus control system (FCS) and intelligent control instrument are the four major types of control devices commonly used in the field of industrial automation. Among them, PLC is most widely used in industrial automation engineering due to its high reliability and ease of use. Large and medium-sized PLCs are mainly used for complex production line control, sequence control, batch control and process industry control. Among many large and medium-sized PLC systems, Siemens' S7-300 or S7-400 series PLC has become one of the large and medium-sized PLC systems widely used in my country by virtue of its good architecture, network functions, integrated technology and technical support.

In 1960, Kalman proposed a filtering and forecasting algorithm, the Kalman filter, which provides an efficient and computable method to estimate the state of the process and minimize the mean square error of the estimated value. It has been widely used in robot navigation, process control, data fusion, radar system, missile tracking, etc., to solve problems such as parameter estimation, state prediction and industrial process fault diagnosis.

The currently widely used PLC system provides programming language and command system according to the IEC61131-3 international standard, and each PLC system manufacturer has not yet provided the Kalman filter module or command library. The Kalman filter involves complex matrix operations, and today's research or application mostly uses the PC-based Matlab platform or C language platform. In the existing technology, observation data must be obtained from PLC system and other control equipment first, and then the complex calculation of the Kalman filter is completed through the Matlab platform of the PC, and finally the calculation result is formed into a control quantity and then sent back to the PLC, which is realized by the PLC system Control of industrial processes. The existing technology needs to repeatedly obtain data from the PLC system and transmit optimization information to the PLC system. It needs to rely on the communication network and the PC platform to cooperate to complete the state estimation or optimal control in the advanced control system.

The prior art solution needs to exchange data repeatedly between the PC system and the PLC system, and needs to be equipped with a PC, a Windows platform, a Matlab platform and a communication network. Network faults, especially inherent network delays, will affect the application effect of the Kalman filter. In addition, the high requirements of the industrial environment also pose challenges to the operating environment of the PC platform, and the increased PC platform will also lead to an increase in the cost of the system.

Therefore, how to directly realize the Kalman filter in the PLC system is the goal that the automation engineering circles have been pursuing.

Contents of the invention

The invention provides a method for realizing the Kalman filter in the S7-300 series PLC, and solves the problem that the method for realizing the Kalman filter in the PLC is complicated at present.

With the help of data blocks in Siemens S7-300 series PLC, various accesses and operations between Kalman filter matrices are converted into accesses and operations of each element between data blocks, using Siemens S7-300 series PLC The instruction system realizes the addition, subtraction, multiplication and inverse operation of the matrix, and then implements the Kalman filter in the PLC step by step according to the iterative formula of the Kalman filter and the module programming method provided by the S7-300 series PLC.

The technical solution adopted by the present invention is to convert the access operations of several matrix variables in the Kalman filter iterative formula into the access operations of the variables in the corresponding data blocks of the S7-300 series PLC system, and realize the addition and retrieval of the matrix in the PLC system. Subtraction, multiplication and inverse operations.

Further, the steps for implementing the Kalman filter in the S7-300 series PLC are:

Step 1: define data block DB1～DB14, store the variable in the Kalman filter formula; DB1～DB6 respectively store the matrix variable X(k), Φ(k), Η(k), y(k), ω(k) and v(k), the data block DB7 stores the state estimation value vector DB8 stores state one-step predicted value vector DB9 stores the filter gain matrix K(k), DB10 stores the covariance matrix P(k-1,k-1) of the estimated error, DB11 stores the one-step predicted value P(k,k-1) of the covariance matrix, and DB12 stores Q , DB13 stores R, and DB14 stores the identity matrix I;

Step 2: Calculate the one-step predicted value of the Kalman filter, which is taken from the data blocks DB7 and DB2 respectively and Φ(k), calculate the one-step predicted value of the Kalman filter as follows

Will Stored in data block DB8;

Step 3: Calculate the estimated value of the state and take it out from the data blocks DB8, DB9, DB3, and DB4 respectively K(k), H(k) and y(k), calculate the state estimation value according to the following formula

use Update DB7 in

Step 4: Calculate the filter gain matrix, take out P(k,k-1), H(k) and R from the data blocks DB11, DB3 and DB13 respectively, and calculate the filter gain matrix K(k) as follows:

K(k)=P(k,k-1) ^HT (k)[H(k)P(k,k-1) ^HT (k)+R] ^-1

Store K(k) in data block DB9;

Step 5: Calculate the one-step predicted value of the error covariance matrix, take out Φ(k), P(k-1,k-1) and Q from the data blocks DB2, DB10, and DB12 respectively, and calculate the error covariance matrix by the following formula One-step predictive value P(k,k-1):

P(k,k-1)=Φ(k)P(k-1,k-1) ^ΦT (k)+Q

Store P(k,k-1) in data block DB11;

Step 6: Calculate the covariance matrix of the estimation error, take out I, K(k), H(k) and P(k,k-1) from the data blocks DB14, DB9, DB3 and DB11 respectively, and calculate the estimation according to the following formula Error covariance matrix P(k,k):

P(k,k)=[I-K(k)H(k)]P(k,k-1)

Use P(k,k) to update P(k-1,k-1) in DB10, so far, an iterative operation of the Kalman filter is completed.

The beneficial effect of the invention is that the method is simple and the cost is low.

Description of drawings

Fig. 1 is a storage example diagram of two matrix elements for addition and subtraction in the PLC system;

Fig. 2 is the storage example figure of two matrix elements that PLC system carries out multiplication operation;

Fig. 3 is a matrix element access flow chart in the PLC system;

Fig. 4 is the flow chart of matrix addition and subtraction in the PLC system;

Fig. 5 is a flow chart of matrix multiplication in the PLC system;

Fig. 6 is the matrix determinant calculation flowchart in PLC system;

Fig. 7 is the accompanying matrix calculation flowchart in the PLC system;

Fig. 8 is the flow chart of matrix inverse operation in the PLC system;

Fig. 9 is a program block and a data block diagram used by the Kalman filter in the PLC system;

Fig. 10 is a Kalman filter in the PLC system to track the motion trajectory of the ball;

Figure 11 is a Kalman filter provided by Matlab to track the trajectory of the ball.

detailed description

The present invention will be described in detail below in conjunction with the accompanying drawings and specific embodiments.

The present invention first uses the data blocks in the S7-300 series PLC system to define the matrix, and to realize the iterative operation of the Kalman filter needs to define 14 matrices or vectors. Considering that the matrix operation needs to be temporarily stored, a total of 16 matrices are defined, corresponding to the PLC There are 16 data blocks in the system, and the data block numbers are DB1~DB16. Secondly, the addition, subtraction, multiplication and inverse operations of the matrix are converted into the four arithmetic operations of basic matrix elements, and the matrices involved in the operation are accessed by accessing the corresponding data block, and the matrix is respectively realized by using the floating-point operation instructions of the S7-300 series PLC addition, subtraction, multiplication and inverse operations. Finally, implement the iterative calculation of the Kalman filter according to the following six steps, in which the first step is performed only once, and the subsequent five steps are steps for each Kalman filter iterative calculation.

The implementation steps of the Kalman filter in the S7-300 series PLC system are as follows:

Step 1: Define data blocks DB1~DB14 to store the variables in the Kalman filter formula.

1. Use the data blocks in the PLC system to define the matrix:

The largest number of soft components in the S7-300 series PLC system are data blocks, the number of which can reach 1024, and the maximum capacity of each data block is 64KB. The S7-300 series PLC supports single-precision floating-point numbers (occupies 4 bytes, and the type is Real). If each data block corresponds to a matrix, the matrix can define a square matrix of 128×128 at most, which is enough to meet the needs of most practical engineering systems. need. After each data block corresponds to a matrix, the mutual operation between matrices is transformed into the operation between data blocks.

(1) The two matrices participating in the addition and subtraction operations have the same number of rows and columns, and the structures of the elements of the two matrices in the two data blocks are exactly the same, and they are all defined by row, as shown in Figure 1 ( Take a 3×3 dimensional matrix as an example). The address pointers of the elements ai _j and bi _j of the two m×n matrices are calculated according to the following formula:

ptr=[(i-1)n+(j-1)]×4

(2) If the elements of the two matrices involved in the multiplication operation are still stored sequentially by row, the address pointers need to be calculated separately, resulting in double the amount of calculation; for this reason, the multiplier matrix is stored sequentially by column, as shown in the figure 2 (taking a 3×3 dimensional matrix as an example). An m×n dimensional matrix A is multiplied by an n×p dimensional matrix B to obtain an m×p dimensional matrix C.

When the two matrices are stored according to the data structure shown in Figure 2, the address pointers of the elements a _ik and bk _j in the two data blocks are the same, and it only needs to be calculated once according to the formula described in (1).

2. Realize matrix operation in S7-300 series PLC system:

To realize the matrix operation in the S7-300 series PLC system, it is necessary to solve the access of matrix elements, the addition and subtraction of the matrix, the multiplication of the matrix and the inverse operation of the matrix.

(1) Access to matrix elements:

The access of matrix elements includes the read and write operations of matrix elements, which are all operated on a given matrix. Therefore, the read and write operations of matrix elements can be realized by the same program module, and its implementation process is shown in Figure 3. The program number is FC1, and its input parameters are: data block number DBno, matrix row number m, column number n, row i and column j of read and write elements, read and write flag WR (1 is write operation, 0 is read operation); The input and output parameters are: dat (WR=1, dat is the data to be written; when WR=0, dat is the read data).

(2) Matrix addition and subtraction:

The addition and subtraction operation rules of matrix are similar, so the addition and subtraction operation of matrix can be designed as a module, its name is FC2, and its implementation process is shown in Figure 4. The input parameters are: data block numbers DBNo1, DBNo2, DBNo3, the number of rows of the matrix m, the number of columns n; addition and subtraction flag PorM (1: addition, 0: subtraction); no output parameters.

(3) Matrix multiplication:

The flow of matrix multiplication operation is shown in Figure 5, and the module name is FC3. The input parameters are: data block numbers DBNo1, DBNo2, DBNo3, the number of rows m and the number of columns of the multiplicand matrix, the number of rows n and the number of columns of the multiplier matrix; no output parameters. An m×n dimensional matrix A is multiplied by an n×p dimensional matrix B to obtain an m×p dimensional matrix C, and each element ci _j in the matrix C is calculated according to the following formula:

A total of n times of multiplication and (n-1) times of addition are required. Therefore, the multiplication of the matrix requires a large amount of computing processing, and the alarm timer of the S7-300 series PLC system needs to be set reasonably.

(4) Inverse operation of matrix:

The name of the matrix inverse operation program module is FC4, input parameters: data blocks DBno1, DBNo2, matrix row number m, column number n; no output parameters. The matrix inverse operation of the PLC system is carried out as follows:

The first step: Calculate the determinant A of the matrix A through the elementary transformation angle, and the program flow is shown in Figure 6;

The second step: use the algebraic remainder to calculate the adjoint matrix A ^* of A, the program flow is shown in Figure 7;

Step 3: Calculate the inverse matrix A ^-1 of A according to the following formula, and the program flow is shown in FIG. 8 .

Based on the above-mentioned realized matrix definition and its operation, according to the iteration formula of the Kalman filter, the Kalman filter can be realized in the PLC system by cyclically calling the above-mentioned realized matrix operation program module. First define the data blocks DB1~DB14 to store the variables in the Kalman filter formula

According to the discrete state equation of the linear system shown below, DB1~DB6 store their corresponding matrix variables X(k), Φ(k), Η(k), y(k), ω(k) and v(k) respectively.

X(k) is the state vector; y(k) is the observation vector; ω(k) is the system noise; v(k) is the observation noise vector; Φ(k) is the non-singular state transition matrix; H(k) is the observation matrix . ω(k) and v(k) are uncorrelated Gaussian white noise with zero mean and variance Q and R respectively.

Data block DB7 stores state estimation value vector DB8 stores state one-step predicted value vector DB9 stores the filter gain matrix K(k), DB10 stores the covariance matrix P(k-1,k-1) of the estimated error, DB11 stores the one-step predicted value P(k,k-1) of the covariance matrix, and DB12 stores Q , DB13 stores R, and DB14 stores the identity matrix I.

Step 2: Compute the one-step forecast of the Kalman filter:

Take out from the data block DB7 and DB2 respectively and Φ(k), calculate the one-step predicted value of the Kalman filter as follows

Will Stored in data block DB8.

Step 3: Calculate the state estimate:

Take out from the data blocks DB8, DB9, DB3, DB4 respectively K(k), H(k) and y(k), calculate the state estimation value according to the following formula

use Update DB7 in

Step 4: Calculate the filter gain matrix:

Take out P(k,k-1), H(k) and R from the data blocks DB11, DB3 and DB13 respectively, and calculate the filter gain matrix K(k) as follows:

K(k)=P(k,k-1) ^HT (k)[H(k)P(k,k-1) ^HT (k)+R] ^-1

Store K(k) in data block DB9.

Step 5: Calculate the one-step predictor of the error covariance matrix:

Take out Φ(k), P(k-1,k-1) and Q from the data blocks DB2, DB10, and DB12 respectively, and calculate the one-step predicted value P(k,k-1) of the error covariance matrix according to the following formula:

P(k,k-1)=Φ(k)P(k-1,k-1) ^ΦT (k)+Q

Store P(k,k-1) in the data block DB11.

Step 6: Calculate the covariance matrix of the estimation errors

Take out I, K(k), H(k) and P(k,k-1) from the data blocks DB14, DB9, DB3 and DB11 respectively, and calculate the covariance matrix P(k,k) of the estimation error according to the following formula :

P(k,k)=[I-K(k)H(k)]P(k,k-1)

Use P(k,k) to update P(k-1,k-1) in DB10.

So far, an iterative operation of the Kalman filter is completed, and the organization of program modules and data blocks and their associations are shown in Figure 9 .

List specific embodiment below and illustrate the present invention:

Embodiment 1: The Kalman filter application example in the PLC system, in order to verify the Kalman filter realized in the PLC system, it is proposed to use the Kalman filter algorithm to estimate the height and velocity of a free-falling ball. The 20 measurements of the ball height are shown in Table 1. Assuming that the measurement error of height is Gaussian white noise with zero mean and variance of 1, the initial height h ₀ and velocity V ₀ of the object are also random variables of Gaussian distribution, namely

The state equation of the ball motion is as follows:

In the formula,

Filter initial value:

In order to visually display the execution effect of the Kalman filter in the PLC, use the trend control of the WinCC configuration software to export its data to EXCEL to draw a curve, as shown in Figure 10. The upper left panel is the altitude estimate, the upper right panel is the velocity estimate, the lower left panel is the height estimate covariance, and the lower right panel is the velocity estimate covariance. Table 1 shows the height change value of the free fall of the ball.

Table 1

In order to further verify the Kalman filter implemented in the PLC system, Figure 11 shows the height and velocity estimation values and mean square error curves of the Kalman filter provided by Matlab to realize the ball motion tracking. In Figure 11, the upper left figure is the estimated height, the upper right figure is the estimated velocity, the lower left figure is the estimated covariance of the height, and the lower right figure is the estimated covariance of the velocity.

It can be seen from the comparison that the Kalman filter implemented in the PLC system and the Kalman filter provided by Matlab have similar effects in estimating the velocity and height of the ball.

The above is only a preferred specific embodiment of the present invention, and the scope of protection of the present invention is not limited thereto. Any person familiar with the technical field within the technical scope disclosed in the present invention can obviously obtain the simplicity of the technical solution. Changes or equivalent replacements all fall within the protection scope of the present invention.

Claims (2)

1. implementation method of the Kalman filter in S7-300 series of PLC, it is characterised in that：Kalman filter iteration is calculated In formula, the accessing operation of some matrix variables is converted into the accessing operation of variable in S7-300 series of PLC system corresponding data blocks, And plus and minus calculation, multiplying and the inverse operation of matrix are realized in PLC system.

2. the implementation method according to Kalman filter described in claim 1 in S7-300 series of PLC, it is characterised in that：

Step 1：Data block DB1～DB16 is defined, the variable in Kalman filter formula is deposited；DB1～DB6 deposits card respectively Matrix variables X (k), Φ (k), H (k), y (k), ω (k) and v (k) in Thalmann filter iteration formula, the storage of data block DB7 State estimation vectorDB8 storage configuration one-step predictions value vectorDB9 storage filtering Gain matrix K (k), DB10 storages estimation error covariance matrix P (k-1, k-1), DB11 deposit a step of covariance matrix Predicted value P (k, k-1), DB12 storage Q, DB13 storage R, DB14 storage unit matrix I, DB15 and DB16 are used to keep in；It is based on The data block of definition, realizes plus and minus calculation, multiplying and the inverse operation of matrix in PLC；Wherein, associations of the Q for system noise Variance matrix, covariance matrixes of the R for measurement noise；

Step 2：The one-step prediction value of Kalman filter is calculated, is taken out from data block DB7 and DB2 respectively With Φ (k), the one-step prediction value of Kalman filter is calculated as follows

$\hat{X}(k,k-1)=\mathrm{\Φ}\left(k\right)\hat{X}(k-1,k-1)$

WillIt is stored in data block DB8；

Step 3：State estimation is calculated, is taken out from data block DB8, DB9, DB3, DB4 respectivelyK(k)、H K () and y (k), is calculated as follows state estimation

$\hat{X}(k,k)=\hat{X}(k,k-1)+K\left(k\right)\[y\left(k\right)-H\left(k\right)\hat{X}(k,k-1)\]$

UseUpdate in DB7

Step 4：Filtering gain matrix are calculated, respectively taking-up P (k, k-1), H (k) and R from data block DB11, DB3 and DB13, It is calculated as follows filtering gain matrix K (k)：

K (k)=P (k, k-1) H^T(k)[H(k)P(k,k-1)H^T(k)+R]^-1

K (k) is stored in into data block DB9；

Step 5：The one-step prediction value of calculation error covariance matrix, takes out Φ (k), P from data block DB2, DB10, DB12 respectively (k-1, k-1) and Q, are calculated as follows one-step prediction value P (k, k-1) of error covariance matrix：

P (k, k-1)=Φ (k) P (k-1, k-1) Φ^T(k)+Q

P (k, k-1) is stored in into data block DB11；

Step 6：Estimation error covariance matrix are calculated, respectively taking-up I, K from data block DB14, DB9, DB3 and DB11 K (), H (k) and P (k, k-1), are calculated as follows estimation error covariance matrix P (k, k)：

P (k, k)=[I-K (k) H (k)] P (k, k-1)

The P (k-1, k-1) in DB10 is updated using P (k, k), so far, complete an iteration computing of Kalman filter.