% Computes linearized dynamics of the form: % [x(t+dt) - xstar1; 1] = A [x(t) - xstar0; 1] + B (u(t) - ustar0) % xstar1 might be your target state at the next time % xstar0 might be your target state at the current time % ustar0 might be your trim control input at current time function [A , B] = linearized_heli_dynamics(xstar0, ... xstar1, ustar0, dt, model, idx) x1 = f_heli(xstar0, ustar0, dt, model, idx); A = []; B = []; epsilon = ones(1, max(size(xstar0))) * .01; for i=1:max(size(epsilon)) delta = zeros(size(xstar0)); delta(i) = epsilon(i); fx_t1m = f_heli(xstar0 - delta, ustar0, dt, model, idx); fx_t1p = f_heli(xstar0 + delta, ustar0, dt, model, idx); A = [ A (fx_t1p - fx_t1m)/epsilon(i)/2 ]; end epsilon = ones(1,max(size(ustar0))) * .01; for i=1:max(size(ustar0)) delta = zeros(size(ustar0)); delta(i) = epsilon(i); fx_t1m = f_heli(xstar0, ustar0 - delta, dt, model, idx); fx_t1p = f_heli(xstar0, ustar0 + delta, dt, model, idx); B = [ B (fx_t1p - fx_t1m)/epsilon(i)/2 ]; end A = [A (x1-xstar1)]; A = [A; zeros(1,size(A,1)) 1]; B = [B; zeros(1,size(B,2))];