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
|
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
|
-
+
-
+
+
+
-
+
-
-
+
-
+
+
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
|
endif
if nargin<6
weight = ones(size(y_s));
endif
if nargin<7
criterion = 1e-5;
criterion = 1e-7;
endif
if nargin <8
max_iterations = 10;
max_iterations = 25;
endif
if nargin<9
alpha = 1.0;
endif
if (!strcmp(method,"time"))&&(!strcmp(method,"freq"))
error("method must be either time or freq")
endif
[n_data,n_y] = size(y_s);
N_theta = length(free);
n_th = length(free);
Weight = weight*ones(1,N_theta); # Sensitivity weight
e_last = 1e20;
while (abs(e_last-error)>criterion)&&(iterations<max_iterations)
while (abs(error_old-error)>criterion)&&(iterations<max_iterations)
iterations = iterations + 1;
error_old_old = error_old;
e_last = error;
error_old = error;
eval(sprintf("[t,y,y_theta] = mtt_s%s(system_name,theta,free);",method)); # Simulate system
error = 0;
J = zeros(n_th,1);
JJ = zeros(n_th,n_th);
for i = 1:n_y
E = weight(:,i).*(y(:,i) - y_s(:,i)); # Weighted error
error = error + (E'*E); # Sum the error
Weight = weight(:,i)*ones(1,n_th); # Sensitivity weight
y_theta_w = Weight.*y_theta(:,i:n_y:n_y*n_th); # Weighted sensitivity
J = J + real(y_theta_w'*E); # Jacobian
JJ = JJ + real(y_theta_w'*y_theta_w); # Newton Euler approx Hessian
endfor
error = error
## Diagnostics
Error = [Error error]; # Save error
Theta = [Theta theta]; # Save parameters
Y = [Y y]; # Save output
E = weight.*(y - y_s); # Weighted error
y_theta = Weight.*y_theta; # Weighted sensitivity
error = (E'*E); # Sum the error
Error = [Error error];
## theta(free) = theta(free) - alpha*(real(y_theta'*y_theta)\real(y_theta'*E));
tol = 1e-4;
JJ = real(y_theta'*y_theta);
## sigma = svd(JJ)
theta(free) = theta(free) - alpha*( pinv(JJ,tol)*real(y_theta'*E) );
endwhile
|