-
Notifications
You must be signed in to change notification settings - Fork 3
/
Solve_MPC.m
98 lines (73 loc) · 3.91 KB
/
Solve_MPC.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
function ControlIncrement_MPC = Solve_MPC(CurrentStates, LThrusterForceX,...
RThrusterForceX, NumberOfControls, ...
Control_Horizon, ...
Prediction_Horizon, ...
Ts_MPC, Q1, Input_Weight, ...
Future_Predicted_Output, ...
OutputConstraints_MPC, ...
ControlIncrementConstraints_MPC, ...
ControlConstraints_MPC, ops)
% ---------------------------------------------------------------------
% Current States
% MotionConfig.LinearizationPoint = InitStates;
MotionConfig.LinearizationPoint = CurrentStates;
MotionConfig.ThrusterForces = [LThrusterForceX;RThrusterForceX];
% Linearization to find State Stape Equation
[A,B,C,D] = SystemMatrixCalculator(MotionConfig);
% Convert Discrete State Space Model
% [A_Dis, B_Dis, C_Dis] = ContinousToDiscrete(A,B,C,D,TsPlant);
[A_Dis, B_Dis, C_Dis] = c2dm(A,B,C,D,Ts_MPC,'zoh');
%A_Dis = TsPlant * A + eye(7);
%B_Dis = TsPlant * B;
%C_Dis = C;
% Construct Yalmip problem for MIQP - Path Planner
yalmip('clear');
delta_Thrusts_MPC = sdpvar(NumberOfControls, Control_Horizon, 'full');
Constraints_MPC = [];
objective_MPC = 0;
Thruster_Vector = [LThrusterForceX; RThrusterForceX];
PredictedState_MPC = [CurrentStates; 0];
Constraints_MPC = [];
for k = 1:Control_Horizon
% Calculate Thrust
Thruster_Vector = Thruster_Vector + delta_Thrusts_MPC(:,k);
% Provide formula for u{k}: state update
PredictedState_MPC = A_Dis * PredictedState_MPC + B_Dis * Thruster_Vector;
Future_Surge_Yaw = [Future_Predicted_Output(2*k-1); ...
Future_Predicted_Output(2*k)];
Surge_Yaw = [PredictedState_MPC(4); ...
PredictedState_MPC(3)];
% Calculate objective
objective_MPC = objective_MPC + (Surge_Yaw - Future_Surge_Yaw)' *...
[Q1(2*k-1,2*k-1) 0; 0 Q1(2*k,2*k)] * ...
(Surge_Yaw - Future_Surge_Yaw) + ...
delta_Thrusts_MPC(:,k)' * [Input_Weight 0; 0 Input_Weight]...
* delta_Thrusts_MPC(:,k);
% Add constraints on OutputState for MPC
Constraints_MPC = [Constraints_MPC, OutputConstraints_MPC(:,1) <= ...
Surge_Yaw <= OutputConstraints_MPC(:,2)];
% Add Constraints on Control Increment
Constraints_MPC = [Constraints_MPC, ControlIncrementConstraints_MPC(:,1) <= ...
delta_Thrusts_MPC(:,k) <= ControlIncrementConstraints_MPC(:,2)];
% Add constraints on Control Input
Constraints_MPC = [Constraints_MPC, ControlConstraints_MPC(:,1) <= ...
Thruster_Vector <= ControlConstraints_MPC(:,2)];
end
for k = Control_Horizon:Prediction_Horizon
% Provide formula for u{k}: state update
PredictedState_MPC = A_Dis * PredictedState_MPC + B_Dis * Thruster_Vector;
Future_Surge_Yaw = [Future_Predicted_Output(2*k-1); ...
Future_Predicted_Output(2*k)];
Surge_Yaw = [PredictedState_MPC(4); ...
PredictedState_MPC(3)];
% Calculate objective
objective_MPC = objective_MPC + (Surge_Yaw - Future_Surge_Yaw)' *...
[Q1(2*k-1,2*k-1) 0; 0 Q1(2*k,2*k)] * ...
(Surge_Yaw - Future_Surge_Yaw);
% Add constraints on OutputState for MPC
Constraints_MPC = [Constraints_MPC, OutputConstraints_MPC(:,1) <= ...
Surge_Yaw <= OutputConstraints_MPC(:,2)];
end
OPT = optimize(Constraints_MPC, objective_MPC, ops);
ControlIncrement_MPC = value(delta_Thrusts_MPC(:,1));
end