Telemark University College Department of Electrical Engineering, Information Technology and Cybernetics
Examples
State Estimation with Kalman Filter HANS-‐PETTER H ALVORSEN, 2 014.08.15
Faculty of Technology,
Postboks 203, Kjølnes ring 56, N-3901 Porsgrunn, Norway.
Tel: +47 35 57 50 00
Fax: +47 35 57 54 01
Table of Contents Table of Contents .................................................................................................................................... ii 1 Introduction ...................................................................................................................................... 3 2 Modelling .......................................................................................................................................... 6 2.1 Model parameter adjustment – Trial and Error ........................................................................ 6 2.2 Discrete Model .......................................................................................................................... 9 2.3 State-‐space Model ................................................................................................................... 12 2.4 Discrete State-‐space Model .................................................................................................... 15 3 System Identification ...................................................................................................................... 18 3.1 Datalogging ............................................................................................................................. 20 3.2 Least Square method .............................................................................................................. 25 3.3 Trial and Error Adjustment ...................................................................................................... 29 3.4 System Identification using Step Response ............................................................................. 30 3.5 Subspace/Blackbox methods .................................................................................................. 30 4 Kalman Filter ................................................................................................................................... 32 4.1 Using the built-‐in Kalman Filter in LabVIEW ............................................................................ 32 4.2 Implement your own Kalman Filter ......................................................................................... 34 5 Feedforward Control ...................................................................................................................... 41 5.1 Feedforward Controller ........................................................................................................... 41 5.2 Implementing Feedforward Control ........................................................................................ 43 Appendix A -‐Tuning the PI(D) Controller ............................................................................................... 45
ii
1 Introduction This example is about implementing a state estimator of the outflow of a water tank based on level measurement, and implementing a level control system based on feedback control. Next, we want to improve the level control in the water tank. We measure the level ℎ in the water tank and using an ordinary PID controller (image to the left). Out of the water tank is a flow 𝐹!"# which can be consider as noise, this makes it harder to control the level. 𝐹!"# can be manually adjusted with a handle. Tradition PID Control (Feedback Control):
Feedforward Control:
We want to improve the control of the level by implementing a Feedforward controller in addition to the PID controller (Feedback), see image to the right. The problem is that we cannot measure 𝐹!"# . We want to use a state estimator, such as a Kalman Filter and an Observer in order to estimate 𝐹!"# . The LM-‐900 Level Tank equipment will be used in this example:
3
4
Introduction
A very simple (linear) model of the water tank is as follows: 𝐴! ℎ = 𝐾! 𝑢−𝐹!"# or ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
Where: • • • •
ℎ [𝑐𝑚] is the level in the water tank 𝑢 [𝑉] is the pump control signal to the pump 𝐴! [𝑐𝑚2] is the cross-‐sectional area in the tank 𝐾! [(𝑐𝑚3/𝑠)/𝑉] is the pump gain
•
𝐹!"# [𝑐𝑚3/𝑠] is the outflow through the valve (this outflow can be modeled more accurately taking into account the valve characteristic expressing the relation between pressure drop across the valve and the flow through the valve).
Control Signal: A pump fills the tank with water from the reservoir. The pump speed can be controlled by a voltage signal in the range 0 − 5𝑉. The pump can be controlled by an external voltage signal at the “FROM PC” connector. Measurement Signal: The measurement is a voltage signal in the range 0 − 5𝑉 available at the “TO PC” connector. This voltage range corresponds to a level range of 0 − 20 𝑐𝑚, approximately (unless you need a more accurate relation, you can assume this range in your applications). Scaling: You need to scale the signal 0 − 5𝑉 to 0 − 20𝑐𝑚. The following linear relationship applies: 𝑦 = 𝑎𝑥 + 𝑏 In detail: State Estimation with Kalman Filter
5
Introduction
𝑦 = 𝑎𝑥 + 𝑏
You have to find 𝒂 (slope) and 𝒃 (intercept). The following formulas may be used: 𝑦 − 𝑦! =
𝑦! − 𝑦! (𝑥 − 𝑥! ) 𝑥! − 𝑥!
This gives: 𝑦=
𝑦! − 𝑦! 𝑦! − 𝑦! 𝑥 + 𝑦! − 𝑥 𝑥! − 𝑥! 𝑥! − 𝑥! ! !
! !
where 𝑎=
𝑦! − 𝑦! 𝑥! − 𝑥!
𝑏 = 𝑦! − 𝑎𝑥!
State Estimation with Kalman Filter
2 Modelling 2.1 Model parameter adjustment – Trial a nd E rror Adjust the model parameters 𝐴! , 𝐾! from experiments on the real process. You need to use the LM-‐900 Level Tank equipment and the USB-‐6008 DAQ for this task. Show by simulations where you run the real process in parallel with the simulated model that you have found proper values. A can be found directly if you measure the radius in the tank (𝐴! = 𝜋𝑟 ! ). Note! 0 − 5 𝑉 → 0 − 20[𝑐𝑚]
Solutions: We get the following results: 𝐴! = 𝜋𝑟 ! We measure the diameter in the tank to be: 𝑑 = 10𝑐𝑚. This gives: 𝑨𝒕 = 𝝅𝟓𝟐 = 𝟕𝟖. 𝟓𝒄𝒎𝟐 𝐾! is found to be (trial and error method): 𝑲𝒑 = 𝟏𝟔. 𝟓 𝒄𝒎𝟑 /𝒔 The following LabVIEW program have been used in the trial and error: LabVIEW: LabVIEW Front Panel:
6
7
Modelling
LabVIEW Block Diagram:
SubVI for getting and sending information to the real process:
State Estimation with Kalman Filter
8
Modelling
→ As seen from the Figure above the signal from the process is going through a low-‐pass filter an it is scaled from 0 − 5𝑉 to 0 − 20𝑐𝑚. The following linear scaling is used: 𝒚 = 𝟒𝒙 where 𝑥[0 − 5𝑉] and 𝑦[0 − 20𝑐𝑚] Simulation Sub System for the model: A Simple block diagram of the model:
Block Diagram implemented in LabVIEW:
State Estimation with Kalman Filter
9
Modelling
Front Panel:
2.2
Discrete M odel
We have the following mathematical model for the water tank: ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
Discretization: We use the Euler Forward discretization method: 𝑥≈
𝑥!!! − 𝑥! 𝑇!
Where 𝑇! is the sampling time in seconds. This gives the following discrete system: ℎ!!! − ℎ! 1 = 𝐾 𝑢 −𝐹 𝑇! 𝐴! ! ! !"# This gives: 𝒉𝒌!𝟏 = 𝒉𝒌 + 𝑻𝒔
𝟏 𝑲 𝒖 −𝑭 𝑨𝒕 𝒑 𝒌 𝒐𝒖𝒕
Or: ℎ!!! = ℎ! −
𝑇! 𝐾! 𝑇! 𝐹!"# + 𝑢 𝐴! 𝐴! !
State Estimation with Kalman Filter
10
Modelling
Simulation in MathScript: We want to implement the model in MathScript and simulate it and see the step response for the system. We use the following values: 𝐾! = 15(𝑐𝑚 ! /𝑠)/𝑉 𝐴! = 80𝑐𝑚 ! 𝐹!"# = 10𝑐𝑚 ! /𝑠 𝑇! = 0.1𝑠 MathScript Code: clear, clc % Model % Model parameters Kp = 15; At = 80; Fout = 10; h(1) = 0; %Initial value Ts = 0.1; %Sampleing Time uk = 1; %Step k = 1:100; N = length(k); for i=2:N h(i) = h(i-1) + (Ts/At)*(Kp*uk-Fout); end plot(k, h) title('Simulation of discrete Level model after a step in u(k) at k=0') xlabel('k') ylabel('h(k)')
This gives the following plot:
State Estimation with Kalman Filter
11
Modelling
Discussion: Note! 𝑘 (𝑡! ) is the discrete integer time index. 𝑇! is the sampling interval between 𝑡! and 𝑡!!! . Example if 𝑘 = 10, then the real time is 𝑡 = 𝑇! ×𝑘 = 0.10𝑠×10 = 1𝑠 Note! You should always show units on the x-‐axis and the y-‐axis in the plot. We can use the built-‐in functions title(), xlabel() and ylabel() for this. We see this is a typical behavior of such as level tank system, i.e., it is an integrator. We can find the transfer function 𝐻 𝑠 =
!(!) !(!)
We start with the differential equation: ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
Laplace gives: 𝑠ℎ(𝑠) =
𝐾! 𝑢(𝑠) 𝐴!
We have set 𝐹!"# = 0 This gives: 𝐻 𝑠 = We introduce
!! !!
ℎ(𝑠) 𝐾! 1 = 𝑢(𝑠) 𝑢(𝑠) 𝐴! 𝑠
≡ 𝐾, then we get:
State Estimation with Kalman Filter
12
𝐻 𝑠 =
Modelling
ℎ(𝑠) 𝐾 = 𝑢(𝑠) 𝑢(𝑠) 𝑠
→ We see that the process is an integrator. We want to find the mathematical expression for the step response ℎ(𝑡). The Laplace Transformation pair for a step is as follows: 1 ⇔ 1 𝑠 The step response of an integrator then becomes: ℎ 𝑠 =𝐻 𝑠 𝑢 𝑠 =
𝐾 𝑈 1 ∙ = 𝐾𝑈 ! 𝑠 𝑠 𝑠
We use the following Laplace Transformation pair in order to find ℎ(𝑡): 1 ⇔ 𝑡 𝑠! Then we finally get: ℎ 𝑡 = 𝐾𝑈𝑡 → We see that the step response of the integrator is a Ramp. A bigger K will give a bigger slope (In Norwegian: “stigningstall”) and the integration will go faster. The simulation in MathScript will also show this. We should also try different values for 𝐾! and 𝐹!"# . If 𝐹!" < 𝐹!"# the level will decrease. In the mathematical model the level will also be less than zero – which of course is not possible in the real world. The same if 𝐹!" > 𝐹!"# – the level will decrease and finally be larger than 20𝑐𝑚. You should add the following in your code: if h > hmax h = hmax if h < hmin h = hmin …
In our case the water level is between 0 − 20𝑐𝑚.
2.3
State-‐space M odel State Estimation with Kalman Filter
13
Modelling
For the real system, only the level (ℎ) is measured, so we want to use a Kalman Filter for estimating the outflow (𝐹!"# ) of the tank. Set up the system on the following form (“pen and paper”): 𝑥 = 𝐴𝑥 + 𝐵𝑢 𝑦 = 𝐶𝑥 + 𝐷𝑢 Set 𝑥! = ℎ and 𝑥! = 𝐹!"# Assume that 𝐹!"# is constant, i.e., 𝐹!"# = 0 → Find the matrices 𝐴, 𝐵, 𝐶 𝑎𝑛𝑑 𝐷. → Implement the state-‐space model in LabVIEW.
Solutions: We have: ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
𝑥! =
1 𝐾 𝑢−𝐹!"# 𝐴! !
ℎ = 𝑥! gives:
𝑥! = 𝐹!"# : We then assume that 𝐹!"# is constant (changes very slowly), thus 𝐹!"# = 0. This gives: 𝑥! = 0 Then we have the following differential equations for the system: 𝑥! =
1 𝐾 𝑢−𝑥! 𝐴! ! 𝑥! = 0
This gives the linear state-‐space model: 𝑥! = −
1 1 𝑥! + 𝐾! 𝑢 𝐴! 𝐴! 𝑥! = 0 𝑦 = 𝑥!
Which gives: State Estimation with Kalman Filter
14
𝑥! 0 = 𝑥! 0
1 𝐴! 0
−
Modelling 𝐾! 𝑥! + 𝐴! 𝑢 𝑥! 0
!
!
𝑦= 1 !
𝑥! 0 𝑥 !
i.e., 𝐷 = [0] LabVIEW: This is just an example; there are many ways to create a state-‐space model in LabVIEW: Block Diagram:
Front Panel:
State Estimation with Kalman Filter
15
Modelling
Note! You can create both symbolic and numeric matrices: Numeric
Symbolic
2.4
Discrete S tate-‐space M odel
Find the discrete linear state-‐space model on the following form: 𝑥!!! = 𝐴𝑥! + 𝐵𝑢! 𝑦! = 𝐶𝑥! + 𝐷𝑢! Use the Euler Forward discretization method. 𝑥≈
𝑥!!! − 𝑥! 𝑇!
Where 𝑇! is the sampling time. → Find the matrices 𝐴, 𝐵, 𝐶 𝑎𝑛𝑑 𝐷 in the discrete state-‐space model (pen and paper). → Use LabVIEW to convert the continuous model to a discrete state-‐space model from the previous task. Compare the results with your calculations.
Solutions: We have from the previous task: 𝑥! = −
1 1 𝑥! + 𝐾! 𝑢 𝐴! 𝐴! 𝑥! = 0
We use Euler Forward discretization method: 𝑥≈
𝑥 𝑘 + 1 − 𝑥(𝑘) 𝑇!
Applying Euler Forward discretization method gives:
State Estimation with Kalman Filter
16
Modelling
𝑥! 𝑘 + 1 − 𝑥! 𝑘 1 1 = − 𝑥! (𝑘) + 𝐾! 𝑢(𝑘) 𝑇! 𝐴! 𝐴! 𝑥! 𝑘 + 1 − 𝑥! 𝑘 = 0 𝑇! This gives: 𝑥! 𝑘 + 1 = 𝑥! 𝑘 −
𝑇! 𝐾! 𝑇! 𝑥! 𝑘 + 𝑢(𝑘) 𝐴! 𝐴!
𝑥! 𝑘 + 1 = 𝑥! 𝑘 The discrete state-‐space model then becomes: 𝑥! 𝑘 + 1 𝑥! 𝑘 + 1
=
1
𝑇! 𝐴! 1
𝑥! (𝑘) + 𝑥! (𝑘)
−
0 !
𝑇! 𝐾! 𝐴! 𝑢(𝑘) 0 !
𝑦(𝑘) = 1
0 !
𝑥! (𝑘) 𝑥! (𝑘)
LabVIEW: Block Diagram:
Front Panel:
State Estimation with Kalman Filter
17
Modelling
State Estimation with Kalman Filter
3 System Identification What is System Identification? System Identification methods may be used to build mathematical models of dynamic systems based on observed and measured input and output data from the system. System Identification is defined as: Identification is the determination, on the basis of input and output, of a system within a specified class of systems, to which the system under test is equivalent. In system identification, different steps are involved: 1. Log Data – We need to log input and output signals from the process we want to identify. A Good excitation signal is important. 2. Split Data – It is good practice to split your data; one data set is used to find the model, while the other is used to validate the model. A 50-‐50 split is normal to use. 3. Model estimation – We find the model or the model parameters based on one or more system identification methods, e.g. Least Square, Black-‐box methods, etc. 4. Validate – We need to check if the model or the model parameters we have found is good by simulations. It is important that we use another data set – not the same as used when finding the model. We will use all these steps in this assignment. We will also use different methods for finding the model of the system. These steps are illustrated below:
18
19
System Identification
In this lab we will try out different system identification methods. The methods we will use are as follows: • • • •
“Trial and Error” method “Step response” method “Least Square” method “Blackbox/Subspace” methods
All the methods are based on input-‐output data from the real process, as shown below:
We will test the different methods mentioned above and compare the results. We can divide system identification into two different situations: Case 1: When you have a mathematical model and want to find the unknown parameters in the model. Then we will use Parameter Estimation and use, e.g., the Least Square Method (LS). Parameter Estimation using, e.g., the Least Square Method (LS) is used to find a model with unknown physical parameters in a mathematical model. Case 2: When we don’t have a mathematical model, i.e., we have only input/output data of the system. Then we will use Sub-‐space methods/Black-‐Box methods in order to find a model of the system. Sub-‐space methods or Black-‐Box methods is used to find a model with non-‐physical parameters. A sub-‐space methods/Black-‐Box method estimates a linear discrete State-‐space model on the following general state-‐space form: 𝑥!!! = 𝐴𝑥! + 𝐵𝑢! 𝑦! = 𝐶𝑥! + 𝐷𝑢!
State Estimation with Kalman Filter
20
3.1
System Identification
Datalogging
We have different exitation signals: • • •
A PRBS signal (Pseudo Random Binary Signal) A Chirp Signal A Up-‐down signal
A PRBS signal looks like this:
General Introduction to the Task: Use LabVIEW for exciting the process and logging signals. Use open-‐loop experiments (no feedback control system). You can use the Write to Measurement File function on the File I/O palette in LabVIEW for writing data to text files (use the LVM data file format, not the TDMS file format which give binary files). In the File I/O palette in LabVIEW we have lots of functionality for writing and reading files. Below we see the “File I/O” palette in LabVIEW:
State Estimation with Kalman Filter
21
System Identification
Use the “Write To Measurement File” and “Read From Measurement File”.
A PRBS signal (Pseudo Random Binary Signal) In LabVIEW you can use the “SI Generate Pseudo-‐Random Binary Sequence.vi” function. LabVIEW Functions Palette: Control Design & Simulation → System identification → Utilities → SI Generate Pseudo-‐Random Binary Sequence.vi
Excitation signals: We can use different excitation signals, in my example I have just used a simple Up-‐Down signal. This signal is simple to generate manually during the experiment. This signal gives in many cases enough excitation for the estimator to calculate accurate parameter estimates, but the period of the signal shift must be large enough to give the system output a chance to approximately stabilize between the steps.
State Estimation with Kalman Filter
22
System Identification
A simple Up-‐Down signal look could look like this:
LabVIEW program for Datalogging: Front Panel:
Block Diagram:
State Estimation with Kalman Filter
23
System Identification
Settings for Measurement File: Recommended settings for the “Write To Measurement File”:
It is also recommended that you create a simple program that opens the data from the file and shows the data in arrays and graphs, just to check that everything is OK. This is a simple check to verify your logged data is OK. Front Panel (View Measurement Data):
State Estimation with Kalman Filter
24
System Identification
Block Diagram (View Measurement Data):
Recommended settings for the “Read From Measurement File”:
State Estimation with Kalman Filter
25
System Identification
3.2
Least Square m ethod
Parameter Estimation using the Least Square Method (LS) is used to find a model with unknown physical parameters in a mathematical model. From the mathematics when solving a set of linear equations, we have: 𝐴𝑥 = 𝑏 The following equations: 𝑥! + 2𝑥! = 5 3𝑥! + 4𝑥! = 6 Can be written in matrix form (𝐴𝑥 = 𝑏): 2 𝑥! 5 = 4 𝑥! 6
1 3 !
!
!
Then we can solve the equations, i.e., finding x using different methods (Gauss Elimination, Least Square, etc.) The Least Square method: When using the Least Square method in System Identification theory it is normal to use the following notation: State Estimation with Kalman Filter
26
System Identification
𝑌 = Φ𝜃 (which is the same as 𝑏 = 𝐴𝑥) Where 𝜽 is the unknown parameter vector 𝑌 is the known measurement vector Φ is the known regression matrix It can be found that the least square solution for 𝑌 = Φθ is: 𝜃!" = (Φ ! Φ)!! Φ ! Y This equation can easily be implemented in a programming language, such as MathScript or LabVIEW. LabVIEW: You may want to use the blocks in the “Linear Algebra” (located in the Mathematics palette) palette in order to fin the Least Square solution:
Example: Given the following model: 𝑦 𝑢 = 𝑎𝑢 + 𝑏 The following values are found from experiments: 𝑦 1 = 0.8 𝑦 2 = 3.0 𝑦 3 = 4.0 State Estimation with Kalman Filter
27
System Identification
We will find the unknowns 𝑎 and 𝑏 using the Least Square (LS) method in MathScript/LabVIEW. We have that: 𝑌 = Φ𝜃 Where 𝜃 is the unknown parameter vector 𝑌 is the known measurement vector Φ is the known regression matrix The solution for 𝜃 may be found as (if Φ is a quadratic matrix): 𝜃 = Φ !! 𝑌 It can be found that the least square solution for 𝑌 = Φ𝜃 is: 𝜃!" = (Φ ! Φ)!! Φ ! Y We get: 0.8 = 𝑎 ∙ 1 + 𝑏 3.0 = 𝑎 ∙ 2 + 𝑏 4.0 = 𝑎 ∙ 3 + 𝑏 This becomes: 0.8 1 3.0 = 2 4.0 3 !
!
1 𝑎 1 𝑏 1 !
MathScript: We define 𝑌 and Φ in MathScript and find 𝜃 by: phi = [1 1; 2 1; 3 1]; Y = [0.8 3.0 4.0]'; theta = inv(phi'*phi)* phi'*Y %or simply by theta=phi\Y
The answer becomes:
State Estimation with Kalman Filter
28
theta =
System Identification
1.6 -0.6
i.e.: 𝑎 = 1.6 𝑏 = −0.6 This gives: 𝑦 𝑢 = 1.6𝑢 − 0.6 LabVIEW: In LabVIEW we can implement this as follows: Block Diagram:
As you see this code implements the formula 𝜃!" = (Φ ! Φ)!! Φ ! Y. Front Panel:
We can also use the “Solve Linear Equations.vi” directly:
[End of Example]
State Estimation with Kalman Filter
29
System Identification
Tip! It is a good idea to split your program into different logical parts using, e.g., SubVIs in LabVIEW.
The different parts/steps could, e.g., be: 1. Get Logged Data from File a. Input: File Name b. Outputs: 𝑢 and 𝑦 (𝑇!"# ) 2. Transform the data and stack data into 𝑌 and Φ a. Inputs: 𝑢 and 𝑦 (𝑇!"# ) b. Outputs: 𝑌 and Φ 3. Find the Least Square solution 𝜃!" = (Φ ! Φ)!! Φ ! Y a. Input: 𝑌 and Φ
3.3
Trial a nd E rror Adjustment
Parameter Adjustment: Adjust the unknown model parameters (𝜃! , 𝐾! , 𝑇!"# ) of the model by some simple experiments where you run the simulator in parallel with the real process. Use the “Trial and Error” method in order to find the unknown parameters. The LabVIEW program could be like this:
This means that we plot the output of the model and the process and compare them after a change in the control signal. If they are “almost” identical, we have found good model parameters, if not, we need to adjust the modelparameters and try again. You need to use the USB-‐6008 DAQ for this task. State Estimation with Kalman Filter
30
System Identification
3.4 System Identification u sing Step R esponse Model Parameters: Find the unknown model parameters (𝜃! , 𝐾! ) by running a step response on the real process. Find the transfer function 𝐻(𝑠) for the system on the form: 𝐻 𝑠 =
𝑇!"# (𝑠) 𝐾 = 𝑒 !!" 𝑢(𝑠) 𝑇𝑠 + 1
Note! In this Lab Work you may want to use Temperature sensor 2 on the Air Heater and assume that the time-‐delay 𝜃! = 0 (for simplicity). Perform a step response on the real process to find the unknown model parameters as shown below:
You need to use the USB-‐6008 DAQ for this task.
3.5
Subspace/Blackbox m ethods
Sub-‐space methods/Black-‐Box methods is used to find a model with non-‐physical parameters. A sub-‐space methods/Black-‐Box method estimates a linear discrete State-‐space model on the form: 𝑥!!! = 𝐴𝑥! + 𝐵𝑢! 𝑦! = 𝐶𝑥! + 𝐷𝑢! LabVIEW offers functionality for this. In the “Parametric Model Estimation” palette we find the “SI Estimate State-‐Space Model.vi” which can be used for sub-‐space identification. “Parametric Model Estimation” palette: State Estimation with Kalman Filter
31
System Identification
This VI estimates the parameters of a state-‐space model for an unknown system. Example of Implementation in LabVIEW:
State Estimation with Kalman Filter
4 Kalman Filter The Kalman Filter is a commonly used method to estimate the values of state variables of a dynamic system that is excited by stochastic (random) disturbances and stochastic (random) measurement noise. The Kalman Filter is a state estimator which produces an optimal estimate in the sense that the mean value of the sum of the estimation errors gets a minimal value. In this task we will estimate the process variable(s) for the water tank using a Kalman Filter. Below we see a sketch of how a Kalman Filter is working:
The estimator (model of the system) runs in parallel with the system (real system or model). The measurement(s) is used to update the estimator.
4.1 Using the b uilt-‐in Kalman Filter in LabVIEW LabVIEW has several built-‐in Kalman Filter algorithms in addition to other estimation functionality. Below we see the “Estimation” palette (Control and Simulation → Simulation → Estimation) in LabVIEW (which is part of the LabVIEW Control Design and Simulation Module):
32
33
Kalman Filter
In this example we will use the “Discrete Kalman Filter”. Use one of the built-‐in discrete Kalman Filter functions available in LabVIEW (LabVIEW Design and Simulation Module). → Plot the simulated results vs. the estimated results. Your application should work both for a simulated model and for the real process. Add noise in the simulated model to make it more realistic. Use, e.g., the “Gaussian White Noise PtByPt.vi”. → Tune the Kalman Filter by adjusting the covariance matrices 𝑃 and 𝑄. Discuss the results. Change the outflow by adjusting manually the outlet valve. What happens?
Solutions: Block Diagram:
State Estimation with Kalman Filter
34
Kalman Filter
Front Panel:
4.2 Implement your o wn Kalman Filter The Kalman Filter algorithm is as follows: Pre Step: Find the steady state Kalman Gain K K is time-‐varying, but you normally implement the steady state version of Kalman Gain K. Use the “CD Kalman Gain.vi” in LabVIEW or one of the functions kalman, kalman_d or lqe in MathScript. Init Step: Set the initial Apriori (Predicted) state estimate 𝑥! = 𝑥!
State Estimation with Kalman Filter
35
Kalman Filter
Step 1: Find Measurement model update 𝑦! = 𝑔(𝑥! , 𝑢! ) For Linear State-‐space model: 𝑦! = 𝐶𝑥! + 𝐷𝑢! Step 2: Find the Estimator Error 𝑒! = 𝑦! − 𝑦! Step 3: Find the Aposteriori (Corrected) state estimate 𝑥! = 𝑥! + 𝐾𝑒! Where K is the Kalman Filter Gain. Use the steady state Kalman Gain or calculate the time-‐varying Kalman Gain. Step 4: Find the Apriori (Predicted) state estimate update 𝑥!!! = 𝑓(𝑥! , 𝑢! ) For Linear State-‐space model: 𝑥!!! = 𝐴𝑥! + 𝐵𝑢! Step 1-‐4 goes inside a loop in your program. Note! Different notation is used in different literature: Apriori (or Predicted) state estimate: 𝑥 or 𝑥! Aposteriori (or Corrected) state estimate: 𝑥 or 𝑥! With the simulated tank model we will implement our own discrete steady-‐state Kalman Filter in LabVIEW for estimation of 𝐹!"# (and ℎ). Use the state-‐space model found in the previous task. 𝑥 = 𝐴𝑥 + 𝐵𝑢 𝑦 = 𝐶𝑥 + 𝐷𝑢 Where 𝑥! = ℎ and 𝑥! = 𝐹!"# Create, e.g., the Kalman Filter Algorithm in a SubVI in LabVIEW.
State Estimation with Kalman Filter
36
Kalman Filter
Create a simulated model using, e.g., a “Simulation Subsystem” in LabVIEW that runs in parallel with the Estimator. A simple (linear) model is: ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
Where 𝑦 = ℎ. Add some white noise to the model (use one of the built-‐in functions in LabVIEW) This will be used to update (correct) the estimate in the Kalman Filter algorithm. → Plot the simulated results vs. the estimated results. Add noise in the simulated model to make it more realistic. Use, e.g., the “Gaussian White Noise PtByPt.vi”. If you have time and the results from the simulation is poor, you should consider using the more accurate and (nonlinear) model found here: http://home.hit.no/~finnh/dok_tankmodell/. I this task we will use the steady-‐state Kalman Gain. The following information is necessary in order to find the steady-‐state Kalman Gain:
State Estimation with Kalman Filter
37
Kalman Filter
Find the steady-‐state Kalman Gain using the built-‐in function available in LabVIEW (LabVIEW Design and Simulation Module) and Check for Observability. → Tune the Kalman Filter by adjusting the covariance matrices 𝑃 and 𝑄. Discuss the results. → Finally, test your algorithm on the real process. If you have time and the results from the simulation is poor, you should consider using the more accurate and (nonlinear) model found here: http://home.hit.no/~finnh/dok_tankmodell/.
Change the outflow by adjusting manually the outlet valve. What happens?
Solutions: Example 1 – Linear Kalman Filter: Front Panel:
Block Diagram:
State Estimation with Kalman Filter
38
Kalman Filter
The Kalman Filter Algorithm is implemented in a SubVI:
State Estimation with Kalman Filter
39
Kalman Filter
Example 2 – Using the Nonlinear model: Same as Example 1, but the Kalman Filter implementation uses the nonlinear model:
The Algorithm is as follows:
State Estimation with Kalman Filter
40
Kalman Filter
The system is given by: 𝑥! 𝑘 + 1 = 𝑥! 𝑘 −
𝑇! 𝐾! 𝑇! 𝑥! 𝑘 + 𝑢(𝑘) 𝐴! 𝐴!
𝑥! 𝑘 + 1 = 𝑥! 𝑘 𝑦 𝑘 = 𝑥! 𝑘 𝐾 is an (𝑛𝑥𝑟) matrix: 𝐾=
𝐾! 𝐾!
The algorithm then becomes: Initial values: 𝑥! 0 , 𝑥! (0) Step 1: Find Measurement estimate: 𝑦 𝑘 = 𝑥! (𝑘) Step 2: Find estimator error: 𝑒 𝑘 = 𝑦 𝑘 − 𝑦(𝑘) Step 3: Find the Corrected state estimate: 𝑥! 𝑘 = 𝑥! 𝑘 + 𝐾! 𝑒(𝑘) 𝑥! 𝑘 = 𝑥! 𝑘 + 𝐾! 𝑒(𝑘) Step 4: Find the Predicted state estimate: 𝑥! 𝑘 + 1 = 𝑥! 𝑘 −
𝑇! 𝐾! 𝑇! 𝑥! 𝑘 + 𝑢(𝑘) 𝐴! 𝐴!
𝑥! 𝑘 + 1 = 𝑥! 𝑘 Note! Different notation exists, e.g.: 𝑥 = 𝑥! 𝑥 = 𝑥!
State Estimation with Kalman Filter
5 Feedforward Control 5.1
Feedforward C ontroller
Design the Feedforward Controller. Use “Pen and Paper”. We have the following model: ℎ=
1 𝐾𝑢−𝐹!"# 𝐴
We will use this model in order to find the feedforward controller. In this model is 𝐹!"# a noise signal/disturbance that we want to remove by using Feedforward. We want to design the Feedforward controller so that 𝐹!"# is eliminated. Tip! Solve for the control variable 𝑢, and substituting the process output variable ℎ by its setpoint ℎ!" . 𝐹!"# is not measured, so you need to use the estimated value instead.
Solutions: We have the following model: ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
We will use this model in order to find the feedforward controller. In this model is 𝐹!"# a noise signal/disturbance that we want to remove by using Feedforward. We introduce the following parameters: 𝑢!"# -‐ Control signal from original feedback controller (PID controller) 𝑢! – Control signal from feedforward controller 𝑢 = 𝑢!"# + 𝑢! – Total control signal
41
42
Feedforward Control
We want to design the Feedforward controller so that 𝐹!"# is eliminated. We solve for the control variable 𝑢, and substituting the process output variable ℎ by its setpoint ℎ!" . This gives the following feedforward controller 𝑢! : 𝑢! =
𝐴ℎ!" 𝐹!"# + 𝐾! 𝐾!
We assume that the setpoint is constant, i.e. ℎ!" = 0. This gives: 𝑢! =
𝐹!"# 𝐾!
𝐹!"# is not measured, so we have to use the estimated value instead. This gives the following Feedforward controller: 𝒖𝒇𝒇 =
𝑭𝒐𝒖𝒕,𝒆𝒔𝒕 𝑲𝒑
Alternativ utledning (egentlig samme, men gjort litt anderledes): Gitt Modell: ℎ=
1 𝐾 𝑢−𝐹!"# 𝐴! !
Løser med tanke på 𝑢 (𝑢 → 𝑢!! ), erstatter ℎ med ℎ!" samt antar ℎ!" er konstant, dvs. ℎ!" = 0 Får da: ℎ!" =
1 𝐾 𝑢 −𝐹 = 0 𝐴! ! !! !"#
Som gir: 𝐾! 𝐹!"# 𝑢!! − = 0 𝐴! 𝐴! State Estimation with Kalman Filter
43
Feedforward Control
Dvs.: 𝑢!! =
𝐹!"# 𝐾!
Siden vi ikke måler 𝐹!"# , må vi bruke estimatet: 𝑢!! =
𝐹!"#,!"# 𝐾!
5.2 Implementing F eedforward Control Implement a level control system with a standard PI controller (Feedback control from the measured level) and a feedforward controller from the estimated flow. You should first test it on the simulator then on the real system afterwards. Start by using the simulator and then extend the program to make it easy to switch between the real process and the simulator. → Does the feedforward control improve the level control (compare with not using feedforward control, but only feedback control)? → You should make it possible to turn the feedforward controller on/off from the Front Panel so it is easy to see the difference. Note! Without feedforward control the control signal range of the PID controller is normally [0, 5]. With feedforward the output signal can be set to have the range [-‐5, +5], so the contribution 𝑢!"# from the PID controller can be negative. If 𝑢!"# cannot be negative, the total signal 𝑢 = 𝑢!"# + 𝑢! may not be small enough value to give proper control when the outflow is small.
Solutions: The control signal becomes: 𝑢 = 𝑢!"# + 𝑢!! LabVIEW: Front Panel:
State Estimation with Kalman Filter
44
Feedforward Control
Block Diagram:
State Estimation with Kalman Filter
Appendix A -‐Tuning the PI(D) Controller You should tune a PI controller for the process (including the measurement filter) using, e.g., Skogestad's method. Note! You cannot use the Good Gain method or the Ziegler-‐Nichols' method because the process has almost no time-‐delay. A reasonable process model is time-‐constant (with zero time-‐delay). The method is as follows: Try to find the gain and the time-‐constant of the process model from an experiment on the tank. (You can first set the control signal to 1.5 V and let the level stabilize. Then you apply a step change of the control signal of amplitude, e.g. 1 V. From this experiment you can calculate the time-‐constant and the gain.) In Skogestad's method you can try with a specified time-‐constant of the control system equal to TC = 10 sec. After you have calculated the PI parameters, set the controller in automatic mode, and check if the stability of the control system is ok (by applying a step change to the setpoint and looking at the response in the controlled variable). PID results: We set: Tc=10 sec and c=1.5. Originally, Skogestad defined the factor c in the table above to 4. This gives good setpoint tracking. But the disturbance compensation may become quite sluggish. To obtain faster disturbance compensation, you can use c=1.5. The drawback of such a reduction of c is that there will be more overshoot. Finding the process parameters: 𝐻 𝑠 =
ℎ(𝑠) 𝐾 = 𝑢(𝑠) 𝑇𝑠 + 1
From experiments as explained above: 45
46
Appendix A -‐Tuning the PI(D) Controller
K = 1.2 -‐4 T = 26-‐43 sec This gives the following PID parameters: Kp = 3 Ti = 15 Td = 0
State Estimation with Kalman Filter
Telemark University College Faculty of Technology Kjølnes Ring 56 N-‐3918 Porsgrunn, Norway www.hit.no Hans-‐Petter Halvorsen, M.Sc. Telemark University College Department of Electrical Engineering, Information Technology and Cybernetics E-‐mail:
[email protected] Blog: http://home.hit.no/~hansha/
