-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathDRS_DAE4.m
491 lines (475 loc) · 24.6 KB
/
DRS_DAE4.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
function res__f = f( self, t, vars__ )
% 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;
t11 = T ^ 2;
t13 = t11 * T;
t15 = t11 ^ 2;
t17 = t15 * T;
t19 = t15 * t11;
t21 = t15 * t13;
t23 = t15 ^ 2;
t27 = -0.751955623278408802e4 + T * (-0.842425904730574985e4) + 4 * lambda1 + t11 * 0.383423234576880559e7 + t13 * (-0.792204368426877022e9) + t15 * 0.875404664876312256e11 + t17 * (-0.668158137074275781e13) + t19 * 0.362867993008093500e15 + t21 * (-0.14532836515239424e17) + t23 * 0.445723582146351104e18 + t23 * T * (-0.106504805786393641e20);
t40 = t23 ^ 2;
t50 = t23 * t11 * 0.199405400642109833e21 + t23 * t13 * (-0.296781020442136674e22) + t23 * t15 * 0.350704036376488685e23 + t23 * t17 * (-0.331007654721895424e24) + t23 * t19 * 0.246741716116033256e25 + t23 * t21 * (-0.143674186916536294e26) + t40 * 0.638781271869255143e26 + t40 * T * (-0.210942894860188724e27) + t40 * t11 * 0.482431980518515082e27 + t40 * t13 * (-0.688053123266102803e27) + t40 * t15 * 0.459987288070949229e27;
res__4 = 0.1e1 / m1 * (t27 + 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);
t92 = 0.6096051475e5 * t53;
t93 = 0.2071123758e6 * theta3;
t95 = LFb * (0.2328287627e6 + t92 - t93 - t55 + t58);
t96 = LP ^ 2;
t97 = t69 * t96;
t101 = t64 + t53 * (-0.445821231199999966e5) + theta3 * 0.139256177600000025e6 - 0.145264992200000037e6;
t102 = LFb * t101;
t103 = t62 * t96;
t105 = lambda1 * t96;
t106 = anc * t105;
t107 = t103 * t102 + t97 * t95 + t106;
t108 = t1 ^ 2;
t111 = -LFb * t101;
t114 = lambda2 * t96;
t115 = anc * t114;
t118 = theta3__dot ^ 2;
t119 = Iz3 * t118;
t123 = m3 * t118;
t126 = -0.25e0 * t119 - 0.3200158492e3 * t53 + 0.2165866944e4 * theta3 - 0.25e0 * t87 * t123 - 0.3638259882e4;
t127 = t96 * t126;
t133 = theta3 * 0.109147796500000004e5 + 0.3200158492e3 * t54 + t53 * (-0.324880041799999981e4) - 0.121408275100000028e5;
t134 = t96 * t133;
t139 = 0.75e0 * t58;
t140 = 0.4444664588e4 * t54 - 0.4305358732e5 * t53 + 0.1369492365e6 * theta3 - 0.1431293403e6 - t139;
t141 = LFb * t140;
t146 = -0.355573167e4 * t54 + t53 * 0.367700907800000059e5 + theta3 * (-0.125968926899998565e6) + 0.143186053299993364e6;
t147 = LFb * t146;
t149 = 0.75e0 * t106;
t151 = t5 ^ 2;
t152 = t151 ^ 2;
t153 = t152 * t151;
t155 = t108 * t1;
t156 = t155 * LP;
t157 = anc ^ 2;
t158 = t157 * lambda1;
t160 = 0.25e0 * t158 * t156;
t163 = LFb * (t55 - t92 + t93 - 0.2328287627e6 - t59);
t166 = t97 * t102 + t103 * t163 - 0.1e1 * t115;
t167 = sin(theta1);
t169 = t81 * LP;
t170 = t157 * lambda2;
t177 = -t96 * t133;
t186 = LFb * (-0.118524389e4 * t54 + t53 * 0.781203233999999793e4 + theta3 * (-0.132872507600000008e5) + 0.207893892499999993e4);
t193 = LFb * (0.896994224e5 + 0.25e0 * t58 - 0.1481554862e4 * t54 + 0.1790692744e5 * t53 - 0.7016313935e5 * theta3);
t199 = t152 * t5;
t201 = t157 * t167;
t202 = lambda2 * t156;
t205 = lambda1 * t169;
t215 = -t96 * t126;
t220 = -LFb * t140;
t223 = -LFb * t146;
t227 = t108 * LP;
t228 = lambda2 * t81;
t233 = t151 * t5;
t236 = theta3 * anc;
t238 = anc * t54;
t240 = anc * t53;
t243 = LFb * (0.1431860533e6 * anc - 0.1259689269e6 * t236 - 0.355573167e4 * t238 + 0.3677009078e5 * t240);
t244 = t69 * LP;
t246 = 0.4444664588e4 * t238;
t247 = 0.430535873e5 * t240;
t248 = 0.1369492365e6 * t236;
t249 = 0.1431293403e6 + t139;
t252 = LFb * (anc * t249 - t246 + t247 - t248);
t253 = t62 * LP;
t255 = lambda2 * LP;
t261 = 0.75e0 * t123 * t157 * t87;
t264 = 0.75e0 * t118 * t157 * Iz3;
t270 = LFb * (-anc * t249 + t246 - t247 + t248);
t273 = lambda1 * LP;
t283 = LP * (-0.1214082751e5 * anc + 0.3200158492e3 * t238 - 0.3248800418e4 * t240 + 0.1091477965e5 * t236);
t294 = LP * (0.75e0 * anc * t119 + 0.75e0 * anc * m3 * t87 * t118);
t296 = t1 * t167 * t81;
t300 = -t261 - t264;
t308 = m1 * t157;
t312 = (0.2e1 * m1 * t108 * t157 - 0.4e1 * t88 - 0.1e1 * t308 - 0.4e1 * Iz3) * t96;
t315 = t157 * t96;
t316 = t1 * t315;
t317 = t167 ^ 2;
t324 = t167 * m1;
res__6 = 0.4000000000e1 / (t199 * t167 * t312 + 0.2e1 * t152 * t81 * t317 * m1 * t316 - 0.1e1 * t233 * t324 * t108 * t315) * anc * (t153 * (t108 * t107 + t1 * (t81 * (t103 * t95 + t97 * t111 + t115) + t127) + t81 * t134 + t97 * t141 + t103 * t147 - t149) + t199 * (-t160 + t108 * (t167 * t166 + 0.25e0 * t170 * t169) + t1 * t167 * (t81 * t107 + t177) + t167 * (t81 * t127 + t97 * t186 + t103 * t193 + 0.25e0 * t115)) + t152 * (-0.25e0 * t202 * t201 + t108 * (-0.25e0 * t205 * t201 + t97 * t163 + t103 * t111 - 0.1e1 * t106) + t1 * (t81 * t166 + t215) + t81 * t177 + t97 * t220 + t103 * t223 + t149) + t233 * (t160 - 0.25e0 * t157 * t228 * t227) + t151 * (t155 * (t167 * (t157 * t255 + t244 * t243 + t253 * t252) + t261 + t264) + t108 * t167 * (t81 * (t244 * t270 + t253 * t243 - 0.75e0 * t157 * t273) + t283) + t296 * t294) + t155 * t300) * m1 * theta3__dot;
t332 = m1 * t96;
t333 = t69 * t332;
t335 = t62 * t332;
t337 = anc * m1;
t338 = t337 * t105;
t339 = t335 * t102 + t333 * t95 + t338;
t343 = t337 * t114;
t349 = t81 * m1;
t353 = 0.75e0 * t338;
t362 = t333 * t102 + t335 * t163 - 0.1e1 * t343;
t373 = t308 * t273;
t392 = t157 * t324;
t402 = t308 * t255;
t426 = LP * m1;
t427 = t69 * t426;
t429 = t62 * t426;
res__7 = 0.4000000000e1 / (t153 * t312 + 0.2e1 * t199 * t81 * t324 * t316 - 0.1e1 * t152 * m1 * t108 * t315) * anc * (t153 * (t108 * t339 + t1 * (t81 * (t333 * t111 + t335 * t95 + t343) + m1 * t127) + t349 * t134 + t333 * t141 + t335 * t147 - t353) + t199 * (0.25e0 * m1 * t158 * t156 + t108 * (t167 * t362 - 0.25e0 * m1 * t170 * t169) + t1 * (t167 * (m1 * t177 + t81 * t339) - 0.25e0 * t373 + LP * (-0.1e1 * t87 * m3 * lambda1 - 0.1e1 * lambda1 * Iz3)) + t167 * (t349 * t127 + t333 * t186 + t335 * t193 + 0.25e0 * t343)) + t152 * (0.25e0 * t202 * t392 + t108 * (0.25e0 * t205 * t392 + t333 * t163 + t335 * t111 - 0.1e1 * t338) + t1 * (t167 * (-0.25e0 * t402 + LP * (-0.1e1 * t87 * m3 * lambda2 - 0.1e1 * lambda2 * Iz3)) + t81 * t362 + m1 * t215) + t349 * t177 + t333 * t220 + t335 * t223 + t353) + 0.25e0 * t308 * t228 * t233 * t227 + t151 * (t155 * (t167 * (t427 * t243 + t429 * t252 + 0.75e0 * t402) - m1 * t300) + t108 * t167 * (t81 * (t427 * t270 + t429 * t243 - 0.75e0 * t373) + m1 * t283) + t296 * m1 * t294) + t155 * m1 * t300) * theta3__dot;
% 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__ )
% 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;
t11 = T ^ 2;
t13 = t11 * T;
t15 = t11 ^ 2;
t17 = t15 * T;
t19 = t15 * t11;
t21 = t15 * t13;
t23 = t15 ^ 2;
t27 = -0.751955623278408802e4 + T * (-0.842425904730574985e4) + 4 * lambda1 + t11 * 0.383423234576880559e7 + t13 * (-0.792204368426877022e9) + t15 * 0.875404664876312256e11 + t17 * (-0.668158137074275781e13) + t19 * 0.362867993008093500e15 + t21 * (-0.14532836515239424e17) + t23 * 0.445723582146351104e18 + t23 * T * (-0.106504805786393641e20);
t40 = t23 ^ 2;
t50 = t23 * t11 * 0.199405400642109833e21 + t23 * t13 * (-0.296781020442136674e22) + t23 * t15 * 0.350704036376488685e23 + t23 * t17 * (-0.331007654721895424e24) + t23 * t19 * 0.246741716116033256e25 + t23 * t21 * (-0.143674186916536294e26) + t40 * 0.638781271869255143e26 + t40 * T * (-0.210942894860188724e27) + t40 * t11 * 0.482431980518515082e27 + t40 * t13 * (-0.688053123266102803e27) + t40 * t15 * 0.459987288070949229e27;
res__4_1 = 0.1e1 / m1 * (t27 + 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);
t92 = 0.6096051475e5 * t53;
t93 = 0.2071123758e6 * theta3;
t95 = LFb * (0.2328287627e6 + t92 - t93 - t55 + t58);
t96 = LP ^ 2;
t97 = t69 * t96;
t101 = t64 + t53 * (-0.445821231199999966e5) + theta3 * 0.139256177600000025e6 - 0.145264992200000037e6;
t102 = LFb * t101;
t103 = t62 * t96;
t105 = lambda1 * t96;
t106 = anc * t105;
t107 = t103 * t102 + t97 * t95 + t106;
t108 = t1 ^ 2;
t111 = -LFb * t101;
t114 = lambda2 * t96;
t115 = anc * t114;
t118 = theta3__dot ^ 2;
t119 = Iz3 * t118;
t123 = m3 * t118;
t126 = -0.25e0 * t119 - 0.3200158492e3 * t53 + 0.2165866944e4 * theta3 - 0.25e0 * t87 * t123 - 0.3638259882e4;
t127 = t96 * t126;
t133 = theta3 * 0.109147796500000004e5 + 0.3200158492e3 * t54 + t53 * (-0.324880041799999981e4) - 0.121408275100000028e5;
t134 = t96 * t133;
t139 = 0.75e0 * t58;
t140 = 0.4444664588e4 * t54 - 0.4305358732e5 * t53 + 0.1369492365e6 * theta3 - 0.1431293403e6 - t139;
t141 = LFb * t140;
t146 = -0.355573167e4 * t54 + t53 * 0.367700907800000059e5 + theta3 * (-0.125968926899998565e6) + 0.143186053299993364e6;
t147 = LFb * t146;
t149 = 0.75e0 * t106;
t151 = t5 ^ 2;
t152 = t151 ^ 2;
t153 = t152 * t151;
t155 = t108 * t1;
t156 = t155 * LP;
t157 = anc ^ 2;
t158 = t157 * lambda1;
t160 = 0.25e0 * t158 * t156;
t163 = LFb * (t55 - t92 + t93 - 0.2328287627e6 - t59);
t166 = t97 * t102 + t103 * t163 - 0.1e1 * t115;
t167 = sin(theta1);
t169 = t81 * LP;
t170 = t157 * lambda2;
t177 = -t96 * t133;
t186 = LFb * (-0.118524389e4 * t54 + t53 * 0.781203233999999793e4 + theta3 * (-0.132872507600000008e5) + 0.207893892499999993e4);
t193 = LFb * (0.896994224e5 + 0.25e0 * t58 - 0.1481554862e4 * t54 + 0.1790692744e5 * t53 - 0.7016313935e5 * theta3);
t199 = t152 * t5;
t201 = t157 * t167;
t202 = lambda2 * t156;
t205 = lambda1 * t169;
t215 = -t96 * t126;
t220 = -LFb * t140;
t223 = -LFb * t146;
t227 = t108 * LP;
t228 = lambda2 * t81;
t233 = t151 * t5;
t236 = theta3 * anc;
t238 = anc * t54;
t240 = anc * t53;
t243 = LFb * (0.1431860533e6 * anc - 0.1259689269e6 * t236 - 0.355573167e4 * t238 + 0.3677009078e5 * t240);
t244 = t69 * LP;
t246 = 0.4444664588e4 * t238;
t247 = 0.430535873e5 * t240;
t248 = 0.1369492365e6 * t236;
t249 = 0.1431293403e6 + t139;
t252 = LFb * (anc * t249 - t246 + t247 - t248);
t253 = t62 * LP;
t255 = lambda2 * LP;
t261 = 0.75e0 * t123 * t157 * t87;
t264 = 0.75e0 * t118 * t157 * Iz3;
t270 = LFb * (-anc * t249 + t246 - t247 + t248);
t273 = lambda1 * LP;
t283 = LP * (-0.1214082751e5 * anc + 0.3200158492e3 * t238 - 0.3248800418e4 * t240 + 0.1091477965e5 * t236);
t294 = LP * (0.75e0 * anc * t119 + 0.75e0 * anc * m3 * t87 * t118);
t296 = t1 * t167 * t81;
t300 = -t261 - t264;
t308 = m1 * t157;
t312 = (0.2e1 * m1 * t108 * t157 - 0.4e1 * t88 - 0.1e1 * t308 - 0.4e1 * Iz3) * t96;
t315 = t157 * t96;
t316 = t1 * t315;
t317 = t167 ^ 2;
t324 = t167 * m1;
res__6_1 = 0.4000000000e1 / (t199 * t167 * t312 + 0.2e1 * t152 * t81 * t317 * m1 * t316 - 0.1e1 * t233 * t324 * t108 * t315) * anc * (t153 * (t108 * t107 + t1 * (t81 * (t103 * t95 + t97 * t111 + t115) + t127) + t81 * t134 + t97 * t141 + t103 * t147 - t149) + t199 * (-t160 + t108 * (t167 * t166 + 0.25e0 * t170 * t169) + t1 * t167 * (t81 * t107 + t177) + t167 * (t81 * t127 + t97 * t186 + t103 * t193 + 0.25e0 * t115)) + t152 * (-0.25e0 * t202 * t201 + t108 * (-0.25e0 * t205 * t201 + t97 * t163 + t103 * t111 - 0.1e1 * t106) + t1 * (t81 * t166 + t215) + t81 * t177 + t97 * t220 + t103 * t223 + t149) + t233 * (t160 - 0.25e0 * t157 * t228 * t227) + t151 * (t155 * (t167 * (t157 * t255 + t244 * t243 + t253 * t252) + t261 + t264) + t108 * t167 * (t81 * (t244 * t270 + t253 * t243 - 0.75e0 * t157 * t273) + t283) + t296 * t294) + t155 * t300) * m1 * theta3__dot;
t332 = m1 * t96;
t333 = t69 * t332;
t335 = t62 * t332;
t337 = anc * m1;
t338 = t337 * t105;
t339 = t335 * t102 + t333 * t95 + t338;
t343 = t337 * t114;
t349 = t81 * m1;
t353 = 0.75e0 * t338;
t362 = t333 * t102 + t335 * t163 - 0.1e1 * t343;
t373 = t308 * t273;
t392 = t157 * t324;
t402 = t308 * t255;
t426 = LP * m1;
t427 = t69 * t426;
t429 = t62 * t426;
res__7_1 = 0.4000000000e1 / (t153 * t312 + 0.2e1 * t199 * t81 * t324 * t316 - 0.1e1 * t152 * m1 * t108 * t315) * anc * (t153 * (t108 * t339 + t1 * (t81 * (t333 * t111 + t335 * t95 + t343) + m1 * t127) + t349 * t134 + t333 * t141 + t335 * t147 - t353) + t199 * (0.25e0 * m1 * t158 * t156 + t108 * (t167 * t362 - 0.25e0 * m1 * t170 * t169) + t1 * (t167 * (m1 * t177 + t81 * t339) - 0.25e0 * t373 + LP * (-0.1e1 * t87 * m3 * lambda1 - 0.1e1 * lambda1 * Iz3)) + t167 * (t349 * t127 + t333 * t186 + t335 * t193 + 0.25e0 * t343)) + t152 * (0.25e0 * t202 * t392 + t108 * (0.25e0 * t205 * t392 + t333 * t163 + t335 * t111 - 0.1e1 * t338) + t1 * (t167 * (-0.25e0 * t402 + LP * (-0.1e1 * t87 * m3 * lambda2 - 0.1e1 * lambda2 * Iz3)) + t81 * t362 + m1 * t215) + t349 * t177 + t333 * t220 + t335 * t223 + t353) + 0.25e0 * t308 * t228 * t233 * t227 + t151 * (t155 * (t167 * (t427 * t243 + t429 * t252 + 0.75e0 * t402) - m1 * t300) + t108 * t167 * (t81 * (t427 * t270 + t429 * t243 - 0.75e0 * t373) + m1 * t283) + t296 * m1 * t294) + t155 * m1 * t300) * theta3__dot;
% 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__ )
% 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 = (-t1 * lambda1 + 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.7519556233e4 + 0.4e1 * lambda1;
t146 = 0.1994054006e21 * m3 * t121 - 0.1065048058e20 * m3 * t124 + 0.4457235821e18 * m3 * t85 - 0.1453283652e17 * m3 * t102 + 0.3628679930e15 * m3 * t106 - 0.6681581371e13 * m3 * t110 + 0.8754046649e11 * m3 * t84 - 0.7922043684e9 * m3 * t90 + 0.3834232346e7 * m3 * t83 - 0.8424259047e4 * T * m3 + m3 * t144;
t169 = t63 * (0.4599872881e27 * t87 * m3 - 0.6880531233e27 * m3 * t91 + 0.4824319805e27 * m3 * t94 - 0.2109428949e27 * m3 * t97 + 0.6387812719e26 * m3 * t86 - 0.1436741869e26 * m3 * t103 + 0.2467417161e25 * m3 * t107 - 0.3310076547e24 * m3 * t111 + 0.3507040364e23 * m3 * t114 - 0.2967810204e22 * m3 * t117 + t146) + m1 * t43 * lambda1 + 0.4599872881e27 * Iz3 * t87 - 0.6880531233e27 * Iz3 * t91 + 0.4824319805e27 * Iz3 * t94 - 0.2109428949e27 * Iz3 * t97 + 0.6387812719e26 * Iz3 * t86 - 0.1436741869e26 * Iz3 * t103 + 0.2467417161e25 * Iz3 * t107 - 0.3310076547e24 * Iz3 * t111 + 0.3507040364e23 * Iz3 * t114;
t193 = -0.2967810204e22 * Iz3 * t117 + 0.1994054006e21 * Iz3 * t121 - 0.1065048058e20 * Iz3 * t124 + 0.4457235821e18 * Iz3 * t85 - 0.1453283652e17 * Iz3 * t102 + 0.3628679930e15 * Iz3 * t106 - 0.6681581371e13 * Iz3 * t110 + 0.8754046649e11 * Iz3 * t84 - 0.7922043684e9 * Iz3 * t90 + 0.3834232346e7 * Iz3 * t83 - 0.8424259047e4 * 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__ )
% 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 + t4 * lambda2) * LP;
res__3_6 = res__2_2;
res__3_7 = res__1_2;
t12 = t1 ^ 2;
res__4_2 = 0.1e1 / t12 * t2 * anc * theta3__dot;
t19 = 0.1e1 / t1;
res__4_3 = -theta3__dot * t19 * anc * (t1 * t2 + t6 * t4);
res__4_4 = -1;
t24 = -t6 * t1 + t2 * t4;
res__4_5 = t19 * anc * t24;
t26 = theta3 ^ 2;
t27 = LP * t26;
t28 = cos(psi);
t30 = t28 * LFb * t27;
t32 = sin(psi);
t34 = t32 * LFb * t27;
t37 = t2 ^ 2;
t43 = t26 * theta3;
t51 = t12 ^ 2;
t53 = 0.57404783e5 * t26;
t54 = g * m3;
t55 = 0.592621945e4 * t43;
t56 = 0.182598982e6 * theta3;
t59 = t28 * LP;
t61 = 0.474097556e4 * t43;
t65 = LFb * (-t61 + t26 * 0.490267876799999940e5 + theta3 * (-0.167958569199998077e6) + 0.190914737699991150e6);
t66 = t32 * LP;
t69 = LP * lambda2 * anc;
t73 = 0.1e1 * t54;
t78 = LP * lambda1 * anc;
t79 = 0.1e1 * t78;
t89 = theta3__dot ^ 2;
t91 = LFb ^ 2;
t92 = t91 * m3;
t93 = t92 + Iz3;
t100 = anc * m3 * t91 * t89;
t102 = Iz3 * t89;
t103 = anc * t102;
t110 = 0.1e1 / LP;
t111 = 0.1e1 / t93;
t112 = t111 * t110;
res__5_2 = 0.1e1 / t51 * t112 * (t51 * (t37 * (0.8e-4 * t30 + 0.3e-4 * t34) + t2 * (t6 * (-0.8e-4 * t34 + 0.3e-4 * t30) + LP * (0.3e-6 * t43 + 0.3e-5 * t26))) + t12 * (t37 * (t59 * LFb * (t53 + t54 - t55 - t56 + 0.1908391204e6) + t66 * t65 + t69) + t2 * (t6 * (t59 * t65 + t66 * LFb * (t55 + t56 - 0.1908391204e6 - t53 - t73) - t79) + LP * (0.4266877988e3 * t43 + t26 * (-0.433173389199999929e4) + theta3 * 0.145530395299999982e5 - 0.161877700099999965e5)) + t93 * LP * t6 * t89) + t37 * t4 * (-0.3e1 * t100 - 0.3e1 * t103)) * anc;
t116 = 0.2316257697e6 * theta3;
t118 = LFb * (0.645162464e5 * t26 + t54 - t55 - t116 + 0.274818405e6);
t122 = -t61 + t26 * 0.401374585399999924e5 + theta3 * (-0.110553786099999983e6) + 0.996152466999999888e5;
t123 = LFb * t122;
t124 = t66 * t123;
t128 = 0.6451624645e5 * t26;
t138 = LP * (0.2133438996e3 * t43 + t26 * (-0.216586694399999988e4) + theta3 * 0.727651976499999819e4 - 0.809388500500000009e4);
t147 = -0.4331733889e4 * theta3 + 0.6400316985e3 * t26 + 0.7276519765e4 + 0.5e0 * t91 * m3 * t89 + 0.5e0 * t102;
t150 = 0.5e0 * t54;
t151 = 0.2963109725e4 * t43;
t157 = 0.237048778e4 * t43;
t165 = t12 * t1;
t207 = 0.1e1 / t165;
res__5_3 = -0.2e1 * t207 * t112 * anc * (t165 * (t37 * (t59 * t118 + t124 + t69) + t2 * (t6 * (t59 * t123 + t66 * LFb * (t55 + t116 - 0.274818405e6 - t128 - t73) - t79) + t138) + t6 * LP * t147 + t59 * LFb * (-t150 + t151 + 0.1403262787e6 * theta3 - 0.3581385488e5 * t26 - 0.1793988448e6) + t66 * LFb * (t157 + theta3 * 0.265745014999999948e5 + t26 * (-0.156240646799999959e5) - 0.415787784999999894e4) - 0.5e0 * t69) + t12 * (t37 * t4 * (-t59 * LFb * t122 + t66 * t118 + t78) + t2 * t4 * (t6 * (t59 * LFb * (t54 + t128 - t116 - t55 + 0.274818405e6) + t124 + t69) - LP * t147) + t4 * (t6 * t138 + t59 * LFb * (-t157 + t26 * 0.245133938600000001e5 + theta3 * (-0.839792845999990241e5) + 0.954573688499955606e5) + t66 * LFb * (t151 - 0.2870239154e5 * t26 + 0.91299491e5 * theta3 - 0.954195602e5 - t150) - 0.5e0 * t78)) + t2 * t6 * (-0.1e1 * t103 - 0.1e1 * t100));
res__5_5 = t110 * t207 * theta3__dot * anc * (0.2e1 * t4 * t12 * t6 * LP + 0.2e1 * t165 * t2 * LP - 0.2e1 * t37 * anc);
t224 = anc ^ 2;
res__5_6 = t19 * t111 / m1 * (t1 * (-0.1e1 * m1 * t37 * t224 + 0.4e1 * Iz3 + 0.4e1 * t92 + m1 * t224) - 0.1e1 * t2 * t4 * m1 * t6 * t224);
res__5_7 = 0.1e1 * t19 * t111 * t2 * t24 * t224;
% 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