clear all
clc
% this is a test of the filter in ADCS BDOT

ts = 0.1;   % bdot algorithm smaple time
ts2 = 0.01; % Resolution on B-field oscillation
ft = 0.539; % Tumble frequency
ft = 0.839
tend = 30;  % Runtime in seconds

g1 = .075; % Gain1 - akt = g2*(meas-meas_old + g1*act);
g2 =  1;   % Gain2
% g1 = -0.5
% g2 = -1
% g1 = -9.5
% g2 = -0.1

t = ts:ts:tend;
t2 = ts:ts2:tend;

for(n = 1:200)
  n
  ft = n/100;
  ft_stab(n) = ft;
  signal = sin(2*pi*t2*ft);
  meas = sin(2*pi*t*ft);
  
  act(1) = 0;
  for(k = 2:length(t))
    act(k) = bdot(meas(k),g1,g2);
  end
  
  signal2 = signal(1:ts/ts2:length(t2));
  
  [act2, act2_time] = resample_unequal(act, t, ts2);
  
  stab(n) =  sum(act2(1:end-1).*diff(signal));
  
end

stab1 = stab;

figure(6)
% clf
hold on
plot(ft_stab,stab1,'g')
grid on
xlabel('Tumbling Frequency [Hz]')
ylabel('Stability >0 == stable')
legend('g1 = 0.075, g2 = 0.1', 'g1 = -5, g2 = -0.01','g1 = -9.5, g2 = -0.1')