Skip to content

Commit 3af4c10

Browse files
DanielMartenssonDanielMartensson
authored
Update kf_qmpcExample.m - Update example with separate sample time
1 parent a355a02 commit 3af4c10

File tree

1 file changed

+106
-105
lines changed

1 file changed

+106
-105
lines changed

‎examples/kf_qmpcExample.m‎

Lines changed: 106 additions & 105 deletions
Original file line numberDiff line numberDiff line change
@@ -1,113 +1,114 @@
11
%% This is an oscillating mass-spring-damper system. Classical second order system
2-
% ---------------------------------------------
3-
% / / / / / / / / / / / / / / / / / / / / / / /
4-
% ---------------------------------------------
5-
% | |
6-
% | |
7-
% | |
8-
% | |
9-
% | |
10-
% | |
11-
% | | | /
12-
% | | | /
13-
% | | | \
14-
% | | | \
15-
% | | | \
16-
% | | | \
17-
% | | | b = 1.2 [Ns/m] / k = 10 [N/m]
18-
% |_|_| /
19-
% | | |
20-
% | | |
21-
% |_ _| |
22-
% | |
23-
% | |
24-
% | |
25-
% | |
26-
% ------------------------------------
27-
% | |
28-
% | |
29-
% | m = 10 [kg] |
30-
% | |---------
31-
% ------------------------------------ |
32-
% | |
33-
% | | x [m]
34-
% | V
35-
% V
36-
% F [N]
37-
%
38-
% System of equation (second order system):
39-
% m*ddx + b*dx + k*x = F
40-
%
41-
% State space (first order system):
42-
% dx1 = x2
43-
% dx2 = -x1*k/m - x2*b/m + F/m
44-
% [dx1] = [0 1]*[x1] + [0 ]
45-
% [dx2] = [-k/m -b/m]*[x2] + [1/m]*F
46-
% dx A x B u
47-
%
2+
% ---------------------------------------------
3+
% / / / / / / / / / / / / / / / / / / / / / / /
4+
% ---------------------------------------------
5+
% | |
6+
% | |
7+
% | |
8+
% | |
9+
% | |
10+
% | |
11+
% | | | /
12+
% | | | /
13+
% | | | \
14+
% | | | \
15+
% | | | \
16+
% | | | \
17+
% | | | b = 1.2 [Ns/m] / k = 10 [N/m]
18+
% |_|_| /
19+
% | | |
20+
% | | |
21+
% |_ _| |
22+
% | |
23+
% | |
24+
% | |
25+
% | |
26+
% ------------------------------------
27+
% | |
28+
% | |
29+
% | m = 10 [kg] |
30+
% | |---------
31+
% ------------------------------------ |
32+
% | |
33+
% | | x [m]
34+
% | V
35+
% V
36+
% F [N]
37+
%
38+
% System of equation (second order system):
39+
% m*ddx + b*dx + k*x = F
40+
%
41+
% State space (first order system):
42+
% dx1 = x2
43+
% dx2 = -x1*k/m - x2*b/m + F/m
44+
% [dx1] = [0 1]*[x1] + [0 ]
45+
% [dx2] = [-k/m -b/m]*[x2] + [1/m]*F
46+
% dx A x B u
47+
%
4848

4949

50-
%% Build the system plant model (The real world system plant model which we don't know)
51-
m = 10; % Weight of the mass (mandatory)
52-
k = 10; % Spring force (mandatory)
53-
b = 2.2; % Damper force (mandatory)
54-
A = [0 1; -k/m -b/m]; % System matrix (mandatory)
55-
B = [0; 1/m]; % Input matrix (mandatory)
56-
C = [2 0]; % Output matrix (mandatory)
57-
delay = 0;
58-
sysp = mc.ss(delay, A, B, C);
50+
%% Build the system plant model (The real world system plant model which we don't know)
51+
m = 10; % Weight of the mass (mandatory)
52+
k = 10; % Spring force (mandatory)
53+
b = 2.2; % Damper force (mandatory)
54+
A = [0 1; -k/m -b/m]; % System matrix (mandatory)
55+
B = [0; 1/m]; % Input matrix (mandatory)
56+
C = [2 0]; % Output matrix (mandatory)
57+
delay = 0;
58+
sysp = mc.ss(delay, A, B, C);
5959

60-
%% Build the system controller model (The estimated real world model which is very similar to the real world system plant model)
61-
m = 13.5; % Weight of the mass (mandatory)
62-
k = 8.7; % Spring force (mandatory)
63-
b = 3.1; % Damper force (mandatory)
64-
A = [0 1; -k/m -b/m]; % System matrix (mandatory)
65-
B = [0; 1/m]; % Input matrix (mandatory)
66-
C = [0.5 0]; % Output matrix (mandatory)
67-
delay = 0;
68-
sysc = mc.ss(delay, A, B, C);
60+
%% Build the system controller model (The estimated real world model which is very similar to the real world system plant model)
61+
m = 13.5; % Weight of the mass (mandatory)
62+
k = 8.7; % Spring force (mandatory)
63+
b = 3.1; % Damper force (mandatory)
64+
A = [0 1; -k/m -b/m]; % System matrix (mandatory)
65+
B = [0; 1/m]; % Input matrix (mandatory)
66+
C = [0.5 0]; % Output matrix (mandatory)
67+
delay = 0;
68+
sysc = mc.ss(delay, A, B, C);
6969

70-
%% Create the MPC parameters
71-
N = 20; % Control horizon (mandatory)
72-
r = 10; % Reference (mandatory)
73-
umin = 0; % Minimum input value on u (mandatory)
74-
umax = 60; % Maximum input value on u (mandatory)
75-
zmin = -1; % Minimum output value on y (mandatory)
76-
zmax = 100; % Maximum output value on y (mandatory)
77-
deltaumin = -30; % Minimum rate of change on u (mandatory)
78-
deltaumax = 30; % Maximum rate of change on u (mandatory)
79-
antiwindup = 100; % Limits for the 1/s integral (mandatory)
80-
lambda = 0.1; % Integral rate (optional)
81-
Ts = 1; % Sample time for both models (optional)
82-
T = 500; % End time for the simulation (optional)
83-
x0 = [0; 0]; % Intial state for the models sysp and sysc (optional)
84-
s = 1; % Regularization parameter (Faster to solve QP-problem) (optional)
85-
Qz = 1; % Weight parameter for QP-solver (optional)
86-
qw = 1; % Disturbance tuning for Kalman-Bucy filter (optional)
87-
rv = 0.1; % Noise tuning for Kalman-Bucy filter (optional)
88-
Spsi_spsi = 1; % Slackvariable tuning for H matrix and gradient g (optional)
89-
d = 3; % Disturbance signal at step iteration k = 0 (optional)
90-
E = [0; 0]; % Disturbance signal matrix for both sysp and sysc (optional)
70+
%% Create the MPC parameters
71+
N = 20; % Control horizon (mandatory)
72+
r = 10; % Reference (mandatory)
73+
umin = 0; % Minimum input value on u (mandatory)
74+
umax = 60; % Maximum input value on u (mandatory)
75+
zmin = -1; % Minimum output value on y (mandatory)
76+
zmax = 100; % Maximum output value on y (mandatory)
77+
deltaumin = -30; % Minimum rate of change on u (mandatory)
78+
deltaumax = 30; % Maximum rate of change on u (mandatory)
79+
antiwindup = 100; % Limits for the 1/s integral (mandatory)
80+
alpha = 0.1; % Integral rate (optional)
81+
Ts_mpc = 10; % Sample time for MPC. Total control horizon: Ts_mpc + N (optional)
82+
Ts_kf = 1; % Sample time for KF (and also plant) (optional)
83+
T = 500; % End time for the simulation (optional)
84+
x0 = [0; 0]; % Intial state for the models sysp and sysc (optional)
85+
s = 1; % Regularization parameter (Faster to solve QP-problem) (optional)
86+
Qz = 1; % Weight parameter for QP-solver (optional)
87+
qw = 1; % Disturbance tuning for Kalman-Bucy filter (optional)
88+
rv = 0.1; % Noise tuning for Kalman-Bucy filter (optional)
89+
Spsi_spsi = 1; % Slackvariable tuning for H matrix and gradient g (optional)
90+
d = 3; % Disturbance signal at step iteration k = 0 (optional)
91+
E = [0; 0]; % Disturbance signal matrix for both sysp and sysc (optional)
9192

92-
%% Run MPC with Kalman-bucy filter
93-
% d
94-
% |
95-
% |
96-
% ____________ _____V_______
97-
% + _ | | | |
98-
% ---r--|---|_|--R-->| MPC + KF |-------u------>| PLANT |----------y------>
99-
% | +Λ |____________| |____________| |
100-
% | | ___ |
101-
% | | | 1 | _ _ - |
102-
% | -------------------------- - |<----|λ|<----|_|<---------------
103-
% | | s | - |+
104-
% | --- |
105-
% -------------------------------------------------
93+
%% Run MPC with Kalman-bucy filter
94+
% d
95+
% |
96+
% |
97+
% ____________ _____V_______
98+
% + _ | | | |
99+
% ---r--|---|_|--R-->| MPC + KF |-------u------>| PLANT |----------y------>
100+
% | +Λ |____________| |____________| |
101+
% | | ___ |
102+
% | | | 1 | _ _ - |
103+
% | -------------------------- - |<----|λ|<----|_|<---------------
104+
% | | s | - |+
105+
% | --- |
106+
% -------------------------------------------------
106107

107-
[Y, T, X, U] = mc.kf_qmpc(sysp, sysc, N, r, umin, umax, zmin, zmax, deltaumin, deltaumax, antiwindup, lambda, Ts, T, x0, s, Qz, qw, rv, Spsi_spsi, d, E);
108+
[Y, T, X, U] = mc.kf_qmpc(sysp, sysc, N, r, umin, umax, zmin, zmax, deltaumin, deltaumax, antiwindup, alpha, Ts_mpc, Ts_kf, T, x0, s, Qz, qw, rv, Spsi_spsi, d, E);
108109

109-
% Print the output
110-
R = r*ones(length(T));
111-
plot(T, U, T, Y, T, R, 'r--');
112-
legend('Input', 'Output', 'Reference')
113-
grid on
110+
% Print the output
111+
R = r*ones(length(T));
112+
plot(T, U, T, Y, T, R, 'r--');
113+
legend('Input', 'Output', 'Reference')
114+
grid on

0 commit comments

Comments
 (0)