initial_lqr.m

% Given the state-space linearization, diagonal weighting matrices, and
% update period, compute the discrete LQR model for the system, described
% in Matt Rich's thesis section 7.3
function [lqr_K, lqr_S, lqr_E] = initial_lqr(A, B, state_weights, command_weights, T)
    lqr_Q = diag(1./state_weights.^2);
    lqr_R = diag(1./command_weights.^2);
    [lqr_K, lqr_S, lqr_E] = lqrd(A, B, lqr_Q, lqr_R, zeros(12, 4), T);
end