This article introduces the usage of the KalmanFilter custom library function in TwinCAT 3. It will first briefly explain the basic principles of Kalman filtering, then introduce the pin definitions and usage of two function blocks within this library, and finally present an application case of this library.
Kalman Filtering
For users who wish to delve into the specific derivation of Kalman filtering, please refer to textbooks or relevant papers (Baidu is not recommended). Kalman filtering is specifically tailored for linear sequences. If you need to handle nonlinear sequences, consider options such as the Extended Kalman Filter (EKF) or the Unscented Kalman Filter (UKF). Their basic principles are comparable to Kalman filtering, but they require the use of Taylor series expansion for deriving the function terms.
The following provides the state-space representation in difference form:
Where, A is the system matrix, B is the input matrix, H is the measurement matrix,， Wk and 分are process noise and measurement noise, respectively, and both are assumed to be Gaussian white noise.
Q and R represent the process standard deviation and measurement standard deviation, respectively. Due to the properties of the normal distribution, both can be considered as covariances. To eliminate noise, three variables are introduced：
is referred to as the true value;
is referred to as the predicted value, also known as the prior state estimate.
is referred to as the estimated value, also known as the posterior state estimate.
The prior covariance matrix 和 and posterior covariance matrix ， at time K are given. Combining the definitions mentioned above and omitting the mathematical derivations (note that this omission assumes familiarity with Bayes' theorem, the basic definitions of Markov chains, and the use of matrix covariance definitions, the trace of a matrix, and matrix calculus formulas for derivations), the following five expressions are obtained:
Definition and Usage of One-Dimensional Function Blocks
Function Block Diagram and Variable Definitions
Both the output and input are of type LREAL. The variable type can be modified according to requirements, but the data type of all internal variables must be changed accordingly.
The default initial values for system covariance and measurement covariance are 2 and 100, respectively, and can be modified as input variables.
Usage Notes
Due to the lack of effective matrix operations in TwinCAT, this function block is specifically designed for one-dimensional signals. The transition matrices are all set to 1, and matrix inversion simplifies to division. To instantiate the function, define a variable named kalmanfilter of type KalmanFilter in the variable declaration area, as follows:
APPALICATION I
Periodic jump signal with noise
Kalman Filtering Program
Kalman Filtering Effect
The green curve represents the noisy signal with jumps, the blue signal represents the dynamic low-pass filtering, and the yellow curve represents the result of Kalman filtering. In this example, Kalman filtering is not used dynamically, so its dynamic performance is not as good as that of a dynamic filter. However, it exhibits better steady-state response with less curve fluctuation.
Definition and Application of Two-Dimensional Function Blocks
Function Block Diagram and Variable Definitions
Usage Instructions
For a given model, fill in all the system matrices in the input and output sections, and input the measurement signal to "NewData". You can adjust the values of Q and R to achieve optimal filtering results. Please note the controllability and observability of the system, which will be reflected in error reports. If an error occurs, please set and then reset the "clearError" bit. If the system matrices are too arbitrary, it may cause the estimated state to overflow during iteration, which is avoided within the function block. If such an error occurs, please re-activate the configuration.
Please assign a value to this pin even when there is no input control, with 0 being acceptable.
APPALICATION II
The input signal is as follows: a noisy signal with jumps.
The filtering results are as follows. The observation matrix consists of 0.1 and 1, resulting in state 2 (orange curve) being close to the output signal, while state 1 (blue curve) represents an arbitrary state.
Common Issues
Limitations
The two function blocks in this library have limitations: one can only process one-dimensional states without control signals, while the other can only handle two-dimensional SISO (Single Input Single Output) state signals. The code source is not compiled, allowing for modifications to expressions or the use of external software for matrix operations such as multiplication, transposition, and inversion, in order to process signals with more than two-dimensional states or MIMO (Multiple Input Multiple Output) systems.