SCL-RKF-code/generate_system_data.m at main · kaiodt/SCL-RKF-code · GitHub

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
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                         Generate System Data                        %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%                                                                         %
% * Description:                                                          %
%   - Generates the actual state evolution of the target system and       %
%     all sensor measurements.                                            %
%                                                                         %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%                                Setup                                %%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% ======================================================================= %
% Clear workspace
% ======================================================================= %

clc
clear
close all

% ======================================================================= %
% Simulation parameters
% ======================================================================= %

fprintf("--> Starting...\n");

example = 1;

N = 1000;
T = 1000;

% ======================================================================= %
% Get example initial parameters
% ======================================================================= %

fprintf("--> Defining simulation parameters for Example %d...\n", example);

k = 1;  % In case the system is time-varying, consider the initial step

addpath example_parameters;

filename = sprintf('example_parameters_%d', example);
eval(filename);

% ======================================================================= %
% Problem dimensions
% ======================================================================= %

n = size(F,1);  % System state
m = size(G,2);  % System input
p = size(H,2);  % System noise
r = size(C,1);  % Sensor measurements
q = size(D,2);  % Measurement noise

s1 = size(M1,2);
t1 = size(EF,1);
s2 = size(M2,2);
t2 = size(EC,1);

% ======================================================================= %
% Pre-allocation
% ======================================================================= %

% System state (all experiments)
x = zeros(n,N+2,T);

% System input (all experiments)
u = zeros(m,N+1,T);

% System noise (all experiments)
w = zeros(p,N+1,T);

% Sensor measurements (all experiments)
y = zeros(r,N+1,T);

% Measurement noise (all experiments)
v = zeros(q,N+1,T);

% Contraction matrices (all experiments)
Delta1 = zeros(s1,t1,N+1,T);
Delta2 = zeros(s2,t2,N+1,T);

% ======================================================================= %
% Data generation
% ======================================================================= %

fprintf("--> Generating system data...\n");

% Experiments loop
for e = 1:T

    fprintf('===== Experiment %d =====\n', e);

    % Initialize system state for current experiment
    x(:,1,e) = x0;

    % Time loop
    for k = 1:N+1
        % Get system parameters
        eval(filename);

        % Current system state
        x_k = x(:,k,e);

        % Update system input vector
        u(:,k,e) = u_k;

        % Update target system state
        [x(:,k+1,e), w(:,k,e), Delta1(:,:,k,e)] = ...
                                update_target_system(x_k,u_k,sys_model);

        % Obtain a measurement of the target system
        [y(:,k,e), v(:,k,e), Delta2(:,:,k,e)] = ...
                                        measure_target(x_k,sys_model);
    end
end

% ======================================================================= %
% Save data
% ======================================================================= %

filename = sprintf("saved_system_data/system_data_%d", example);

save(filename, 'x','u','w','y','v');
% save(filename, 'x','u','w','y','v','Delta1','Delta2');

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%