% 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))];