ReinforcementLearning-QLearning-based-self-tuned-PID-controller-for-AUV-MatLab/AUV_reset.m at main · AlinaBaber/ReinforcementLearning-QLearning-based-self-tuned-PID-controller-for-AUV-MatLab · GitHub

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
%function AUV_reset()
global W;
global B;
global m;
global xg;
global yg;
global zg;
global xb;
global yb;
global zb;
global I;
global X;
global Y;
global Z;
global K;
global M;
global N;
global DD;
global DDD;
global Sv;
clear;
clc;
num=6000;
d1 = 0.191;
l1 = 1.33;
d = 0.2;
l = 2.135;
g=9.8;
W1 = 299;
B1 = 306;
m1 = W1/g;
m=2.18+3.33+25+3.33+8.6;
Bm=2.88+1.14+28.59+1.14+10.1;
W = m*g;
B = Bm*g;
xg=0;
yg=0;
zg=0.05;
xb=0.01;
yb=0;
zb=0;
I.xx=0.177/(W1*l1*d1^2)*(W*l*d^2);
I.yy=3.45/(W1*l1^2*d1^2)*(W*l^2*d^2);
I.zz=3.45/(W1*l1^2*d1^2)*(W*l^2*d^2);
X.u_u = -1.62/(d1^2)*(d^2);
X.ua = -0.93/0.3585*0.02071/(l1*d1^2)*(l*d^2);
Y.v_v = -131/(d1*l1)*(d*l);
Y.r_r = 0.632/(l1^3*d1)*(l^3*d);
Y.va = -35.5/(l1*d1^2)*(l*d^2);
Y.ra = 1.93/(l1^2*d1^2)*(l^2*d^2);
Z.w_w = -131/(d1*l1)*(d*l);
Z.q_q = -0.632/(l1^3*d1)*(l^3*d);
Z.wa = Y.va;
Z.qa = -1.93/(l1^2*d1^2)*(l^2*d^2);
K.p_p = Y.v_v*0.001;
K.pa = -0.0704*l/l1;
M.w_w = 3.18/(l1^2*d1)*(l^2*d);
M.q_q = -188/(l1^4*d1)*(l^4*d);
M.wa = -1.93/(l1^2*d1^2)*(l^2*d^2);
M.qa = -4.88/(l1^3*d1^2)*(l^3*d^2);
N.r_r = -94/(l1^4*d1)*(l^4*d);
N.v_v = -M.w_w;
N.va = 1.93/(l1^2*d1^2)*(l^2*d^2);
N.ra = -4.88/(l1^3*d1^2)*(l^3*d^2);
X.wq = Z.wa;
X.qq = Z.qa;
X.vr = -Y.va;
X.rr = -Y.ra;
Y.ur = 5.22/(l1*d1^2)*(l*d^2);
Y.wp = -Z.wa;
Y.pq = -Z.qa;
Z.uq = -Y.ur;
Z.vp = Y.va;
Z.rp = Y.ra;
M.uw = 24/(l1*d1^2)*(l*d^2);
M.vp = -Y.ra;
M.rp = 4.86/(l1^3*d1^2)*(l^3*d^2);
M.uq = -2/(l1^2*d1^2)*(l^2*d^2);
N.uv = -24/(l1*d1^2)*(l*d^2);
N.wp = Z.qa;
N.pq = -4.86/(l1^3*d1^2)*(l^3*d^2);
N.ur = -2/(l1^2*d1^2)*(l^2*d^2);
Y.uv = -28.6/(d1*l1)*(d*l);
Z.uw = Y.uv;
DD=zeros(6,6);
DD(1,1)=m-X.ua;
DD(1,5)=m*zg;
DD(1,6)=-m*yg;
DD(2,2)=m-Y.va;
DD(2,4)=-m*zg;
DD(2,6)=m*xg-Y.ra;
DD(3,3)=m-Z.wa;
DD(3,4)=m*yg;
DD(3,5)=-m*xg-Z.qa;
DD(4,2)=-m*zg;
DD(4,3)=m*yg;
DD(4,4)=I.xx-K.pa;
DD(5,1)=m*zg;
DD(5,3)=-m*xg-M.wa;
DD(5,5)=I.yy-M.qa;
DD(6,1)=-m*yg;
DD(6,2)=m*xg-N.va;
DD(6,6)=I.zz-N.ra;
DDD=DD;
DD = DD^-1;
for i=1:6;
    for j=1:6
        if abs(DD(i,j))<0.00009
            DD(i,j)=0;
        end
    end
end
Sv.x=0;
Sv.y=0.1;