From 98e1a3bc15560a206eb31f02dd2fcacf51ff19b8 Mon Sep 17 00:00:00 2001 From: Nao Pross Date: Fri, 29 Mar 2024 21:17:40 +0100 Subject: Add MATLAB code --- uav_ctrl_nominal.m | 37 +++++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 uav_ctrl_nominal.m (limited to 'uav_ctrl_nominal.m') diff --git a/uav_ctrl_nominal.m b/uav_ctrl_nominal.m new file mode 100644 index 0000000..d46a1ef --- /dev/null +++ b/uav_ctrl_nominal.m @@ -0,0 +1,37 @@ +% Copyright (C) 2024, Naoki Sean Pross, ETH Zürich +% +% Design a nominal controller for UAV. + +function [ctrl] = uav_ctrl_nominal(params, model) + +% ------------------------------------------------------------------------ +% Design a nominal LQR controller + +% Define penalties according to following priorities + +q_pos = 10; % penalty on position +q_vel = 1; % penalty on linear velocity +q_ang = 100; % high penalty on angles +q_rate = 1000; % very high penalty on angular velocities + +r_ang = 1; % flap movement is cheap +r_thrust = 10; % thrust is more expensive on the battery + +nx = model.linear.Nx; +nu = model.linear.Nu; + +% 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]); +N = zeros(nx, nu); % no cross terms + +% Compute controller +[K, S, poles] = lqr(model.linear.StateSpace, Q, R, N); + +% ------------------------------------------------------------------------ +% Save controller + +ctrl = struct(); +ctrl.K = K; + +end \ No newline at end of file -- cgit v1.2.1