summaryrefslogtreecommitdiffstats
path: root/templates
diff options
context:
space:
mode:
authorYanzhenXiangRobotics <xyz000327@gmail.com>2023-05-01 21:44:20 +0200
committerYanzhenXiangRobotics <xyz000327@gmail.com>2023-05-01 21:44:20 +0200
commit48b79cc6c5ff67842f68a24b1953fcd177d6404e (patch)
tree9e09f663949557dac95e49545b7068c1072a9841 /templates
parentADD: not using traj_constr version (not correct) (diff)
downloadmpc_pe-48b79cc6c5ff67842f68a24b1953fcd177d6404e.tar.gz
mpc_pe-48b79cc6c5ff67842f68a24b1953fcd177d6404e.zip
ADD; pass 10
Diffstat (limited to 'templates')
-rw-r--r--templates/lqr_tuning.m54
1 files changed, 31 insertions, 23 deletions
diff --git a/templates/lqr_tuning.m b/templates/lqr_tuning.m
index 8e6edc1..dc7161b 100644
--- a/templates/lqr_tuning.m
+++ b/templates/lqr_tuning.m
@@ -16,31 +16,39 @@ function [tuning_struct, i_opt] = lqr_tuning(x0,Q,params)
for i = 1:param_choices
Q_mat = diag(Q(:,i));
ctrl = LQR(Q_mat,R,params);
- [Xt, Ut, ctrl_info] = simulate(x0, ctrl, params);
- ctrl_feas_arr = [];
- for k = 1:params.model.HorizonLength
- ctrl_feas_arr = [ctrl_feas_arr,ctrl_info(k).ctrl_feas];
- end
- if all(ctrl_feas_arr)
- J = 0;
- for j = 1:params.model.HorizonLength
- J = J + Xt(:,j)'*Q_mat*Xt(:,j);
- J = J + Ut(:,j)'*R*Ut(:,j);
- end
- J = J + Xt(:,end)'*Q_mat*Xt(:,end);
- J_array = [J_array, J];
- tuning_struct_item.TrajFeasible = true;
- else
- tuning_struct_item.TrajFeasible = false;
- end
+ [Xt, Ut, ~] = simulate(x0, ctrl, params);
+ [s_max, y_max, u_max, J_u, df_max, vf_max, traj_feas] = traj_constraints(Xt,Ut,params);
+ % YOUR CODE HERE
+ % ctrl_feas_arr = [];
+ % for k = 1:params.model.HorizonLength
+ % ctrl_feas_arr = [ctrl_feas_arr,ctrl_info(k).ctrl_feas];
+ % end
+ % if all(ctrl_feas_arr)
+ % J = 0;
+ % for j = 1:params.model.HorizonLength
+ % J = J + Xt(:,j)'*Q_mat*Xt(:,j);
+ % J = J + Ut(:,j)'*R*Ut(:,j);
+ % end
+ % J = J + Xt(:,end)'*Q_mat*Xt(:,end);
+ % J_array = [J_array, J];
+ % tuning_struct_item.TrajFeasible = true;
+ % else
+ % tuning_struct_item.TrajFeasible = false;
+ % end
tuning_struct_item.InitialCondition = x0;
tuning_struct_item.Qdiag = Q(:,i);
- tuning_struct_item.MaxAbsPositionXZ = params.constraints.MaxAbsPositionXZ;
- tuning_struct_item.MaxAbsPositionY = params.constraints.MaxAbsPositionY;
- tuning_struct_item.MaxAbsThrust = params.constraints.MaxAbsThrust;
- tuning_struct_item.InputCost = J_array(i);
- tuning_struct_item.MaxFinalPosDiff = params.constraints.MaxFinalPosDiff;
- tuning_struct_item.MaxFinalVelDiff = params.constraints.MaxFinalVelDiff;
+ tuning_struct_item.MaxAbsPositionXZ = s_max;
+ tuning_struct_item.MaxAbsPositionY = y_max;
+ tuning_struct_item.MaxAbsThrust = u_max;
+ tuning_struct_item.InputCost = J_u;
+ tuning_struct_item.MaxFinalPosDiff = df_max;
+ tuning_struct_item.MaxFinalVelDiff = vf_max;
+ tuning_struct_item.TrajFeasible = traj_feas;
+
+ if traj_feas
+ J_array = [J_array, J_u];
+ end
+
tuning_struct = [tuning_struct,tuning_struct_item];
end
% display(J_array)