diff options
Diffstat (limited to '')
| -rw-r--r-- | uav_ctrl_lqr.m | 39 |
1 files changed, 39 insertions, 0 deletions
diff --git a/uav_ctrl_lqr.m b/uav_ctrl_lqr.m new file mode 100644 index 0000000..8063c64 --- /dev/null +++ b/uav_ctrl_lqr.m @@ -0,0 +1,39 @@ +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% +% Design a nominal controller for UAV. + +function [ctrl_lqr] = uav_ctrl_lqr(params, model) + +% ------------------------------------------------------------------------ +% Design a Kalman filter for state estimation + +G = model.linear.StateSpace +[L, P, E] = lqe(G.A, G. + +% ------------------------------------------------------------------------ +% Design a nominal LQR controller + +% Define penalties according to following priorities + +q_pos = 1; % penalty on position +q_vel = 10; % penalty on linear velocity +q_ang = 10; % high penalty on angles +q_rate = 10; % very high penalty on angular velocities + +r_ang = 1; % flap movement is cheap +r_thrust = 10; % thrust is more expensive on the battery + +% LQR design matrices +Q = kron(diag([q_pos, q_vel, q_ang, q_rate]), eye(3)); +R = diag([r_ang, r_ang, r_ang, r_ang, r_thrust]); + +% Compute controller using output lqr +[K, S, poles] = lqry(model.linear.StateSpace, Q, R); + +% ------------------------------------------------------------------------ +% Save controller + +ctrl_lqr = struct('Name', 'LQR', 'K', K); + +end +% vim: ts=2 sw=2 et: |