-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathDRS_DAE2.m
563 lines (540 loc) · 26.1 KB
/
DRS_DAE2.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
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
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
classdef DRS_DAE2 < DAC_ODEclass
properties (SetAccess = protected, Hidden = true)
g ;
m1 ;
m3 ;
psi;
%LF;
anc;
LFb;
LP ;
%Iz1;
Iz3;
end
methods
function self = DRS_DAE2(g,m1,m3,psi,anc,LFb,LP,Iz3)
neq = 7; %number of equations
ninv = 0; %number of hidden constraints
self@DAC_ODEclass('DRS_DAE',neq,ninv);
%setup of the parameter of the ODE
self.g= g;
self.m1 = m1;
self.m3 = m3;
self.psi = psi;
%LF = self.LF;
self.anc = anc;
self.LFb = LFb;
self.LP = LP;
%Iz1 = self.Iz1;
self.Iz3 = Iz3;
end
function res__f = f( self, T, vars__ )
g = self.g ;
m1 = self.m1 ;
m3 = self.m3 ;
psi = self.psi;
anc = self.anc;
LFb = self.LFb;
LP = self.LP ;
Iz3 = self.Iz3;
% extract states
s = vars__(1);
theta1 = vars__(2);
theta3 = vars__(3);
s__dot = vars__(4);
theta3__dot = vars__(5);
lambda1 = vars__(6);
lambda2 = vars__(7);
% evaluate function
res__1 = s__dot;
t1 = cos(theta3);
t5 = cos(theta1);
res__2 = 0.1e1 / t5 / LP * theta3__dot * anc * t1;
res__3 = 0.1e1 * theta3__dot;
t10 = T ^ 2;
t12 = t10 ^ 2;
t13 = t12 ^ 2;
t14 = t13 ^ 2;
t17 = t10 * T;
t22 = t12 * T;
t25 = t12 * t10;
t28 = t12 * t17;
t35 = T * (-0.842440939932290348e4) + t10 * 0.383430217499691993e7 + t14 * t10 * 0.482444575016428782e27 + t14 * t17 * (-0.688070945275634472e27) + t14 * t12 * 0.459999031476182025e27 + t13 * t22 * (-0.331015878264927398e24) + t13 * t25 * 0.246747971203868537e25 + t13 * t28 * (-0.143677885840555936e26) + t14 * 0.638797916093246213e26 + t14 * T * (-0.210948396965421712e27) + t25 * 0.362875310128658500e15;
t50 = -0.751955623198760350e4 + t28 * (-0.14533139800998720e17) + t13 * 0.445733172871705600e18 + t13 * T * (-0.106507163441203282e20) + t13 * t10 * 0.199409961230139916e21 + t13 * t17 * (-0.296788011655680020e22) + t13 * t12 * 0.350712551358880285e23 + t17 * (-0.792218460298014641e9) + t12 * 0.875421334421550293e11 + t22 * (-0.668171233756255469e13) + 4 * lambda1;
res__4 = 0.1e1 / m1 * (t35 + t50);
t53 = theta3 ^ 2;
t54 = t53 * theta3;
t55 = 0.592621945e4 * t54;
t56 = 0.5740478308e5 * t53;
t57 = 0.182598982e6 * theta3;
t58 = g * m3;
t59 = 0.1e1 * t58;
t62 = cos(psi);
t64 = 0.474097556e4 * t54;
t68 = LFb * (t64 + t53 * (-0.490267877100000042e5) + theta3 * 0.167958569199998077e6 - 0.190914737699991150e6);
t69 = sin(psi);
t81 = sin(theta3);
t87 = LFb ^ 2;
t88 = t87 * m3;
res__5 = 0.1e1 / (t88 + Iz3) * (t1 * (t62 * LFb * (t55 - t56 + t57 - t59 - 0.1908391204e6) + t69 * t68 - 0.1e1 * anc * lambda2) + t81 * (t62 * t68 + t69 * LFb * (-t55 + t56 - t57 + t58 + 0.1908391204e6) + lambda1 * anc) - 0.4266877991e3 * t54 + t53 * 0.433173388900000373e4 + theta3 * (-0.145530395300000237e5) + 0.161877700100000457e5);
t91 = 0.6096051475e5 * t53;
t92 = 0.2071123758e6 * theta3;
t94 = LFb * (0.2328287627e6 + t58 - t55 + t91 - t92);
t95 = LP ^ 2;
t96 = t69 * t95;
t100 = t64 + t53 * (-0.445821231199999966e5) + theta3 * 0.139256177600000025e6 - 0.145264992200000037e6;
t101 = LFb * t100;
t102 = t62 * t95;
t104 = lambda1 * t95;
t105 = anc * t104;
t106 = t102 * t101 + t96 * t94 + t105;
t107 = t1 ^ 2;
t110 = -LFb * t100;
t113 = lambda2 * t95;
t114 = anc * t113;
t117 = theta3__dot ^ 2;
t118 = Iz3 * t117;
t120 = m3 * t117;
t125 = -0.25e0 * t118 - 0.25e0 * t87 * t120 - 0.3638259882e4 - 0.3200158492e3 * t53 + 0.2165866944e4 * theta3;
t126 = t95 * t125;
t132 = -0.121408275100000028e5 + t53 * (-0.324880041799999981e4) + theta3 * 0.109147796500000004e5 + 0.3200158492e3 * t54;
t133 = t95 * t132;
t138 = 0.75e0 * t58;
t139 = 0.4444664588e4 * t54 - 0.4305358732e5 * t53 + 0.1369492365e6 * theta3 - t138 - 0.1431293403e6;
t140 = LFb * t139;
t145 = -0.355573167e4 * t54 + t53 * 0.367700907800000059e5 + theta3 * (-0.125968926899998565e6) + 0.143186053299993364e6;
t146 = LFb * t145;
t148 = 0.75e0 * t105;
t150 = t5 ^ 2;
t151 = t150 ^ 2;
t152 = t151 * t150;
t154 = t107 * t1;
t155 = t154 * LP;
t156 = anc ^ 2;
t157 = t156 * lambda1;
t159 = 0.25e0 * t157 * t155;
t162 = LFb * (t55 - t91 + t92 - t59 - 0.2328287627e6);
t165 = t96 * t101 + t102 * t162 - 0.1e1 * t114;
t166 = sin(theta1);
t168 = t81 * LP;
t169 = t156 * lambda2;
t176 = -t95 * t132;
t185 = LFb * (-0.118524389e4 * t54 + t53 * 0.781203233999999793e4 + theta3 * (-0.132872507600000008e5) + 0.207893892499999993e4);
t192 = LFb * (0.25e0 * t58 + 0.896994224e5 - 0.1481554862e4 * t54 + 0.1790692744e5 * t53 - 0.7016313935e5 * theta3);
t198 = t151 * t5;
t200 = t156 * t166;
t201 = lambda2 * t155;
t204 = lambda1 * t168;
t214 = -t95 * t125;
t219 = -LFb * t139;
t222 = -LFb * t145;
t227 = lambda2 * t81;
t232 = t150 * t5;
t234 = anc * t53;
t236 = theta3 * anc;
t239 = anc * t54;
t242 = LFb * (0.3677009078e5 * t234 - 0.1259689269e6 * t236 + 0.1431860533e6 * anc - 0.355573167e4 * t239);
t243 = t69 * LP;
t245 = 0.4444664588e4 * t239;
t246 = 0.430535873e5 * t234;
t247 = 0.1369492365e6 * t236;
t248 = t138 + 0.1431293403e6;
t251 = LFb * (anc * t248 - t245 + t246 - t247);
t252 = t62 * LP;
t254 = lambda2 * LP;
t260 = 0.75e0 * t120 * t156 * t87;
t263 = 0.75e0 * t117 * t156 * Iz3;
t269 = LFb * (-anc * t248 + t245 - t246 + t247);
t272 = lambda1 * LP;
t282 = LP * (-0.1214082751e5 * anc - 0.3248800418e4 * t234 + 0.1091477965e5 * t236 + 0.3200158492e3 * t239);
t293 = LP * (0.75e0 * anc * t118 + 0.75e0 * anc * m3 * t87 * t117);
t295 = t1 * t166 * t81;
t299 = -t260 - t263;
t308 = m1 * t156;
t312 = (0.2e1 * m1 * t107 * t156 - 0.4e1 * t88 - 0.1e1 * t308 - 0.4e1 * Iz3) * t95;
t315 = t156 * t95;
t316 = t1 * t315;
t317 = t166 ^ 2;
t324 = t166 * m1;
res__6 = 0.4000000000e1 / (t198 * t166 * t312 + 0.2e1 * t151 * t81 * t317 * m1 * t316 - 0.1e1 * t232 * t324 * t107 * t315) * theta3__dot * anc * m1 * (t152 * (t107 * t106 + t1 * (t81 * (t102 * t94 + t96 * t110 + t114) + t126) + t81 * t133 + t96 * t140 + t102 * t146 - t148) + t198 * (-t159 + t107 * (t166 * t165 + 0.25e0 * t169 * t168) + t1 * t166 * (t81 * t106 + t176) + t166 * (t81 * t126 + t96 * t185 + t102 * t192 + 0.25e0 * t114)) + t151 * (-0.25e0 * t201 * t200 + t107 * (-0.25e0 * t204 * t200 + t96 * t162 + t102 * t110 - 0.1e1 * t105) + t1 * (t81 * t165 + t214) + t81 * t176 + t96 * t219 + t102 * t222 + t148) + t232 * (t159 - 0.25e0 * t156 * t227 * t107 * LP) + t150 * (t154 * (t166 * (t156 * t254 + t243 * t242 + t252 * t251) + t260 + t263) + t107 * t166 * (t81 * (t243 * t269 + t252 * t242 - 0.75e0 * t156 * t272) + t282) + t295 * t293) + t154 * t299);
t332 = m1 * t95;
t333 = t69 * t332;
t335 = t62 * t332;
t337 = anc * m1;
t338 = t337 * t104;
t339 = t335 * t101 + t333 * t94 + t338;
t343 = t337 * t113;
t349 = t81 * m1;
t353 = 0.75e0 * t338;
t362 = t333 * t101 + t335 * t162 - 0.1e1 * t343;
t373 = t308 * t272;
t392 = t156 * t324;
t402 = t308 * t254;
t427 = LP * m1;
t428 = t69 * t427;
t430 = t62 * t427;
res__7 = 0.4000000000e1 / (t152 * t312 + 0.2e1 * t198 * t81 * t324 * t316 - 0.1e1 * t151 * m1 * t107 * t315) * theta3__dot * (t152 * (t107 * t339 + t1 * (t81 * (t333 * t110 + t335 * t94 + t343) + m1 * t126) + t349 * t133 + t333 * t140 + t335 * t146 - t353) + t198 * (0.25e0 * m1 * t157 * t155 + t107 * (t166 * t362 - 0.25e0 * m1 * t169 * t168) + t1 * (t166 * (m1 * t176 + t81 * t339) - 0.25e0 * t373 + LP * (-0.1e1 * lambda1 * Iz3 - 0.1e1 * t87 * m3 * lambda1)) + t166 * (t349 * t126 + t333 * t185 + t335 * t192 + 0.25e0 * t343)) + t151 * (0.25e0 * t201 * t392 + t107 * (0.25e0 * t204 * t392 + t333 * t162 + t335 * t110 - 0.1e1 * t338) + t1 * (t166 * (-0.25e0 * t402 + LP * (-0.1e1 * lambda2 * Iz3 - 0.1e1 * t87 * m3 * lambda2)) + t81 * t362 + m1 * t214) + t349 * t176 + t333 * t219 + t335 * t222 + t353) + 0.25e0 * t308 * t227 * t107 * t232 * LP + t150 * (t154 * (t166 * (t428 * t242 + t430 * t251 + 0.75e0 * t402) - m1 * t299) + t107 * t166 * (t81 * (t428 * t269 + t430 * t242 - 0.75e0 * t373) + m1 * t282) + t295 * m1 * t293) + t154 * m1 * t299) * anc;
% store on output
res__f = zeros(7,1);
res__f(1) = res__1;
res__f(2) = res__2;
res__f(3) = res__3;
res__f(4) = res__4;
res__f(5) = res__5;
res__f(6) = res__6;
res__f(7) = res__7;
end
function res__DfDx = DfDx( self, T, vars__ )
g = self.g ;
m1 = self.m1 ;
m3 = self.m3 ;
psi = self.psi;
anc = self.anc;
LFb = self.LFb;
LP = self.LP ;
Iz3 = self.Iz3;
% extract states
s = vars__(1);
theta1 = vars__(2);
theta3 = vars__(3);
s__dot = vars__(4);
theta3__dot = vars__(5);
lambda1 = vars__(6);
lambda2 = vars__(7);
% evaluate function
res__1_1 = s__dot;
t1 = cos(theta3);
t5 = cos(theta1);
res__2_1 = 0.1e1 / t5 / LP * theta3__dot * anc * t1;
res__3_1 = 0.1e1 * theta3__dot;
t10 = T ^ 2;
t12 = t10 ^ 2;
t13 = t12 ^ 2;
t14 = t13 ^ 2;
t17 = t10 * T;
t22 = t12 * T;
t25 = t12 * t10;
t28 = t12 * t17;
t35 = T * (-0.842440939932290348e4) + t10 * 0.383430217499691993e7 + t14 * t10 * 0.482444575016428782e27 + t14 * t17 * (-0.688070945275634472e27) + t14 * t12 * 0.459999031476182025e27 + t13 * t22 * (-0.331015878264927398e24) + t13 * t25 * 0.246747971203868537e25 + t13 * t28 * (-0.143677885840555936e26) + t14 * 0.638797916093246213e26 + t14 * T * (-0.210948396965421712e27) + t25 * 0.362875310128658500e15;
t50 = -0.751955623198760350e4 + t28 * (-0.14533139800998720e17) + t13 * 0.445733172871705600e18 + t13 * T * (-0.106507163441203282e20) + t13 * t10 * 0.199409961230139916e21 + t13 * t17 * (-0.296788011655680020e22) + t13 * t12 * 0.350712551358880285e23 + t17 * (-0.792218460298014641e9) + t12 * 0.875421334421550293e11 + t22 * (-0.668171233756255469e13) + 4 * lambda1;
res__4_1 = 0.1e1 / m1 * (t35 + t50);
t53 = theta3 ^ 2;
t54 = t53 * theta3;
t55 = 0.592621945e4 * t54;
t56 = 0.5740478308e5 * t53;
t57 = 0.182598982e6 * theta3;
t58 = g * m3;
t59 = 0.1e1 * t58;
t62 = cos(psi);
t64 = 0.474097556e4 * t54;
t68 = LFb * (t64 + t53 * (-0.490267877100000042e5) + theta3 * 0.167958569199998077e6 - 0.190914737699991150e6);
t69 = sin(psi);
t81 = sin(theta3);
t87 = LFb ^ 2;
t88 = t87 * m3;
res__5_1 = 0.1e1 / (t88 + Iz3) * (t1 * (t62 * LFb * (t55 - t56 + t57 - t59 - 0.1908391204e6) + t69 * t68 - 0.1e1 * anc * lambda2) + t81 * (t62 * t68 + t69 * LFb * (-t55 + t56 - t57 + t58 + 0.1908391204e6) + lambda1 * anc) - 0.4266877991e3 * t54 + t53 * 0.433173388900000373e4 + theta3 * (-0.145530395300000237e5) + 0.161877700100000457e5);
t91 = 0.6096051475e5 * t53;
t92 = 0.2071123758e6 * theta3;
t94 = LFb * (0.2328287627e6 + t58 - t55 + t91 - t92);
t95 = LP ^ 2;
t96 = t69 * t95;
t100 = t64 + t53 * (-0.445821231199999966e5) + theta3 * 0.139256177600000025e6 - 0.145264992200000037e6;
t101 = LFb * t100;
t102 = t62 * t95;
t104 = lambda1 * t95;
t105 = anc * t104;
t106 = t102 * t101 + t96 * t94 + t105;
t107 = t1 ^ 2;
t110 = -LFb * t100;
t113 = lambda2 * t95;
t114 = anc * t113;
t117 = theta3__dot ^ 2;
t118 = Iz3 * t117;
t120 = m3 * t117;
t125 = -0.25e0 * t118 - 0.25e0 * t87 * t120 - 0.3638259882e4 - 0.3200158492e3 * t53 + 0.2165866944e4 * theta3;
t126 = t95 * t125;
t132 = -0.121408275100000028e5 + t53 * (-0.324880041799999981e4) + theta3 * 0.109147796500000004e5 + 0.3200158492e3 * t54;
t133 = t95 * t132;
t138 = 0.75e0 * t58;
t139 = 0.4444664588e4 * t54 - 0.4305358732e5 * t53 + 0.1369492365e6 * theta3 - t138 - 0.1431293403e6;
t140 = LFb * t139;
t145 = -0.355573167e4 * t54 + t53 * 0.367700907800000059e5 + theta3 * (-0.125968926899998565e6) + 0.143186053299993364e6;
t146 = LFb * t145;
t148 = 0.75e0 * t105;
t150 = t5 ^ 2;
t151 = t150 ^ 2;
t152 = t151 * t150;
t154 = t107 * t1;
t155 = t154 * LP;
t156 = anc ^ 2;
t157 = t156 * lambda1;
t159 = 0.25e0 * t157 * t155;
t162 = LFb * (t55 - t91 + t92 - t59 - 0.2328287627e6);
t165 = t96 * t101 + t102 * t162 - 0.1e1 * t114;
t166 = sin(theta1);
t168 = t81 * LP;
t169 = t156 * lambda2;
t176 = -t95 * t132;
t185 = LFb * (-0.118524389e4 * t54 + t53 * 0.781203233999999793e4 + theta3 * (-0.132872507600000008e5) + 0.207893892499999993e4);
t192 = LFb * (0.25e0 * t58 + 0.896994224e5 - 0.1481554862e4 * t54 + 0.1790692744e5 * t53 - 0.7016313935e5 * theta3);
t198 = t151 * t5;
t200 = t156 * t166;
t201 = lambda2 * t155;
t204 = lambda1 * t168;
t214 = -t95 * t125;
t219 = -LFb * t139;
t222 = -LFb * t145;
t227 = lambda2 * t81;
t232 = t150 * t5;
t234 = anc * t53;
t236 = theta3 * anc;
t239 = anc * t54;
t242 = LFb * (0.3677009078e5 * t234 - 0.1259689269e6 * t236 + 0.1431860533e6 * anc - 0.355573167e4 * t239);
t243 = t69 * LP;
t245 = 0.4444664588e4 * t239;
t246 = 0.430535873e5 * t234;
t247 = 0.1369492365e6 * t236;
t248 = t138 + 0.1431293403e6;
t251 = LFb * (anc * t248 - t245 + t246 - t247);
t252 = t62 * LP;
t254 = lambda2 * LP;
t260 = 0.75e0 * t120 * t156 * t87;
t263 = 0.75e0 * t117 * t156 * Iz3;
t269 = LFb * (-anc * t248 + t245 - t246 + t247);
t272 = lambda1 * LP;
t282 = LP * (-0.1214082751e5 * anc - 0.3248800418e4 * t234 + 0.1091477965e5 * t236 + 0.3200158492e3 * t239);
t293 = LP * (0.75e0 * anc * t118 + 0.75e0 * anc * m3 * t87 * t117);
t295 = t1 * t166 * t81;
t299 = -t260 - t263;
t308 = m1 * t156;
t312 = (0.2e1 * m1 * t107 * t156 - 0.4e1 * t88 - 0.1e1 * t308 - 0.4e1 * Iz3) * t95;
t315 = t156 * t95;
t316 = t1 * t315;
t317 = t166 ^ 2;
t324 = t166 * m1;
res__6_1 = 0.4000000000e1 / (t198 * t166 * t312 + 0.2e1 * t151 * t81 * t317 * m1 * t316 - 0.1e1 * t232 * t324 * t107 * t315) * theta3__dot * anc * m1 * (t152 * (t107 * t106 + t1 * (t81 * (t102 * t94 + t96 * t110 + t114) + t126) + t81 * t133 + t96 * t140 + t102 * t146 - t148) + t198 * (-t159 + t107 * (t166 * t165 + 0.25e0 * t169 * t168) + t1 * t166 * (t81 * t106 + t176) + t166 * (t81 * t126 + t96 * t185 + t102 * t192 + 0.25e0 * t114)) + t151 * (-0.25e0 * t201 * t200 + t107 * (-0.25e0 * t204 * t200 + t96 * t162 + t102 * t110 - 0.1e1 * t105) + t1 * (t81 * t165 + t214) + t81 * t176 + t96 * t219 + t102 * t222 + t148) + t232 * (t159 - 0.25e0 * t156 * t227 * t107 * LP) + t150 * (t154 * (t166 * (t156 * t254 + t243 * t242 + t252 * t251) + t260 + t263) + t107 * t166 * (t81 * (t243 * t269 + t252 * t242 - 0.75e0 * t156 * t272) + t282) + t295 * t293) + t154 * t299);
t332 = m1 * t95;
t333 = t69 * t332;
t335 = t62 * t332;
t337 = anc * m1;
t338 = t337 * t104;
t339 = t335 * t101 + t333 * t94 + t338;
t343 = t337 * t113;
t349 = t81 * m1;
t353 = 0.75e0 * t338;
t362 = t333 * t101 + t335 * t162 - 0.1e1 * t343;
t373 = t308 * t272;
t392 = t156 * t324;
t402 = t308 * t254;
t427 = LP * m1;
t428 = t69 * t427;
t430 = t62 * t427;
res__7_1 = 0.4000000000e1 / (t152 * t312 + 0.2e1 * t198 * t81 * t324 * t316 - 0.1e1 * t151 * m1 * t107 * t315) * theta3__dot * (t152 * (t107 * t339 + t1 * (t81 * (t333 * t110 + t335 * t94 + t343) + m1 * t126) + t349 * t133 + t333 * t140 + t335 * t146 - t353) + t198 * (0.25e0 * m1 * t157 * t155 + t107 * (t166 * t362 - 0.25e0 * m1 * t169 * t168) + t1 * (t166 * (m1 * t176 + t81 * t339) - 0.25e0 * t373 + LP * (-0.1e1 * lambda1 * Iz3 - 0.1e1 * t87 * m3 * lambda1)) + t166 * (t349 * t126 + t333 * t185 + t335 * t192 + 0.25e0 * t343)) + t151 * (0.25e0 * t201 * t392 + t107 * (0.25e0 * t204 * t392 + t333 * t162 + t335 * t110 - 0.1e1 * t338) + t1 * (t166 * (-0.25e0 * t402 + LP * (-0.1e1 * lambda2 * Iz3 - 0.1e1 * t87 * m3 * lambda2)) + t81 * t362 + m1 * t214) + t349 * t176 + t333 * t219 + t335 * t222 + t353) + 0.25e0 * t308 * t227 * t107 * t232 * LP + t150 * (t154 * (t166 * (t428 * t242 + t430 * t251 + 0.75e0 * t402) - m1 * t299) + t107 * t166 * (t81 * (t428 * t269 + t430 * t242 - 0.75e0 * t373) + m1 * t282) + t295 * m1 * t293) + t154 * m1 * t299) * anc;
% store on output
res__DfDx = zeros(7,1);
res__DfDx(1,1) = res__1_1;
res__DfDx(2,1) = res__2_1;
res__DfDx(3,1) = res__3_1;
res__DfDx(4,1) = res__4_1;
res__DfDx(5,1) = res__5_1;
res__DfDx(6,1) = res__6_1;
res__DfDx(7,1) = res__7_1;
end
function res__h = h( self, T, vars__ )
g = self.g ;
m1 = self.m1 ;
m3 = self.m3 ;
psi = self.psi;
anc = self.anc;
LFb = self.LFb;
LP = self.LP ;
Iz3 = self.Iz3;
% extract states
s = vars__(1);
theta1 = vars__(2);
theta3 = vars__(3);
s__dot = vars__(4);
theta3__dot = vars__(5);
lambda1 = vars__(6);
lambda2 = vars__(7);
% evaluate function
t1 = sin(theta1);
t3 = sin(theta3);
res__1 = t1 * LP - anc * t3 + y1 - y4;
t5 = cos(theta1);
t7 = cos(theta3);
res__2 = t5 * LP - anc * t7 + L1 + s + x1 - x4;
res__3 = (-lambda1 * t1 + lambda2 * t5) * LP;
res__4 = -s__dot - theta3__dot / t5 * anc * (-t7 * t1 + t3 * t5);
t19 = theta3 ^ 2;
t20 = t19 * theta3;
t21 = 0.592621945e4 * t20;
t22 = 0.5740478308e5 * t19;
t23 = 0.182598982e6 * theta3;
t24 = g * m3;
t28 = m1 * anc * (t21 - t22 + t23 - 0.1e1 * t24 - 0.1908391204e6);
t29 = LP * LFb;
t30 = sin(psi);
t31 = t30 * t29;
t36 = -0.474097556e4 * t20 + t19 * 0.490267877100000042e5 + theta3 * (-0.167958569199998077e6) + 0.190914737699991150e6;
t38 = m1 * anc * t36;
t39 = cos(psi);
t40 = t39 * t29;
t43 = anc ^ 2;
t44 = m1 * t43;
t47 = t31 * t28 + t40 * t38 - 0.1e1 * t44 * LP * lambda1;
t48 = t7 ^ 2;
t52 = -m1 * anc * t36;
t56 = t44 * LP * lambda2;
t60 = theta3__dot ^ 2;
t63 = LFb ^ 2;
t65 = t63 * m3 + Iz3;
t73 = 0.161877700100000038e5 - 0.4266877991e3 * t20 + t19 * 0.433173388900000009e4 + theta3 * (-0.145530395300000000e5);
t75 = LP * m1;
t80 = m1 * anc * (-t21 + t22 - t23 + t24 + 0.1908391204e6);
t83 = T ^ 2;
t84 = t83 ^ 2;
t85 = t84 ^ 2;
t86 = t85 ^ 2;
t87 = t86 * t84;
t90 = t83 * T;
t91 = t86 * t90;
t94 = t86 * t83;
t97 = t86 * T;
t102 = t84 * t90;
t103 = t85 * t102;
t106 = t84 * t83;
t107 = t85 * t106;
t110 = t84 * T;
t111 = t85 * t110;
t114 = t85 * t84;
t117 = t85 * t90;
t121 = t85 * t83;
t124 = t85 * T;
t144 = -0.7519556232e4 + 0.4e1 * lambda1;
t146 = 0.1994099612e21 * m3 * t121 - 0.1065071634e20 * m3 * t124 + 0.4457331729e18 * m3 * t85 - 0.1453313980e17 * m3 * t102 + 0.3628753101e15 * m3 * t106 - 0.6681712338e13 * m3 * t110 + 0.8754213344e11 * m3 * t84 - 0.7922184603e9 * m3 * t90 + 0.3834302175e7 * m3 * t83 - 0.8424409399e4 * T * m3 + m3 * t144;
t169 = t63 * (0.4599990315e27 * t87 * m3 - 0.6880709453e27 * m3 * t91 + 0.4824445750e27 * m3 * t94 - 0.2109483970e27 * m3 * t97 + 0.6387979161e26 * m3 * t86 - 0.1436778858e26 * m3 * t103 + 0.2467479712e25 * m3 * t107 - 0.3310158783e24 * m3 * t111 + 0.3507125514e23 * m3 * t114 - 0.2967880117e22 * m3 * t117 + t146) + m1 * t43 * lambda1 + 0.4599990315e27 * Iz3 * t87 - 0.6880709453e27 * Iz3 * t91 + 0.4824445750e27 * Iz3 * t94 - 0.2109483970e27 * Iz3 * t97 + 0.6387979161e26 * Iz3 * t86 - 0.1436778858e26 * Iz3 * t103 + 0.2467479712e25 * Iz3 * t107 - 0.3310158783e24 * Iz3 * t111 + 0.3507125514e23 * Iz3 * t114;
t193 = -0.2967880117e22 * Iz3 * t117 + 0.1994099612e21 * Iz3 * t121 - 0.1065071634e20 * Iz3 * t124 + 0.4457331729e18 * Iz3 * t85 - 0.1453313980e17 * Iz3 * t102 + 0.3628753101e15 * Iz3 * t106 - 0.6681712338e13 * Iz3 * t110 + 0.8754213344e11 * Iz3 * t84 - 0.7922184603e9 * Iz3 * t90 + 0.3834302175e7 * Iz3 * t83 - 0.8424409399e4 * T * Iz3 + Iz3 * t144;
t197 = t5 ^ 2;
t198 = t197 * t5;
res__5 = 0.1e1 / t198 / t65 / m1 / LP * (t198 * (t48 * t47 + t7 * (t3 * (t31 * t52 + t40 * t28 - 0.1e1 * t56) + t65 * anc * m1 * LP * t60) + t3 * t75 * anc * t73 + t31 * t80 + t40 * t52 + LP * (t169 + t193)) + t197 * (t48 * t1 * (t31 * t38 + t40 * t80 + t56) + t7 * t1 * (-t75 * anc * t73 + t3 * t47) + t65 * anc * t75 * t3 * t1 * t60) + t48 * (-0.1e1 * t60 * t43 * m1 * Iz3 - 0.1e1 * t60 * m3 * t43 * t63 * m1));
% store on output
res__h = zeros(5,1);
res__h(1) = res__1;
res__h(2) = res__2;
res__h(3) = res__3;
res__h(4) = res__4;
res__h(5) = res__5;
end
function res__DhDx = DhDx( self, T, vars__ )
g = self.g ;
m1 = self.m1 ;
m3 = self.m3 ;
psi = self.psi;
anc = self.anc;
LFb = self.LFb;
LP = self.LP ;
Iz3 = self.Iz3;
% extract states
s = vars__(1);
theta1 = vars__(2);
theta3 = vars__(3);
s__dot = vars__(4);
theta3__dot = vars__(5);
lambda1 = vars__(6);
lambda2 = vars__(7);
% evaluate function
t1 = cos(theta1);
res__1_2 = t1 * LP;
t2 = cos(theta3);
res__1_3 = -anc * t2;
res__2_1 = 1;
t4 = sin(theta1);
res__2_2 = -t4 * LP;
t6 = sin(theta3);
res__2_3 = anc * t6;
res__3_2 = (-lambda1 * t1 - lambda2 * t4) * LP;
res__3_6 = res__2_2;
res__3_7 = res__1_2;
t11 = t1 ^ 2;
res__4_2 = 0.1e1 / t11 * t2 * anc * theta3__dot;
t15 = t6 * t4;
t18 = 0.1e1 / t1;
res__4_3 = -theta3__dot * t18 * anc * (t1 * t2 + t15);
res__4_4 = -1;
t23 = -t6 * t1 + t2 * t4;
res__4_5 = t18 * anc * t23;
t25 = theta3 ^ 2;
t26 = LP * t25;
t27 = sin(psi);
t29 = t27 * LFb * t26;
t31 = cos(psi);
t33 = t31 * LFb * t26;
t36 = t2 ^ 2;
t42 = t25 * theta3;
t50 = t11 ^ 2;
t52 = 0.592621945e4 * t42;
t53 = 0.182598982e6 * theta3;
t54 = 0.57404783e5 * t25;
t55 = g * m3;
t58 = t31 * LP;
t60 = 0.474097556e4 * t42;
t64 = LFb * (-t60 + t25 * 0.490267876799999940e5 + theta3 * (-0.167958569199998077e6) + 0.190914737699991150e6);
t65 = t27 * LP;
t68 = LP * lambda2 * anc;
t72 = 0.1e1 * t55;
t77 = LP * lambda1 * anc;
t78 = 0.1e1 * t77;
t88 = theta3__dot ^ 2;
t90 = LFb ^ 2;
t91 = t90 * m3;
t92 = t91 + Iz3;
t97 = Iz3 * t88;
t98 = anc * t97;
t102 = anc * m3 * t90 * t88;
t109 = 0.1e1 / LP;
t110 = 0.1e1 / t92;
t111 = t110 * t109;
res__5_2 = 0.1e1 / t50 * t111 * anc * (t50 * (t36 * (0.3e-4 * t29 + 0.8e-4 * t33) + t2 * (t6 * (-0.8e-4 * t29 + 0.3e-4 * t33) + LP * (0.3e-6 * t42 + 0.3e-5 * t25))) + t11 * (t36 * (t58 * LFb * (0.1908391204e6 - t52 - t53 + t54 + t55) + t65 * t64 + t68) + t2 * (t6 * (t58 * t64 + t65 * LFb * (-t54 - t72 - 0.1908391204e6 + t52 + t53) - t78) + LP * (0.4266877988e3 * t42 + t25 * (-0.433173389199999929e4) + theta3 * 0.145530395299999982e5 - 0.161877700099999965e5)) + t92 * LP * t6 * t88) + t36 * t4 * (-0.3e1 * t98 - 0.3e1 * t102));
t114 = 0.2316257697e6 * theta3;
t117 = LFb * (0.274818405e6 - t52 - t114 + 0.645162464e5 * t25 + t55);
t121 = -t60 + t25 * 0.401374585399999924e5 + theta3 * (-0.110553786099999983e6) + 0.996152466999999888e5;
t122 = LFb * t121;
t123 = t65 * t122;
t127 = 0.6451624645e5 * t25;
t137 = LP * (0.2133438996e3 * t42 + t25 * (-0.216586694399999988e4) + theta3 * 0.727651976499999819e4 - 0.809388500500000009e4);
t146 = 0.5e0 * t90 * m3 * t88 + 0.6400316985e3 * t25 - 0.4331733889e4 * theta3 + 0.5e0 * t97 + 0.7276519765e4;
t149 = 0.2963109725e4 * t42;
t152 = 0.5e0 * t55;
t156 = 0.237048778e4 * t42;
t164 = t11 * t1;
t206 = 0.1e1 / t164;
res__5_3 = -0.2e1 * t206 * t111 * anc * (t164 * (t36 * (t58 * t117 + t123 + t68) + t2 * (t6 * (t58 * t122 + t65 * LFb * (-t127 - t72 - 0.274818405e6 + t52 + t114) - t78) + t137) + t6 * LP * t146 + t58 * LFb * (-0.1793988448e6 + t149 + 0.1403262787e6 * theta3 - 0.3581385488e5 * t25 - t152) + t65 * LFb * (-0.415787784999999894e4 + t156 + theta3 * 0.265745014999999948e5 + t25 * (-0.156240646799999959e5)) - 0.5e0 * t68) + t11 * (t36 * t4 * (-t58 * LFb * t121 + t65 * t117 + t77) + t2 * t4 * (t6 * (t58 * LFb * (-t52 + t127 - t114 + t55 + 0.274818405e6) + t123 + t68) - LP * t146) + t4 * (t6 * t137 + t58 * LFb * (-t156 + t25 * 0.245133938600000001e5 + theta3 * (-0.839792845999990241e5) + 0.954573688499955606e5) + t65 * LFb * (-0.954195602e5 + t149 - 0.2870239154e5 * t25 + 0.91299491e5 * theta3 - t152) - 0.5e0 * t77)) + t2 * t6 * (-0.1e1 * t98 - 0.1e1 * t102));
res__5_5 = t109 * t206 * anc * theta3__dot * (0.2e1 * t2 * t164 * LP + 0.2e1 * t15 * t11 * LP - 0.2e1 * t36 * anc);
t222 = anc ^ 2;
res__5_6 = t18 * t110 / m1 * (t1 * (-0.1e1 * m1 * t36 * t222 + 0.4e1 * t91 + m1 * t222 + 0.4e1 * Iz3) - 0.1e1 * t2 * t4 * m1 * t6 * t222);
res__5_7 = 0.1e1 * t18 * t110 * t2 * t23 * t222;
% store on output
res__DhDx = zeros(5,7);
res__DhDx(1,2) = res__1_2;
res__DhDx(1,3) = res__1_3;
res__DhDx(2,1) = res__2_1;
res__DhDx(2,2) = res__2_2;
res__DhDx(2,3) = res__2_3;
res__DhDx(3,2) = res__3_2;
res__DhDx(3,6) = res__3_6;
res__DhDx(3,7) = res__3_7;
res__DhDx(4,2) = res__4_2;
res__DhDx(4,3) = res__4_3;
res__DhDx(4,4) = res__4_4;
res__DhDx(4,5) = res__4_5;
res__DhDx(5,2) = res__5_2;
res__DhDx(5,3) = res__5_3;
res__DhDx(5,5) = res__5_5;
res__DhDx(5,6) = res__5_6;
res__DhDx(5,7) = res__5_7;
end
function plot(self, T, vars__)
end
end
end