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 = -10.0; % Gain1 - akt = g2*(meas-meas_old + g1*act);
g2 = -0.1;   % Gain2


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


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  =  sum(act2(1:end-1).*diff(signal));



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)')