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
|
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
131
132
133
134
135
136
137
138
139
140
141
142
|
+
-
+
+
+
+
+
+
-
+
+
-
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
|
$1_simpar; # Read in simulation parameters
$1_numpar; # Read in parameters
MTTilast = round(mttlast/mttdt); # Total number of steps
#Initialise
MTTt = 0.0;
[MTTu] = zero_vector($Nu); # Zero the input
if $Nx>0
[MTTx] = $1_state; # Read in initial state
[MTTx] = $1_state; # Read in initial state
else
MTTx = 0; # Dummy value
endif;
[MTTy] = $1_odeo(MTTx,MTTu,MTTt); # Evaluate initial output
[MTTu] = $1_input(MTTt,MTTx,MTTy); # Evaluate initial input
mtt_write(MTTt,MTTx,MTTy,$Nx,$Ny); # And write them
[mttAA] = zero_matrix($Nx); # Zero the A matrix
[mttAAx] = zero_vector($Nx); # Zero the AAx vector
if $Nx>0
[MTTx] = $1_switch(MTTx); # Switches
[MTTx] = $1_switch(MTTx); # Switches
endif;
if mttmethod==1 # Euler
MTTddt = mttdt/mttstepfactor; # The small sample interval
endif;
#Integration loop
for MTTit = 1:MTTilast
if mttmethod==1 # Euler
for MTTjt = 1:mttstepfactor
[MTTdx] = $1_ode(MTTx,MTTu,MTTt); # State derivative
[MTTx] = mtt_euler(MTTx,MTTdx,MTTddt,$Nx); # Euler update
[MTTx] = $1_switch(MTTx); # Switches
MTTt = MTTt + MTTddt;
endfor;
endif;
if mttmethod==2 # Implicit
[MTTdx] = $1_ode(MTTx,MTTu,MTTt); # State derivative
[mttAA,mttAAx] = $1_smx(MTTx,MTTu,mttdt); # (I-Adt) and (I-Adt)x
[mttAA] = $1_switcha(mttAA,MTTx); # Switches
[MTTx] = mtt_implicit(MTTx,MTTdx,mttAA,mttAAx,mttdt,$Nx); # Implicit update
[MTTx] = $1_switch(MTTx); # Switches
MTTt = MTTt + mttdt;
endif;
|