Bonjour,
J'ai l'algorithme suivant que je vais écrire son programme,
Etape1: Choisir une matrices définie positive Q et une matrice D et fixer le paramétre tf=1.5s,
Etape2: Résoudre hors-ligne les équations de P, V et H (premier programme), fixer alpha et calculer la valeur de Sopt0. Si Sopt0=0 à t=0, alors t0=0 et passer à l'étape4. Sinon, passer à l'étape3.
Etape3: Appliquer la loi de commande v=-alpha*sign(D*Sopt0) jusqu'à l'instant t tel que Sopt0=0. Alors t0=t et passer à l'étpe4.
Etape4: pour t apprtenant à [t0, tf], appliquer la loi de commnde v=-alpha*sign(D*Sopt),
Etape5: pour t appartenant à [tf, infini[, appliquer la loi de commande v=-alpha*sign(D*Soptf).
Rq: Sopt=[Sopt1 Sopt2 Sopt3]' de meme por Sopt0 et Soptf.
Mon probléme je doit trouver x10 , x20 et x30 tendent vers zero mais avec mon programme je ne trouve pas ça, est ce que vous pouvez m'aider.
voici le premier programme à exécuter
Code : Sélectionner tout - Visualiser dans une fenêtre à part
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
function y=exerart()
P0=[0 0 0;0 0 0;0 0 0]; y1=0;y2=0;y3=0;y4=0;y5=0;y6=0;y7=0;y8=0;y9=0;
V0=[1 0 0;0 1 0;0 0 1]; y10=1;y11=0;y12=0;y13=0;y14=1;y15=0;y16=0;y17=0;y18=1;
H0=[0 0 0;0 0 0;0 0 0]; y19=0;y20=0;y21=0;y22=0;y23=0;y24=0;y25=0;y26=0;y27=0;
I=[1 0 0;0 1 0;0 0 1];
h=0.001;
for i=0:0.001:1.5;
% Pour P
k1=-P0*P0+I;
k2=-(P0+(h/2)*k1)*(P0+(h/2)*k1)+I;
k3=-(P0+(h/2)*k2)*(P0+(h/2)*k2)+I;
k4=-(P0+h*k3)*(P0+h*k3)+I;
P=P0+(h/6)*(k1+2*k2+2*k3+k4);
% Pour V
r1=-P0'*V0;
r2=-P0'*(V0+(h/2)*r1);
r3=-P0'*(V0+(h/2)*r2);
r4=-P0'*(V0+h*r3);
V=V0+(h/6)*(r1+2*r2+2*r3+r4);
% Pour H
l1=V0'*V0; % l1=l2=l3=l4
H=H0+(h/6)*(l1+2*l1+2*l1+l1);
P0=P;
V0=V;
H0=H;
% Pour P
P1=P0(1);
P2=P0(2);
P3=P0(3);
P4=P0(4);
P5=P0(5);
P6=P0(6);
P7=P0(7);
P8=P0(8);
P9=P0(9);
y1=[y1 P1];
y2=[y2 P2];
y3=[y3 P3];
y4=[y4 P4];
y5=[y5 P5];
y6=[y6 P6];
y7=[y7 P7];
y8=[y8 P8];
y9=[y9 P9];
%Pour V
V1=V0(1);
V2=V0(2);
V3=V0(3);
V4=V0(4);
V5=V0(5);
V6=V0(6);
V7=V0(7);
V8=V0(8);
V9=V0(9);
y10=[y10 V1];
y11=[y11 V2];
y12=[y12 V3];
y13=[y13 V4];
y14=[y14 V5];
y15=[y15 V6];
y16=[y16 V7];
y17=[y17 V8];
y18=[y18 V9];
% Pour H
H1=H0(1);
H2=H0(2);
H3=H0(3);
H4=H0(4);
H5=H0(5);
H6=H0(6);
H7=H0(7);
H8=H0(8);
H9=H0(9);
y19=[y19 H1];
y20=[y20 H2];
y21=[y21 H3];
y22=[y22 H4];
y23=[y23 H5];
y24=[y24 H6];
y25=[y25 H7];
y26=[y26 H8];
y27=[y27 H9];
save param4 y1 y2 y3 y4 y5 y6 y7 y8 y9
save param5 y10 y11 y12 y13 y14 y15 y16 y17 y18
save param6 y19 y20 y21 y22 y23 y24 y25 y26 y27
end
plot(y1)
grid
Mon progrmme principle est

Code : Sélectionner tout - Visualiser dans une fenêtre à part
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
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
load param4
load param5
load param6
h=0.001;alfa=2000; % X1=0;X2=0;X3=0;
x10=0.2-1;x20=1.1-0.5;x30=-0.2;u0=[0 0 0]';Z10=[-0.8 0.6 -0.2]';Z20=[0 0 0]';u0=[0 0 0]';
D=[0.0005 0.0015 0.0015;0.0012 0.0005 0.002;0.002 0.0015 0.0005];
X1=0.2-1;X2=1.1-0.5;X3=-0.2;U1=0;U2=0;U3=0;I=1500;Z3=-0.8;Z4=0.6;Z5=-0.2;
% Matrice P0 et Pf
a1=y1(1500);b1=y1(1);
a2=y2(1500);b2=y2(1);
a3=y3(1500);b3=y3(1);
a4=y4(1500);b4=y4(1);
a5=y5(1500);b5=y5(1);
a6=y6(1500);b6=y6(1);
a7=y7(1500);b7=y7(1);
a8=y8(1500);b8=y8(1);
a9=y9(1500);b9=y9(1);
% Matrice V0 et Vf
a10=y10(1500);b10=y10(1);
a11=y11(1500);b11=y11(1);
a12=y12(1500);b12=y12(1);
a13=y13(1500);b13=y13(1);
a14=y14(1500);b14=y14(1);
a15=y15(1500);b15=y15(1);
a16=y16(1500);b16=y16(1);
a17=y17(1500);b17=y17(1);
a18=y18(1500);b18=y18(1);
% Matrice H0 et Hf
a19=y19(1500);b19=y19(1);
a20=y20(1500);b20=y20(1);
a21=y21(1500);b21=y21(1);
a22=y22(1500);b22=y22(1);
a23=y23(1500);b23=y23(1);
a24=y24(1500);b24=y24(1);
a25=y25(1500);b25=y25(1);
a26=y26(1500);b26=y26(1);
a27=y27(1500);b27=y27(1);
P0=[a1 a4 a7;a2 a5 a8;a3 a6 a9];
V0=[a10 a13 a16;a11 a14 a17;a12 a15 a18];
H0=[a19 a22 a25;a20 a23 a26;a21 a24 a27];
a0=P0-V0*inv(H0)*V0';
Pf=[b1 b4 b7;b2 b5 b8;b3 b6 b9];
Vf=[b10 b13 b16;b11 b14 b17;b12 b15 b18];
Hf=[b19 b22 b25;b20 b23 b26;b21 b24 b27];
Soptf=Z20+(Pf-Vf*inv(Hf)*Vf')*Z10;
af=Pf-Vf*inv(Hf)*Vf';
%for i=0:0.001:0.002;
Sopt0=Z20+(P0-V0*inv(H0)*V0')*Z10;
Sopt01=Z20(1)+a0(1)*Z10(1)+a0(4)*Z10(2)+a0(7)*Z10(3);
Sopt02=Z20(2)+a0(2)*Z10(1)+a0(5)*Z10(2)+a0(8)*Z10(3);
Sopt03=Z20(3)+a0(3)*Z10(1)+a0(6)*Z10(2)+a0(9)*Z10(3);
if (Sopt01==0)&(Sopt02==0)&(Sopt03==0)
j1=0;
for s=j1:0.001:1.499;
ct=floor(1500-(s/h));
% Matrice P
z1=y1(ct);
z2=y2(ct);
z3=y3(ct);
z4=y4(ct);
z5=y5(ct);
z6=y6(ct);
z7=y7(ct);
z8=y8(ct);
z9=y9(ct);
% Matrice V
z11=y10(ct);
z12=y11(ct);
z13=y12(ct);
z14=y13(ct);
z15=y14(ct);
z16=y15(ct);
z17=y16(ct);
z18=y17(ct);
z19=y18(ct);
% Matrice H
z21=y19(ct);
z22=y20(ct);
z23=y21(ct);
z24=y22(ct);
z25=y23(ct);
z26=y24(ct);
z27=y25(ct);
z28=y26(ct);
z29=y27(ct);
P=[z1 z4 z7;z2 z5 z8;z3 z6 z9];
V=[z11 z14 z17;z12 z15 z18;z13 z16 z19];
H=[z21 z24 z27;z22 z25 z28;z23 z26 z29];
deltaf1=cos(s)*(1+0.05*sin(4*s)+0.1*cos(s));
deltaf2=sin(s)*cos(s)*(1+0.05*sin(4*s)+0.1*cos(s));
deltaf3=(sin(s)^2)*(1+0.05*sin(4*s)+0.1*cos(s));
deltaf4=cos(s+h/2)*(1+0.05*sin(4*(s+h/2))+0.1*cos(s+h/2));
deltaf5=sin(s+h/2)*cos(s+h/2)*(1+0.05*sin(4*(s+h/2))+0.1*cos(s+h/2));
deltaf6=(sin(s+h/2)^2)*(1+0.05*sin(4*(s+h/2))+0.1*cos(s+h/2));
deltaf7=cos(s+h)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h));
deltaf8=sin(s+h)*cos(s+h)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h));
deltaf9=(sin(s+h)^2)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h));
deltag1=0.01*sin(s+2.1)*(u0(1)-0.5*u0(3));
deltag2=0.01*cos(s)*(-0.2*u0(2)+0.8*u0(3));
deltag3=0.01*cos(s+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag4=0.01*sin(s+(h/2)+2.1)*(u0(1)-0.5*u0(3));
deltag5=0.01*cos(s+h/2)*(-0.2*u0(2)+0.8*u0(3));
deltag6=0.01*cos(s+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag7=0.01*sin(s+h+2.1)*(u0(1)-0.5*u0(3));
deltag8=0.01*cos(s+h)*(-0.2*u0(2)+0.8*u0(3));
deltag9=0.01*cos(s+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
% Methode de Runge Kutta d'ordre 4
l11=-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3));
l12=x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3));
l13=-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3));
l21=-(x20+(h/2)*l12)*(x30+(h/2)*l13)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l22=(x10+(h/2)*l11)*(x30+(h/2)*l13)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l23=-(1/3)*(x10+(h/2)*l11)*(x20+(h/2)*l12)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l31=-(x20+(h/2)*l22)*(x30+(h/2)*l23)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l32=(x10+(h/2)*l21)*(x30+(h/2)*l23)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l33=-(1/3)*(x10+(h/2)*l21)*(x20+(h/2)*l22)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l41=-(x20+h*l32)*(x30+h*l33)+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3));
l42=(x10+h*l31)*(x30+h*l33)+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3));
l43=-(1/3)*(x10+h*l31)*(x20+h*l32)+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3));
x1=x10+(h/6)*(l11+2*l21+2*l31+l41);
x2=x20+(h/6)*(l12+2*l22+2*l32+l42);
x3=x30+(h/6)*(l13+2*l23+2*l33+l43);
q1=-(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
q2=-x20*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q3=-sin(s)*(1+0.05*sin(4*s)+0.1*cos(s))+cos(s)*(0.2*cos(4*s)-0.1*sin(s))+0.01*cos(s+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*s);
fi1=q1+q2+q3;
q4=(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
q5=x10*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q6=(cos(s)^2)*(1+0.05*sin(4*s)+0.1*cos(s))-(sin(s)^2)*(1+0.05*sin(4*s)+0.1*cos(s));
q7=sin(s)*cos(s)*(0.2*cos(4*s)-0.1*sin(s))-0.01*sin(s)*(-0.2*u0(2)+0.8*u0(3));
q8=(0.5^3)*cos(0.5*s)*cos(s)-(0.5^2)*sin(0.5*s)*sin(s)-(0.5^2)*sin(0.5*s)*sin(s)+0.5*cos(0.5*s)*cos(s);
fi2=q4+q5+q6+q7+q8;
q9=-(1/3)*x20*(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)));
q10=-(1/3)*x10*(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)));
q11=2*sin(s)*cos(s)*(1+0.05*sin(4*s)+0.1*cos(s))+(sin(s)^2)*(0.2*cos(4*s)-0.1*sin(s));
q12=-0.01*sin(s+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
q13=(0.5^3)*cos(0.5*s)*sin(s)+(0.5^2)*sin(0.5*s)*cos(s)+(0.5^2)*sin(0.5*s)*cos(s)+0.5*cos(0.5*s)*sin(s);
fi3=q9+q10+q11+q12+q13;
fi=[fi1 fi2 fi3]';
gama=[0.01*sin(s+2.1)+1 1.2 -0.005*sin(s+2.1)+1.5;1.5 -0.002*cos(s)+1 0.008*cos(s)+1.2;-0.002*cos(s+1.3)+1.2 -0.01*cos(s+1.3)+1.5 0.007*cos(s+1.3)+1];
qr1=-(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qr2=-(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qr3=-x20*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qr4=-x2*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qr5=-sin(s+(h/2))*(1+0.05*sin(4*(s+(h/2)))+0.1*cos(s+(h/2)))+cos(s+(h/2))*(0.2*cos(4*(s+(h/2)))-0.1*sin(s+(h/2)))+0.01*cos(s+(h/2)+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(s+(h/2)));
qr6=-sin(s+h)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h))+cos(s+h)*(0.2*cos(4*(s+h))-0.1*sin(s+h))+0.01*cos(s+h+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(s+h));
qr7=(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qr8=(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qr9=x10*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qr10=x10*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qr11=(cos(s+(h/2))^2)*(1+0.05*sin(4*(s+(h/2)))+0.1*cos(s+(h/2)))-(sin(s+(h/2))^2)*(1+0.05*sin(4*(s+(h/2)))+0.1*cos(s+(h/2)));
qr12=(cos(s+h)^2)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h))-(sin(s+h)^2)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h));
qr13=sin(s+(h/2))*cos(s+(h/2))*(0.2*cos(4*(s+(h/2)))-0.1*sin(s+(h/2)))-0.01*sin(s+(h/2))*(-0.2*u0(2)+0.8*u0(3));
qr14=sin(s+h)*cos(s+h)*(0.2*cos(4*(s+h))-0.1*sin(s+h))-0.01*sin(s+h)*(-0.2*u0(2)+0.8*u0(3));
qr15=(0.5^3)*cos(0.5*(s+(h/2)))*cos(s+(h/2))-(0.5^2)*sin(0.5*(s+(h/2)))*sin(s+(h/2))-(0.5^2)*sin(0.5*(s+(h/2)))*sin(s+(h/2))+0.5*cos(0.5*(s+(h/2)))*cos(s+(h/2));
qr16=(0.5^3)*cos(0.5*(s+h))*cos(s+h)-(0.5^2)*sin(0.5*(s+h))*sin(s+h)-(0.5^2)*sin(0.5*(s+h))*sin(s+h)+0.5*cos(0.5*(s+h))*cos(s+h);
qr17=-(1/3)*x20*(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qr18=-(1/3)*x20*(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qr19=-(1/3)*x10*(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qr20=-(1/3)*x10*(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qr21=2*sin(s+(h/2))*cos(s+(h/2))*(1+0.05*sin(4*(s+(h/2)))+0.1*cos(s+(h/2)))+(sin(s+(h/2))^2)*(0.2*cos(4*(s+(h/2)))-0.1*sin(s+(h/2)));
qr22=2*sin(s+h)*cos(s+h)*(1+0.05*sin(4*(s+h))+0.1*cos(s+h))+(sin(s+h)^2)*(0.2*cos(4*(s+h))-0.1*sin(s+h));
qr23=-0.01*sin(s+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qr24=-0.01*sin(s+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qr25=(0.5^3)*cos(0.5*(s+(h/2)))*sin(s+(h/2))+(0.5^2)*sin(0.5*(s+(h/2)))*cos(s+(h/2))+(0.5^2)*sin(0.5*(s+(h/2)))*cos(s+(h/2))+0.5*cos(0.5*(s+(h/2)))*sin(s+(h/2));
qr26=(0.5^3)*cos(0.5*s+h)*sin(s+h)+(0.5^2)*sin(0.5*(s+h))*cos(s+h)+(0.5^2)*sin(0.5*(s+h))*cos(s+h)+0.5*cos(0.5*(s+h))*sin(s+h);
fi11=qr1+qr3+qr5;
fi12=qr2+qr4+qr6;
fi21=qr7+qr9+qr11+qr13+qr15;
fi22=qr8+qr10+qr12+qr14+qr16;
fi31=qr17+qr19+qr21+qr23+qr25;
fi32=qr18+qr20+qr22+qr24+qr26;
gama1=[0.01*sin(s+(h/2)+2.1)+1 1.2 -0.005*sin(s+(h/2)+2.1)+1.5;1.5 -0.002*cos(s+(h/2))+1 0.008*cos(s+(h/2))+1.2;-0.002*cos(s+(h/2)+1.3)+1.2 -0.01*cos(s+(h/2)+1.3)+1.5 0.007*cos(s+(h/2)+1.3)+1];
gama2=[0.01*sin(s+h+2.1)+1 1.2 -0.005*sin(s+h+2.1)+1.5;1.5 -0.002*cos(s+h)+1 0.008*cos(s+h)+1.2;-0.002*cos(s+h+1.3)+1.2 -0.01*cos(s+h+1.3)+1.5 0.007*cos(s+h+1.3)+1];
fir1=[fi11 fi21 fi31]';
fir2=[fi12 fi22 fi32]';
Sopt=Z20+(P-V*inv(H)*V')*Z10;
v=-alfa*sign(D*Sopt);
% Methode de Runge Kutta d'ordre 4
k1=Z20;
k2=Z20+(h/2)*k1;
k3=Z20+(h/2)*k2;
k4=Z20+h*k3;
Z1=Z10+(h/6)*(k1+2*k2+2*k3+k4);
r1=fi+gama*v;
r2=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r1+(P-V*inv(H)*V')*(Z10+(h/2)*k1)));
r3=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r2+(P-V*inv(H)*V')*(Z10+(h/2)*k2)));
r4=fir2-alfa*gama2*sign(D*(Z20+h*r3+(P-V*inv(H)*V')*(Z10+h*k3)));
Z2=Z20+(h/6)*(r1+2*r2+2*r3+r4);
u=u0+h*v;
u0=u;
Z10=Z1;
Z20=Z2;
x10=x1;
x20=x2;
x30=x3;
X1=[X1 x10];
end
for i=1.499:0.001:2;
Soptf=Z20+(Pf-Vf*inv(Hf)*Vf')*Z10;
Soptf1=Z20(1)+af(1)*Z10(1)+af(4)*Z10(2)+af(7)*Z10(3);
Soptf2=Z20(2)+af(2)*Z10(1)+af(5)*Z10(2)+af(8)*Z10(3);
Soptf3=Z20(3)+af(3)*Z10(1)+af(6)*Z10(2)+af(9)*Z10(3);
vf=-alfa*sign(D*Soptf);
deltaf1=cos(i)*(1+0.05*sin(4*i)+0.1*cos(i));
deltaf2=sin(i)*cos(i)*(1+0.05*sin(4*i)+0.1*cos(i));
deltaf3=(sin(i)^2)*(1+0.05*sin(4*i)+0.1*cos(i));
deltaf4=cos(i+h/2)*(1+0.05*sin(4*(i+h/2))+0.1*cos(i+h/2));
deltaf5=sin(i+h/2)*cos(i+h/2)*(1+0.05*sin(4*(i+h/2))+0.1*cos(i+h/2));
deltaf6=(sin(i+h/2)^2)*(1+0.05*sin(4*(i+h/2))+0.1*cos(i+h/2));
deltaf7=cos(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
deltaf8=sin(i+h)*cos(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
deltaf9=(sin(i+h)^2)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
deltag1=0.01*sin(i+2.1)*(u0(1)-0.5*u0(3));
deltag2=0.01*cos(i)*(-0.2*u0(2)+0.8*u0(3));
deltag3=0.01*cos(i+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag4=0.01*sin(i+(h/2)+2.1)*(u0(1)-0.5*u0(3));
deltag5=0.01*cos(i+h/2)*(-0.2*u0(2)+0.8*u0(3));
deltag6=0.01*cos(i+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag7=0.01*sin(i+h+2.1)*(u0(1)-0.5*u0(3));
deltag8=0.01*cos(i+h)*(-0.2*u0(2)+0.8*u0(3));
deltag9=0.01*cos(i+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
% Methode de Runge Kutta d'ordre 4
l11=-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3));
l12=x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3));
l13=-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3));
l21=-(x20+(h/2)*l12)*(x30+(h/2)*l13)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l22=(x10+(h/2)*l11)*(x30+(h/2)*l13)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l23=-(1/3)*(x10+(h/2)*l11)*(x20+(h/2)*l12)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l31=-(x20+(h/2)*l22)*(x30+(h/2)*l23)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l32=(x10+(h/2)*l21)*(x30+(h/2)*l23)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l33=-(1/3)*(x10+(h/2)*l21)*(x20+(h/2)*l22)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l41=-(x20+h*l32)*(x30+h*l33)+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3));
l42=(x10+h*l31)*(x30+h*l33)+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3));
l43=-(1/3)*(x10+h*l31)*(x20+h*l32)+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3));
x1=x10+(h/6)*(l11+2*l21+2*l31+l41);
x2=x20+(h/6)*(l12+2*l22+2*l32+l42);
x3=x30+(h/6)*(l13+2*l23+2*l33+l43);
q1=-(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
q2=-x20*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q3=-sin(i)*(1+0.05*sin(4*i)+0.1*cos(i))+cos(i)*(0.2*cos(4*i)-0.1*sin(i))+0.01*cos(i+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*i);
fi1=q1+q2+q3;
q4=(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
q5=x10*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q6=(cos(i)^2)*(1+0.05*sin(4*i)+0.1*cos(i))-(sin(i)^2)*(1+0.05*sin(4*i)+0.1*cos(i));
q7=sin(i)*cos(i)*(0.2*cos(4*i)-0.1*sin(i))-0.01*sin(i)*(-0.2*u0(2)+0.8*u0(3));
q8=(0.5^3)*cos(0.5*i)*cos(i)-(0.5^2)*sin(0.5*i)*sin(i)-(0.5^2)*sin(0.5*i)*sin(i)+0.5*cos(0.5*i)*cos(i);
fi2=q4+q5+q6+q7+q8;
q9=-(1/3)*x20*(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)));
q10=-(1/3)*x10*(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)));
q11=2*sin(i)*cos(i)*(1+0.05*sin(4*i)+0.1*cos(i))+(sin(i)^2)*(0.2*cos(4*i)-0.1*sin(i));
q12=-0.01*sin(i+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
q13=(0.5^3)*cos(0.5*i)*sin(i)+(0.5^2)*sin(0.5*i)*cos(i)+(0.5^2)*sin(0.5*i)*cos(i)+0.5*cos(0.5*i)*sin(i);
fi3=q9+q10+q11+q12+q13;
fi=[fi1 fi2 fi3]';
gama=[0.01*sin(i+2.1)+1 1.2 -0.005*sin(i+2.1)+1.5;1.5 -0.002*cos(i)+1 0.008*cos(i)+1.2;-0.002*cos(i+1.3)+1.2 -0.01*cos(i+1.3)+1.5 0.007*cos(i+1.3)+1];
qz1=-(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qz2=-(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qz3=-x20*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qz4=-x20*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qz5=-sin(i+(h/2))*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)))+cos(i+(h/2))*(0.2*cos(4*(i+(h/2)))-0.1*sin(i+(h/2)))+0.01*cos(i+(h/2)+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(i+(h/2)));
qz6=-sin(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h))+cos(i+h)*(0.2*cos(4*(i+h))-0.1*sin(i+h))+0.01*cos(i+h+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(i+h));
qz7=(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qz8=(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qz9=x10*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qz10=x10*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qz11=(cos(i+(h/2))^2)*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)))-(sin(i+(h/2))^2)*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)));
qz12=(cos(i+h)^2)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h))-(sin(i+h)^2)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
qz13=sin(i+(h/2))*cos(i+(h/2))*(0.2*cos(4*(i+(h/2)))-0.1*sin(i+(h/2)))-0.01*sin(i+(h/2))*(-0.2*u0(2)+0.8*u0(3));
qz14=sin(i+h)*cos(i+h)*(0.2*cos(4*(i+h))-0.1*sin(i+h))-0.01*sin(i+h)*(-0.2*u0(2)+0.8*u0(3));
qz15=(0.5^3)*cos(0.5*(i+(h/2)))*cos(i+(h/2))-(0.5^2)*sin(0.5*(i+(h/2)))*sin(i+(h/2))-(0.5^2)*sin(0.5*(i+(h/2)))*sin(i+(h/2))+0.5*cos(0.5*(i+(h/2)))*cos(i+(h/2));
qz16=(0.5^3)*cos(0.5*(i+h))*cos(i+h)-(0.5^2)*sin(0.5*(i+h))*sin(i+h)-(0.5^2)*sin(0.5*(i+h))*sin(i+h)+0.5*cos(0.5*(i+h))*cos(i+h);
qz17=-(1/3)*x20*(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qz18=-(1/3)*x20*(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qz19=-(1/3)*x10*(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qz20=-(1/3)*x10*(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qz21=2*sin(i+(h/2))*cos(i+(h/2))*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)))+(sin(i+(h/2))^2)*(0.2*cos(4*(i+(h/2)))-0.1*sin(i+(h/2)));
qz22=2*sin(i+h)*cos(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h))+(sin(i+h)^2)*(0.2*cos(4*(i+h))-0.1*sin(i+h));
qz23=-0.01*sin(i+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qz24=-0.01*sin(i+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qz25=(0.5^3)*cos(0.5*(i+(h/2)))*sin(i+(h/2))+(0.5^2)*sin(0.5*(i+(h/2)))*cos(i+(h/2))+(0.5^2)*sin(0.5*(i+(h/2)))*cos(i+(h/2))+0.5*cos(0.5*(i+(h/2)))*sin(i+(h/2));
qz26=(0.5^3)*cos(0.5*i+h)*sin(i+h)+(0.5^2)*sin(0.5*(i+h))*cos(i+h)+(0.5^2)*sin(0.5*(i+h))*cos(i+h)+0.5*cos(0.5*(i+h))*sin(i+h);
fi11=qz1+qz3+qz5;
fi12=qz2+qz4+qz6;
fi21=qz7+qz9+qz11+qz13+qz15;
fi22=qz8+qz10+qz12+qz14+qz16;
fi31=qz17+qz19+qz21+qz23+qz25;
fi32=qz18+qz20+qz22+qz24+qz26;
gama1=[0.01*sin(i+(h/2)+2.1)+1 1.2 -0.005*sin(i+(h/2)+2.1)+1.5;1.5 -0.002*cos(i+(h/2))+1 0.008*cos(i+(h/2))+1.2;-0.002*cos(i+(h/2)+1.3)+1.2 -0.01*cos(i+(h/2)+1.3)+1.5 0.007*cos(i+(h/2)+1.3)+1];
gama2=[0.01*sin(i+h+2.1)+1 1.2 -0.005*sin(i+h+2.1)+1.5;1.5 -0.002*cos(i+h)+1 0.008*cos(i+h)+1.2;-0.002*cos(i+h+1.3)+1.2 -0.01*cos(i+h+1.3)+1.5 0.007*cos(i+h+1.3)+1];
fir1=[fi11 fi21 fi31]';
fir2=[fi12 fi22 fi32]';
% Methode de Runge Kutta d'ordre 4
k1=Z20;
k2=Z20+(h/2)*k1;
k3=Z20+(h/2)*k2;
k4=Z20+h*k3;
Z1=Z10+(h/6)*(k1+2*k2+2*k3+k4);
r1=fi+gama*vf;
r2=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r1+(Pf-Vf*inv(Hf)*Vf')*(Z10+(h/2)*k1)));
r3=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r2+(Pf-Vf*inv(Hf)*Vf')*(Z10+(h/2)*k2)));
r4=fir2-alfa*gama2*sign(D*(Z20+h*r3+(Pf-Vf*inv(Hf)*Vf')*(Z10+h*k3)));
Z2=Z20+(h/6)*(r1+2*r2+2*r3+r4);
u=u0+h*vf;
u0=u;
Z10=Z1;
Z20=Z2;
x10=x1;
x20=x2;
x30=x3;
X1=[X1 x10];
end
else
j=0;
J=0;
while (abs(Sopt01)>0.01)|(abs(Sopt02)>0.01)|(abs(Sopt03)>0.01)
deltaf1=cos(j)*(1+0.05*sin(4*j)+0.1*cos(j));
deltaf2=sin(j)*cos(j)*(1+0.05*sin(4*j)+0.1*cos(j));
deltaf3=(sin(j)^2)*(1+0.05*sin(4*j)+0.1*cos(j));
deltaf4=cos(j+h/2)*(1+0.05*sin(4*(j+h/2))+0.1*cos(j+h/2));
deltaf5=sin(j+h/2)*cos(j+h/2)*(1+0.05*sin(4*(j+h/2))+0.1*cos(j+h/2));
deltaf6=(sin(j+h/2)^2)*(1+0.05*sin(4*(j+h/2))+0.1*cos(j+h/2));
deltaf7=cos(j+h)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h));
deltaf8=sin(j+h)*cos(j+h)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h));
deltaf9=(sin(j+h)^2)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h));
deltag1=0.01*sin(j+2.1)*(u0(1)-0.5*u0(3));
deltag2=0.01*cos(j)*(-0.2*u0(2)+0.8*u0(3));
deltag3=0.01*cos(j+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag4=0.01*sin(j+(h/2)+2.1)*(u0(1)-0.5*u0(3));
deltag5=0.01*cos(j+h/2)*(-0.2*u0(2)+0.8*u0(3));
deltag6=0.01*cos(j+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag7=0.01*sin(j+h+2.1)*(u0(1)-0.5*u0(3));
deltag8=0.01*cos(j+h)*(-0.2*u0(2)+0.8*u0(3));
deltag9=0.01*cos(j+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
% Methode de Runge Kutta d'ordre 4
l11=-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3));
l12=x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3));
l13=-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3));
l21=-(x20+(h/2)*l12)*(x30+(h/2)*l13)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l22=(x10+(h/2)*l11)*(x30+(h/2)*l13)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l23=-(1/3)*(x10+(h/2)*l11)*(x20+(h/2)*l12)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l31=-(x20+(h/2)*l22)*(x30+(h/2)*l23)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l32=(x10+(h/2)*l21)*(x30+(h/2)*l23)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l33=-(1/3)*(x10+(h/2)*l21)*(x20+(h/2)*l22)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l41=-(x20+h*l32)*(x30+h*l33)+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3));
l42=(x10+h*l31)*(x30+h*l33)+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3));
l43=-(1/3)*(x10+h*l31)*(x20+h*l32)+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3));
x1=x10+(h/6)*(l11+2*l21+2*l31+l41);
x2=x20+(h/6)*(l12+2*l22+2*l32+l42);
x3=x30+(h/6)*(l13+2*l23+2*l33+l43);
q1=-(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
q2=-x20*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q3=-sin(j)*(1+0.05*sin(4*j)+0.1*cos(j))+cos(j)*(0.2*cos(4*j)-0.1*sin(j))+0.01*cos(j+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*j);
fi1=q1+q2+q3;
q4=(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
q5=x10*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q6=(cos(j)^2)*(1+0.05*sin(4*j)+0.1*cos(j))-(sin(j)^2)*(1+0.05*sin(4*j)+0.1*cos(j));
q7=sin(j)*cos(j)*(0.2*cos(4*j)-0.1*sin(j))-0.01*sin(j)*(-0.2*u0(2)+0.8*u0(3));
q8=(0.5^3)*cos(0.5*j)*cos(j)-(0.5^2)*sin(0.5*j)*sin(j)-(0.5^2)*sin(0.5*j)*sin(j)+0.5*cos(0.5*j)*cos(j);
fi2=q4+q5+q6+q7+q8;
q9=-(1/3)*x20*(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)));
q10=-(1/3)*x10*(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)));
q11=2*sin(j)*cos(j)*(1+0.05*sin(4*j)+0.1*cos(j))+(sin(j)^2)*(0.2*cos(4*j)-0.1*sin(j));
q12=-0.01*sin(j+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
q13=(0.5^3)*cos(0.5*j)*sin(j)+(0.5^2)*sin(0.5*j)*cos(j)+(0.5^2)*sin(0.5*j)*cos(j)+0.5*cos(0.5*j)*sin(j);
fi3=q9+q10+q11+q12+q13;
fi=[fi1 fi2 fi3]';
gama=[0.01*sin(j+2.1)+1 1.2 -0.005*sin(j+2.1)+1.5;1.5 -0.002*cos(j)+1 0.008*cos(j)+1.2;-0.002*cos(j+1.3)+1.2 -0.01*cos(j+1.3)+1.5 0.007*cos(j+1.3)+1];
qs1=-(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qs2=-(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qs3=-x20*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qs4=-x20*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qs5=-sin(j+(h/2))*(1+0.05*sin(4*(j+(h/2)))+0.1*cos(j+(h/2)))+cos(j+(h/2))*(0.2*cos(4*(j+(h/2)))-0.1*sin(j+(h/2)))+0.01*cos(j+(h/2)+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(j+(h/2)));
qs6=-sin(j+h)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h))+cos(j+h)*(0.2*cos(4*(j+h))-0.1*sin(j+h))+0.01*cos(j+h+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(j+h));
qs7=(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qs8=(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qs9=x10*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qs10=x10*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qs11=(cos(j+(h/2))^2)*(1+0.05*sin(4*(j+(h/2)))+0.1*cos(j+(h/2)))-(sin(j+(h/2))^2)*(1+0.05*sin(4*(j+(h/2)))+0.1*cos(j+(h/2)));
qs12=(cos(j+h)^2)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h))-(sin(j+h)^2)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h));
qs13=sin(j+(h/2))*cos(j+(h/2))*(0.2*cos(4*(j+(h/2)))-0.1*sin(j+(h/2)))-0.01*sin(j+(h/2))*(-0.2*u0(2)+0.8*u0(3));
qs14=sin(j+h)*cos(j+h)*(0.2*cos(4*(j+h))-0.1*sin(j+h))-0.01*sin(j+h)*(-0.2*u0(2)+0.8*u0(3));
qs15=(0.5^3)*cos(0.5*(j+(h/2)))*cos(j+(h/2))-(0.5^2)*sin(0.5*(j+(h/2)))*sin(j+(h/2))-(0.5^2)*sin(0.5*(j+(h/2)))*sin(j+(h/2))+0.5*cos(0.5*(j+(h/2)))*cos(j+(h/2));
qs16=(0.5^3)*cos(0.5*(j+h))*cos(j+h)-(0.5^2)*sin(0.5*(j+h))*sin(j+h)-(0.5^2)*sin(0.5*(j+h))*sin(j+h)+0.5*cos(0.5*(j+h))*cos(j+h);
qs17=-(1/3)*x20*(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qs18=-(1/3)*x20*(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qs19=-(1/3)*x10*(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qs20=-(1/3)*x10*(x10*x3+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qs21=2*sin(j+(h/2))*cos(j+(h/2))*(1+0.05*sin(4*(j+(h/2)))+0.1*cos(j+(h/2)))+(sin(j+(h/2))^2)*(0.2*cos(4*(j+(h/2)))-0.1*sin(j+(h/2)));
qs22=2*sin(j+h)*cos(j+h)*(1+0.05*sin(4*(j+h))+0.1*cos(j+h))+(sin(j+h)^2)*(0.2*cos(4*(j+h))-0.1*sin(j+h));
qs23=-0.01*sin(j+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qs24=-0.01*sin(j+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qs25=(0.5^3)*cos(0.5*(j+(h/2)))*sin(j+(h/2))+(0.5^2)*sin(0.5*(j+(h/2)))*cos(j+(h/2))+(0.5^2)*sin(0.5*(j+(h/2)))*cos(j+(h/2))+0.5*cos(0.5*(j+(h/2)))*sin(j+(h/2));
qs26=(0.5^3)*cos(0.5*j+h)*sin(j+h)+(0.5^2)*sin(0.5*(j+h))*cos(j+h)+(0.5^2)*sin(0.5*(j+h))*cos(j+h)+0.5*cos(0.5*(j+h))*sin(j+h);
fi11=qs1+qs3+qs5;
fi12=qs2+qs4+qs6;
fi21=qs7+qs9+qs11+qs13+qs15;
fi22=qs8+qs10+qs12+qs14+qs16;
fi31=qs17+qs19+qs21+qs23+qs25;
fi32=qs18+qs20+qs22+qs24+qs26;
gama1=[0.01*sin(j+(h/2)+2.1)+1 1.2 -0.005*sin(j+(h/2)+2.1)+1.5;1.5 -0.002*cos(j+(h/2))+1 0.008*cos(j+(h/2))+1.2;-0.002*cos(j+(h/2)+1.3)+1.2 -0.01*cos(j+(h/2)+1.3)+1.5 0.007*cos(j+(h/2)+1.3)+1];
gama2=[0.01*sin(j+h+2.1)+1 1.2 -0.005*sin(j+h+2.1)+1.5;1.5 -0.002*cos(j+h)+1 0.008*cos(j+h)+1.2;-0.002*cos(j+h+1.3)+1.2 -0.01*cos(j+h+1.3)+1.5 0.007*cos(j+h+1.3)+1];
fir1=[fi11 fi21 fi31]';
fir2=[fi12 fi22 fi32]';
Sopt0=Z20+(P0-V0*inv(H0)*V0')*Z10;
v0=-alfa*sign(D*Sopt0);
% Methode de Runge Kutta d'ordre 4
k1=Z20;
k2=Z20+(h/2)*k1;
k3=Z20+(h/2)*k2;
k4=Z20+h*k3;
Z1=Z10+(h/6)*(k1+2*k2+2*k3+k4);
r1=fi+gama*v0;
r2=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r1+(P0-V0*inv(H0)*V0')*(Z10+(h/2)*k1)));
r3=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r2+(P0-V0*inv(H0)*V0')*(Z10+(h/2)*k2)));
r4=fir2-alfa*gama2*sign(D*(Z20+h*r3+(P0-V0*inv(H0)*V0')*(Z10+h*k3)));
Z2=Z20+(h/6)*(r1+2*r2+2*r3+r4);
u=u0+h*v0;
u0=u;
Z10=Z1;
Z20=Z2;
x10=x1;
x20=x2;
x30=x3;
Sopt01=Z20(1)+a0(1)*Z10(1)+a0(4)*Z10(2)+a0(7)*Z10(3);
Sopt02=Z20(2)+a0(2)*Z10(1)+a0(5)*Z10(2)+a0(8)*Z10(3);
Sopt03=Z20(3)+a0(3)*Z10(1)+a0(6)*Z10(2)+a0(9)*Z10(3);
j=j+0.001;
X1=[X1 x10];
J=[J j];
S1=[S1 Sopt01];
S2=[S2 Sopt02];
S3=[S3 Sopt03];
end
K=j;
for k=j:0.001:j+1.499;
ct=floor(1500-((k-j)/h));
% Matrice P
z1=y1(ct);
z2=y2(ct);
z3=y3(ct);
z4=y4(ct);
z5=y5(ct);
z6=y6(ct);
z7=y7(ct);
z8=y8(ct);
z9=y9(ct);
% Matrice V
z11=y10(ct);
z12=y11(ct);
z13=y12(ct);
z14=y13(ct);
z15=y14(ct);
z16=y15(ct);
z17=y16(ct);
z18=y17(ct);
z19=y18(ct);
% Matrice H
z21=y19(ct);
z22=y20(ct);
z23=y21(ct);
z24=y22(ct);
z25=y23(ct);
z26=y24(ct);
z27=y25(ct);
z28=y26(ct);
z29=y27(ct);
P=[z1 z4 z7;z2 z5 z8;z3 z6 z9];
V=[z11 z14 z17;z12 z15 z18;z13 z16 z19];
H=[z21 z24 z27;z22 z25 z28;z23 z26 z29];
deltaf1=cos(k)*(1+0.05*sin(4*k)+0.1*cos(k));
deltaf2=sin(k)*cos(k)*(1+0.05*sin(4*k)+0.1*cos(k));
deltaf3=(sin(k)^2)*(1+0.05*sin(4*k)+0.1*cos(k));
deltaf4=cos(k+h/2)*(1+0.05*sin(4*(k+h/2))+0.1*cos(k+h/2));
deltaf5=sin(k+h/2)*cos(k+h/2)*(1+0.05*sin(4*(k+h/2))+0.1*cos(k+h/2));
deltaf6=(sin(k+h/2)^2)*(1+0.05*sin(4*(k+h/2))+0.1*cos(k+h/2));
deltaf7=cos(k+h)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h));
deltaf8=sin(k+h)*cos(k+h)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h));
deltaf9=(sin(k+h)^2)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h));
deltag1=0.01*sin(k+2.1)*(u0(1)-0.5*u0(3));
deltag2=0.01*cos(k)*(-0.2*u0(2)+0.8*u0(3));
deltag3=0.01*cos(k+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag4=0.01*sin(k+(h/2)+2.1)*(u0(1)-0.5*u0(3));
deltag5=0.01*cos(k+h/2)*(-0.2*u0(2)+0.8*u0(3));
deltag6=0.01*cos(k+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag7=0.01*sin(k+h+2.1)*(u0(1)-0.5*u0(3));
deltag8=0.01*cos(k+h)*(-0.2*u0(2)+0.8*u0(3));
deltag9=0.01*cos(k+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
% Methode de Runge Kutta d'ordre 4
l11=-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3));
l12=x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3));
l13=-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3));
l21=-(x20+(h/2)*l12)*(x30+(h/2)*l13)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l22=(x10+(h/2)*l11)*(x30+(h/2)*l13)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l23=-(1/3)*(x10+(h/2)*l11)*(x20+(h/2)*l12)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l31=-(x20+(h/2)*l22)*(x30+(h/2)*l23)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l32=(x10+(h/2)*l21)*(x30+(h/2)*l23)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l33=-(1/3)*(x10+(h/2)*l21)*(x20+(h/2)*l22)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l41=-(x20+h*l32)*(x30+h*l33)+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3));
l42=(x10+h*l31)*(x30+h*l33)+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3));
l43=-(1/3)*(x10+h*l31)*(x20+h*l32)+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3));
x1=x10+(h/6)*(l11+2*l21+2*l31+l41);
x2=x20+(h/6)*(l12+2*l22+2*l32+l42);
x3=x30+(h/6)*(l13+2*l23+2*l33+l43);
q1=-(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
q2=-x20*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q3=-sin(k)*(1+0.05*sin(4*k)+0.1*cos(k))+cos(k)*(0.2*cos(4*k)-0.1*sin(k))+0.01*cos(k+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*k);
fi1=q1+q2+q3;
q4=(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
q5=x10*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q6=(cos(k)^2)*(1+0.05*sin(4*k)+0.1*cos(k))-(sin(k)^2)*(1+0.05*sin(4*k)+0.1*cos(k));
q7=sin(k)*cos(k)*(0.2*cos(4*k)-0.1*sin(k))-0.01*sin(k)*(-0.2*u0(2)+0.8*u0(3));
q8=(0.5^3)*cos(0.5*k)*cos(k)-(0.5^2)*sin(0.5*k)*sin(k)-(0.5^2)*sin(0.5*k)*sin(k)+0.5*cos(0.5*k)*cos(k);
fi2=q4+q5+q6+q7+q8;
q9=-(1/3)*x20*(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)));
q10=-(1/3)*x10*(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)));
q11=2*sin(k)*cos(k)*(1+0.05*sin(4*k)+0.1*cos(k))+(sin(k)^2)*(0.2*cos(4*k)-0.1*sin(k));
q12=-0.01*sin(k+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
q13=(0.5^3)*cos(0.5*k)*sin(k)+(0.5^2)*sin(0.5*k)*cos(k)+(0.5^2)*sin(0.5*k)*cos(k)+0.5*cos(0.5*k)*sin(k);
fi3=q9+q10+q11+q12+q13;
fi=[fi1 fi2 fi3]';
gama=[0.01*sin(k+2.1)+1 1.2 -0.005*sin(k+2.1)+1.5;1.5 -0.002*cos(k)+1 0.008*cos(k)+1.2;-0.002*cos(k+1.3)+1.2 -0.01*cos(k+1.3)+1.5 0.007*cos(k+1.3)+1];
qb1=-(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qb2=-(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qb3=-x20*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qb4=-x20*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qb5=-sin(k+(h/2))*(1+0.05*sin(4*(k+(h/2)))+0.1*cos(k+(h/2)))+cos(k+(h/2))*(0.2*cos(4*(k+(h/2)))-0.1*sin(k+(h/2)))+0.01*cos(k+(h/2)+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(k+(h/2)));
qb6=-sin(k+h)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h))+cos(k+h)*(0.2*cos(4*(k+h))-0.1*sin(k+h))+0.01*cos(k+h+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(k+h));
qb7=(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qb8=(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qb9=x10*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qb10=x10*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qb11=(cos(k+(h/2))^2)*(1+0.05*sin(4*(k+(h/2)))+0.1*cos(k+(h/2)))-(sin(k+(h/2))^2)*(1+0.05*sin(4*(k+(h/2)))+0.1*cos(k+(h/2)));
qb12=(cos(k+h)^2)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h))-(sin(k+h)^2)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h));
qb13=sin(k+(h/2))*cos(k+(h/2))*(0.2*cos(4*(k+(h/2)))-0.1*sin(k+(h/2)))-0.01*sin(k+(h/2))*(-0.2*u0(2)+0.8*u0(3));
qb14=sin(k+h)*cos(k+h)*(0.2*cos(4*(k+h))-0.1*sin(k+h))-0.01*sin(k+h)*(-0.2*u0(2)+0.8*u0(3));
qb15=(0.5^3)*cos(0.5*(k+(h/2)))*cos(k+(h/2))-(0.5^2)*sin(0.5*(k+(h/2)))*sin(k+(h/2))-(0.5^2)*sin(0.5*(k+(h/2)))*sin(k+(h/2))+0.5*cos(0.5*(k+(h/2)))*cos(k+(h/2));
qb16=(0.5^3)*cos(0.5*(k+h))*cos(k+h)-(0.5^2)*sin(0.5*(k+h))*sin(k+h)-(0.5^2)*sin(0.5*(k+h))*sin(k+h)+0.5*cos(0.5*(k+h))*cos(k+h);
qb17=-(1/3)*x20*(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qb18=-(1/3)*x20*(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qb19=-(1/3)*x10*(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qb20=-(1/3)*x10*(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qb21=2*sin(k+(h/2))*cos(k+(h/2))*(1+0.05*sin(4*(k+(h/2)))+0.1*cos(k+(h/2)))+(sin(k+(h/2))^2)*(0.2*cos(4*(k+(h/2)))-0.1*sin(k+(h/2)));
qb22=2*sin(k+h)*cos(k+h)*(1+0.05*sin(4*(k+h))+0.1*cos(k+h))+(sin(k+h)^2)*(0.2*cos(4*(k+h))-0.1*sin(k+h));
qb23=-0.01*sin(k+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qb24=-0.01*sin(k+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qb25=(0.5^3)*cos(0.5*(k+(h/2)))*sin(k+(h/2))+(0.5^2)*sin(0.5*(k+(h/2)))*cos(k+(h/2))+(0.5^2)*sin(0.5*(k+(h/2)))*cos(k+(h/2))+0.5*cos(0.5*(k+(h/2)))*sin(k+(h/2));
qb26=(0.5^3)*cos(0.5*k+h)*sin(k+h)+(0.5^2)*sin(0.5*(k+h))*cos(k+h)+(0.5^2)*sin(0.5*(k+h))*cos(k+h)+0.5*cos(0.5*(k+h))*sin(k+h);
fi11=qb1+qb3+qb5;
fi12=qb2+qb4+qb6;
fi21=qb7+qb9+qb11+qb13+qb15;
fi22=qb8+qb10+qb12+qb14+qb16;
fi31=qb17+qb19+qb21+qb23+qb25;
fi32=qb18+qb20+qb22+qb24+qb26;
gama1=[0.01*sin(k+(h/2)+2.1)+1 1.2 -0.005*sin(k+(h/2)+2.1)+1.5;1.5 -0.002*cos(k+(h/2))+1 0.008*cos(k+(h/2))+1.2;-0.002*cos(k+(h/2)+1.3)+1.2 -0.01*cos(k+(h/2)+1.3)+1.5 0.007*cos(k+(h/2)+1.3)+1];
gama2=[0.01*sin(k+h+2.1)+1 1.2 -0.005*sin(k+h+2.1)+1.5;1.5 -0.002*cos(k+h)+1 0.008*cos(k+h)+1.2;-0.002*cos(k+h+1.3)+1.2 -0.01*cos(k+h+1.3)+1.5 0.007*cos(k+h+1.3)+1];
fir1=[fi11 fi21 fi31]';
fir2=[fi12 fi22 fi32]';
Sopt=Z20+(P-V*inv(H)*V')*Z10;
v=-alfa*sign(D*Sopt);
% Methode de Runge Kutta d'ordre 4
k1=Z20;
k2=Z20+(h/2)*k1;
k3=Z20+(h/2)*k2;
k4=Z20+h*k3;
Z1=Z10+(h/6)*(k1+2*k2+2*k3+k4);
r1=fi+gama*v;
r2=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r1+(P-V*inv(H)*V')*(Z10+(h/2)*k1)));
r3=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r2+(P-V*inv(H)*V')*(Z10+(h/2)*k2)));
r4=fir2-alfa*gama2*sign(D*(Z20+h*r3+(P-V*inv(H)*V')*(Z10+k*k3)));
Z2=Z20+(h/6)*(r1+2*r2+2*r3+r4);
u=u0+h*v;
u0=u;
Z10=Z1;
Z20=Z2;
x10=x1;
x20=x2;
x30=x3;
X1=[X1 x10];
K=[K k];
end
for i=(j+1.499):0.001:5;
Soptf=Z20+(Pf-Vf*inv(Hf)*Vf')*Z10;
Soptf1=Z20(1)+af(1)*Z10(1)+af(4)*Z10(2)+af(7)*Z10(3);
Soptf2=Z20(2)+af(2)*Z10(1)+af(5)*Z10(2)+af(8)*Z10(3);
Soptf3=Z20(3)+af(3)*Z10(1)+af(6)*Z10(2)+af(9)*Z10(3);
vf=-alfa*sign(D*Soptf);
deltaf1=cos(i)*(1+0.05*sin(4*i)+0.1*cos(i));
deltaf2=sin(i)*cos(i)*(1+0.05*sin(4*i)+0.1*cos(i));
deltaf3=(sin(i)^2)*(1+0.05*sin(4*i)+0.1*cos(i));
deltaf4=cos(i+h/2)*(1+0.05*sin(4*(i+h/2))+0.1*cos(i+h/2));
deltaf5=sin(i+h/2)*cos(i+h/2)*(1+0.05*sin(4*(i+h/2))+0.1*cos(i+h/2));
deltaf6=(sin(i+h/2)^2)*(1+0.05*sin(4*(i+h/2))+0.1*cos(i+h/2));
deltaf7=cos(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
deltaf8=sin(i+h)*cos(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
deltaf9=(sin(i+h)^2)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
deltag1=0.01*sin(i+2.1)*(u0(1)-0.5*u0(3));
deltag2=0.01*cos(i)*(-0.2*u0(2)+0.8*u0(3));
deltag3=0.01*cos(i+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag4=0.01*sin(i+(h/2)+2.1)*(u0(1)-0.5*u0(3));
deltag5=0.01*cos(i+h/2)*(-0.2*u0(2)+0.8*u0(3));
deltag6=0.01*cos(i+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
deltag7=0.01*sin(i+h+2.1)*(u0(1)-0.5*u0(3));
deltag8=0.01*cos(i+h)*(-0.2*u0(2)+0.8*u0(3));
deltag9=0.01*cos(i+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
% Methode de Runge Kutta d'ordre 4
l11=-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3));
l12=x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3));
l13=-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3));
l21=-(x20+(h/2)*l12)*(x30+(h/2)*l13)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l22=(x10+(h/2)*l11)*(x30+(h/2)*l13)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l23=-(1/3)*(x10+(h/2)*l11)*(x20+(h/2)*l12)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l31=-(x20+(h/2)*l22)*(x30+(h/2)*l23)+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3));
l32=(x10+(h/2)*l21)*(x30+(h/2)*l23)+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3));
l33=-(1/3)*(x10+(h/2)*l21)*(x20+(h/2)*l22)+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3));
l41=-(x20+h*l32)*(x30+h*l33)+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3));
l42=(x10+h*l31)*(x30+h*l33)+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3));
l43=-(1/3)*(x10+h*l31)*(x20+h*l32)+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3));
x1=x10+(h/6)*(l11+2*l21+2*l31+l41);
x2=x20+(h/6)*(l12+2*l22+2*l32+l42);
x3=x30+(h/6)*(l13+2*l23+2*l33+l43);
q1=-(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
q2=-x20*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q3=-sin(i)*(1+0.05*sin(4*i)+0.1*cos(i))+cos(i)*(0.2*cos(4*i)-0.1*sin(i))+0.01*cos(i+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*i);
fi1=q1+q2+q3;
q4=(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
q5=x10*(-(1/3)*x10*x20+deltaf3+deltag3+(1.2*u0(1)+1.5*u0(2)+u0(3)));
q6=(cos(i)^2)*(1+0.05*sin(4*i)+0.1*cos(i))-(sin(i)^2)*(1+0.05*sin(4*i)+0.1*cos(i));
q7=sin(i)*cos(i)*(0.2*cos(4*i)-0.1*sin(i))-0.01*sin(i)*(-0.2*u0(2)+0.8*u0(3));
q8=(0.5^3)*cos(0.5*i)*cos(i)-(0.5^2)*sin(0.5*i)*sin(i)-(0.5^2)*sin(0.5*i)*sin(i)+0.5*cos(0.5*i)*cos(i);
fi2=q4+q5+q6+q7+q8;
q9=-(1/3)*x20*(-x20*x30+deltaf1+deltag1+(u0(1)+1.2*u0(2)+1.5*u0(3)));
q10=-(1/3)*x10*(x10*x30+deltaf2+deltag2+(1.5*u0(1)+u0(2)+1.2*u0(3)));
q11=2*sin(i)*cos(i)*(1+0.05*sin(4*i)+0.1*cos(i))+(sin(i)^2)*(0.2*cos(4*i)-0.1*sin(i));
q12=-0.01*sin(i+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
q13=(0.5^3)*cos(0.5*i)*sin(i)+(0.5^2)*sin(0.5*i)*cos(i)+(0.5^2)*sin(0.5*i)*cos(i)+0.5*cos(0.5*i)*sin(i);
fi3=q9+q10+q11+q12+q13;
fi=[fi1 fi2 fi3]';
gama=[0.01*sin(i+2.1)+1 1.2 -0.005*sin(i+2.1)+1.5;1.5 -0.002*cos(i)+1 0.008*cos(i)+1.2;-0.002*cos(i+1.3)+1.2 -0.01*cos(i+1.3)+1.5 0.007*cos(i+1.3)+1];
qd1=-(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qd2=-(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)))*x30;
qd3=-x20*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qd4=-x20*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qd5=-sin(i+(h/2))*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)))+cos(i+(h/2))*(0.2*cos(4*(i+(h/2)))-0.1*sin(i+(h/2)))+0.01*cos(i+(h/2)+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(i+(h/2)));
qd6=-sin(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h))+cos(i+h)*(0.2*cos(4*(i+h))-0.1*sin(i+h))+0.01*cos(i+h+2.1)*(u0(1)-0.5*u0(3))-(0.5^2)*sin(0.5*(i+h));
qd7=(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qd8=(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)))*x30;
qd9=x10*(-(1/3)*x10*x20+deltaf6+deltag6+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qd10=x10*(-(1/3)*x10*x20+deltaf9+deltag9+(1.2*u0(1)+1.5*u0(2)+u0(3)));
qd11=(cos(i+(h/2))^2)*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)))-(sin(i+(h/2))^2)*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)));
qd12=(cos(i+h)^2)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h))-(sin(i+h)^2)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h));
qd13=sin(i+(h/2))*cos(i+(h/2))*(0.2*cos(4*(i+(h/2)))-0.1*sin(i+(h/2)))-0.01*sin(i+(h/2))*(-0.2*u0(2)+0.8*u0(3));
qd14=sin(i+h)*cos(i+h)*(0.2*cos(4*(i+h))-0.1*sin(i+h))-0.01*sin(i+h)*(-0.2*u0(2)+0.8*u0(3));
qd15=(0.5^3)*cos(0.5*(i+(h/2)))*cos(i+(h/2))-(0.5^2)*sin(0.5*(i+(h/2)))*sin(i+(h/2))-(0.5^2)*sin(0.5*(i+(h/2)))*sin(i+(h/2))+0.5*cos(0.5*(i+(h/2)))*cos(i+(h/2));
qd16=(0.5^3)*cos(0.5*(i+h))*cos(i+h)-(0.5^2)*sin(0.5*(i+h))*sin(i+h)-(0.5^2)*sin(0.5*(i+h))*sin(i+h)+0.5*cos(0.5*(i+h))*cos(i+h);
qd17=-(1/3)*x20*(-x20*x30+deltaf4+deltag4+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qd18=-(1/3)*x20*(-x20*x30+deltaf7+deltag7+(u0(1)+1.2*u0(2)+1.5*u0(3)));
qd19=-(1/3)*x10*(x10*x30+deltaf5+deltag5+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qd20=-(1/3)*x10*(x10*x30+deltaf8+deltag8+(1.5*u0(1)+u0(2)+1.2*u0(3)));
qd21=2*sin(i+(h/2))*cos(i+(h/2))*(1+0.05*sin(4*(i+(h/2)))+0.1*cos(i+(h/2)))+(sin(i+(h/2))^2)*(0.2*cos(4*(i+(h/2)))-0.1*sin(i+(h/2)));
qd22=2*sin(i+h)*cos(i+h)*(1+0.05*sin(4*(i+h))+0.1*cos(i+h))+(sin(i+h)^2)*(0.2*cos(4*(i+h))-0.1*sin(i+h));
qd23=-0.01*sin(i+(h/2)+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qd24=-0.01*sin(i+h+1.3)*(-0.2*u0(1)-u0(2)+0.7*u0(3));
qd25=(0.5^3)*cos(0.5*(i+(h/2)))*sin(i+(h/2))+(0.5^2)*sin(0.5*(i+(h/2)))*cos(i+(h/2))+(0.5^2)*sin(0.5*(i+(h/2)))*cos(i+(h/2))+0.5*cos(0.5*(i+(h/2)))*sin(i+(h/2));
qd26=(0.5^3)*cos(0.5*i+h)*sin(i+h)+(0.5^2)*sin(0.5*(i+h))*cos(i+h)+(0.5^2)*sin(0.5*(i+h))*cos(i+h)+0.5*cos(0.5*(i+h))*sin(i+h);
fi11=qd1+qd2+qd5;
fi12=qd2+qd4+qd6;
fi21=qd7+qd9+qd11+qd13+qd15;
fi22=qd8+qd10+qd12+qd14+qd16;
fi31=qd17+qd19+qd21+qd23+qd25;
fi32=qd18+qd20+qd22+qd24+qd26;
gama1=[0.01*sin(i+(h/2)+2.1)+1 1.2 -0.005*sin(i+(h/2)+2.1)+1.5;1.5 -0.002*cos(i+(h/2))+1 0.008*cos(i+(h/2))+1.2;-0.002*cos(i+(h/2)+1.3)+1.2 -0.01*cos(i+(h/2)+1.3)+1.5 0.007*cos(i+(h/2)+1.3)+1];
gama2=[0.01*sin(i+h+2.1)+1 1.2 -0.005*sin(i+h+2.1)+1.5;1.5 -0.002*cos(i+h)+1 0.008*cos(i+h)+1.2;-0.002*cos(i+h+1.3)+1.2 -0.01*cos(i+h+1.3)+1.5 0.007*cos(i+h+1.3)+1];
fir1=[fi11 fi21 fi31]';
fir2=[fi12 fi22 fi32]';
% Methode de Runge Kutta d'ordre 4
k1=Z20;
k2=Z20+(h/2)*k1;
k3=Z20+(h/2)*k2;
k4=Z20+h*k3;
Z1=Z10+(h/6)*(k1+2*k2+2*k3+k4);
r1=fi+gama*vf;
r2=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r1+(Pf-Vf*inv(Hf)*Vf')*(Z10+(h/2)*k1)));
r3=fir1-alfa*gama1*sign(D*(Z20+(h/2)*r2+(Pf-Vf*inv(Hf)*Vf')*(Z10+(h/2)*k2)));
r4=fir2-alfa*gama2*sign(D*(Z20+h*r3+(Pf-Vf*inv(Hf)*Vf')*(Z10+h*k3)));
Z2=Z20+(h/6)*(r1+2*r2+2*r3+r4);
u=u0+h*vf;
u0=u;
Z10=Z1;
Z20=Z2;
x10=x1;
x20=x2;
x30=x3;
X1=[X1 x10];
end
end
save paramx X1 J S1 S2 S3 K
plot(X1)