-
Notifications
You must be signed in to change notification settings - Fork 0
/
dynamic_system1.m
106 lines (73 loc) · 2.64 KB
/
dynamic_system1.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
%% Mass-spring-damper dynamic system
clear
clc
% System parameter initialization
m = 10;
b = 0.3;
k = 1.5;
% Initial state
x0(1) = 0;
x0(2) = 0;
% External force
u = @(t) 10 * sin(3*t) + 5;
% Simulation & measurements characteristics
resolution = 0.1;
t_span = 0:resolution:10;
%% Simulate & collect input-output measurements
% Solve the ODE for every t
[t, state] = ode15s(@(t, state)dynamics(t, state, m, b, k, u), t_span, x0);
% Input-output
U = u(t(:));
Y = state(:, 1);
%% Mean Squares Method
% Poles of the stable filter
syms m_ b_ k_;
p = [0.1, 0.4];
% Parameters vector, after linear factorization
theta = [b_ / m_ - (p(1) + p(2)); k_ / m_ - (p(1) * p(2)); 1 / m_];
% Input vector, after linear factorization
zeta1 = lsim(tf([-1, 0], [1, p(1) + p(2), p(1) * p(2)]), state(:, 1), t);
zeta2 = lsim(tf([-1], [1, p(1) + p(2), p(1) * p(2)]), state(:, 1), t);
zeta3 = lsim(tf([1], [1, p(1) + p(2), p(1) * p(2)]), U, t);
zeta = [zeta1, zeta2, zeta3]';
% Estimation of the unknown parameters
to_solve = theta == compute_optimal(state, zeta);
solved = solve(to_solve, [m_, b_, k_]);
m_ = double(solved.m_);
b_ = double(solved.b_);
k_ = double(solved.k_);
%% Result evaluation & deviation from the real model
% Output of the approximated model
[t, state_approx] = ode15s(@(t, state_approx) dynamics(t, state_approx, ...
m_, b_, k_, u), t_span, x0);
error = zeros(length(state), 1);
for i = 1:length(state)
error(i) = abs((state(i) - state_approx(i)) / state(i));
end
%% Plots display
figure(1);
plot(t, error, 'Linewidth', 1);
ylabel('error: $\big| \frac{y(t) - \hat{y}(t)}{y(t)} \big|$', ...
'interpreter', 'latex', 'FontWeight', 'bold');
xlabel('$t(s)$', 'interpreter', 'latex', 'FontWeight', 'bold');
figure(2);
plot(t, state(:, 1), 'Linewidth', 1);
ylabel('output $y(t)$', 'interpreter', 'latex', 'FontWeight', 'bold');
xlabel('$t(s)$', 'interpreter', 'latex', 'FontWeight', 'bold');
%% Dynamics of the system
function dx = dynamics(t, x, m, b, k, u)
dx(1) = x(2);
dx(2) = (1 / m) * (-k * x(1) - b * x(2) + u(t));
dx = dx';
end
%% Optimal parameters computation via optimization of the MSE
function theta = compute_optimal(state, zeta)
sum_denom = 0;
sum_numer = 0;
num_samples = length(state);
for i = 1:num_samples
sum_denom = sum_denom + zeta(:, i) * zeta(:, i)';
sum_numer = sum_numer + zeta(:, i) * state(i, 1);
end
theta = sum_denom \ sum_numer;
end