diff options
Diffstat (limited to 'templates')
| -rw-r--r-- | templates/lqr_tuning.m | 54 |
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) |