-
Notifications
You must be signed in to change notification settings - Fork 3
/
MPC_DampingForces.m
61 lines (45 loc) · 1.1 KB
/
MPC_DampingForces.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
function Fwd = MPC_DampingForces(States,Enable)
% This function calculates the water damping forces
global CDW wi l d roW
if (Enable)
% Extract Velocities
h0 = d; % Height Subject to Water
u = States(4); % Surge Lin. Vel.
v = States(5); % Sway Lin. Vel.
r = States(6); % Yaw Ang. Vel.
%Linear Damping Forces
Xu = -5.6128; %-5.6128; % 5;
Yv = -3.5059; %-3.5059; % 5;
Yr = -0.0001; % Yr;
Nv = Yr; % 5;
%Nv = %-1.0005; % Yr;
Nr = -0.1039; % 5;
%Zw = 150;
%Kp = 5;
%Mq = 5;
%Linear Damping Matrix
Dl = -[Xu 0 0;...
0 Yv Yr;...
0 Nv Nr];
%Nonlinear Damping Forces
%Cross-sectional areas
AUT = wi*h0;
AUL = h0*l;
AWP = l*wi;
Xuu = -2.3136; % 1/2*roW*CDW*AUT;
Yvv = -3.9310; % 1/2*roW*CDW*AUL;
%Zww = 1/2*roW*CDW*AWP;
%Kpp = 1/2*roW*CDW*((AUT+AUL)*2+AWP);
%Mqq = 1/2*roW*CDW*((AUT+AUL)*2+AWP);
Nrr = -0.2605; % 1/2*roW*CDW*((AUT+AUL)*2+AWP);
%Nonlinear damping matrix
Dn = -[ Xuu*abs(u) 0 0;...
0 Yvv*abs(v) 0;...
0 0 Nrr*abs(r)];
%Total damping matrix
D = -(Dl+Dn);
Fwd = D*States(4:end);
else
Fwd = zeros(3,1);
end
end