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 = 0.1;   % Gain2
g1 = -5
g2 = -0.01
% 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_uneq(act, t, ts2);
  
  stab(n) =  sum(act2(1:end-1).*diff(signal));
  
end

% figure(4)
% clf
% plot(t2(1:end-1), diff(signal)/ts2,'g')
% hold on
% plot(t(1:end-1),diff(meas)/ts,'b')
% stairs(t,act,'r')
% %plot(t(1:end-1),diff(signal2)/ts,'g:')
% legend('diff(signal)','diff(meas)','act')%,'diff(signal2)')

figure(6)
% clf
hold on
plot(ft_stab,stab,'r')
grid on
xlabel('Tumbling Frequency [Hz]')
ylabel('Stability >0 == stable')
legend('g1=0.075, g2=0.1', 'g1=-40, g2=-0.02','g1=-95, g2=-0.001')