load_data = true;     # If true load data, only has to be done once
if load_data
 clear all;
  load_data = true;     # If true load data, only has to be done once
else
  load_data = false;     # If true load data, only has to be done once
endif
filter = false;        # If true use Kalman filter on measured phase error
real_data = true;     # If true use real data

if filter
kp = 0.01;
ki = 0.01;
else
kp = 0.01;
ki = 0.001;
endif



if load_data
if real_data
PPS_phase=load("..\\PPS 6H phase.csv");
#PPS=diff(PPS_phase(1:10001),1);
PPS=PPS_phase;#(1:10000)
TCXO_phase = load("..\\TCXO 6h phase.csv");
TCXO = TCXO_phase;#(1:10000);
else # simulated data
TCXO = 1e-6*sin(0:0.001:10)+5e-10*(rand(1,10001)-0.5);
PPS = 2e-8*(rand(1,10001)-0.5);
endif
#PPS = 1.5e-8*(rand(1,length(TCXO)+1)-0.5);
#graphics_toolkit("fltk")
endif

close all

pps = PPS;
osc = TCXO;


  phase(1) = 0;
  mes(1) = 0;
  freq(1) = 0;
  phase_predict(1) = 0;
  freq_predict(1) = 0;
  alpha = 0; #0.36;
  beta = 0; #0.08;
  f(1) = 0;
  p(1) = 0;
  err(1) = 0;
  pps_phase(1) = 0;
  sumerr = 0;
  len = min(length(pps),length(osc));

  k1(1) = alpha;
  k2(1) = beta;
  K = [alpha, beta];  # Kalman gain
  A = [1, 1;0, 1];      # System transition matrix
  X = [0;0];          # system state
  Xp = [0;0];         # System state prediction
  Q = [0 0; 0 1e-11]; # System noise
  R = 1e-8;           # measurement noise
  z = 0;              # mesured x
  H = [1 0];          # Input sensitivity (How X depends in Xp
  I = eye(2);         # Identity matrix
  P = [0 0;0 0];          # Start state variance
  
  
  for i = 2:len
    pps_phase(i) = pps_phase(i-1) + pps(i-1);
    #actual state
    phase(i) = phase(i-1) + f(i-1) + osc(i)-osc(i-1);
    freq(i) = phase(i) - phase(i-1);

    z = phase(i) - pps(i);    # observation incl measurement noise

    if filter   # false means no alpha/beta filter
      #predict
#      phase_predict(i) = mes(i-1) + freq(i-1);
#      freq_predict(i) = freq(i-1);
      Xp = A * X ;
      Pp = A * P * A.' + Q;
      K = Pp * H.' * (H * Pp * H.' + R)^-1;

      k1(i) = K(1);
      k2(i) = K(2);
      phase_predict(i) = Xp(1);
      freq_predict(i) = Xp(2);
      
     #update from measured
 #     mes(i) = phase_predict(i) + alpha * (z - phase_predict(i));
 #     freq(i) = freq_predict(i) + beta * (z - phase_predict(i));
  
      P = (I - K*H)*Pp;  
      X = Xp + K*(z - H*Xp);

      mes(i) = X(1);
      freq(i) = X(2);
      
      err(i) = -mes(i);
    else
      err(i) = - z;
    endif
    sumerr = sumerr + err(i);
    f(i) =  kp * err(i) + ki * sumerr;    # frequency correction
    #f(i) = 0;                            # remove comment to open the loop
    p(i) = p(i-1) + f(i);                 # calculate phase of correction, not needed
  endfor
#  figure()
#  plot(freq(1:1000))
#  hold on
#  plot(f(1:1000))
#  plot(diff(osc(1:1001),1))
#  plot(phase)
#  plot (PPS_phase)
#  hold off
#  legend("freq", "vtune","tcxo")
  figure()
  subplot(2,2,1)
  plot(diff(osc,1))
  hold on
  plot(freq)
  plot(freq_predict)
#  plot(f)
  legend("without", "with" , "predict")
  title("output freq")
  hold off
  subplot(2,2,2)
  plot(osc/100)
  hold on
  plot(pps)
  plot(phase)
# plot(phase_predict)
  title("output phase")

  legend("without/100" , "pps","with", "predict")
  hold off

g = phase;




#g = gpsdo(TCXO,PPS,0.01,0.0001);
#g = gpsdo(TCXO,PPS,0.01,0.0001);
subplot(2,2,3)
if false # plot inputs
plot(PPS)
hold on
plot(TCXO/100)
#plot(10*g)
legend("PPS","TCXO/100")
title("input")
else
if false  # if true plot error
plot(err)
title("Error")
else  # else plot Kalman gain
if filter
plot(k1(1:100))
hold on
plot(k2(1:100))
title("Kalman Gain");
endif
endif
endif
hold off
subplot(2,2,4)
plot_adev(TCXO, PPS,g)
legend("TCXO","PPS","GPSDO")

function retval=adev(data)
  retval = [];
  ADEV_x = [];
  ds = length(data);
  for tau =round(power(10,0:0.1:4))
    if tau< ds/4
      cnt = length(data) - 2*tau;
      retval = cat(1,retval, [tau, sqrt(sumsq(data(1:ds-2*tau) - data(1+tau:ds-tau)- data(1+tau:ds-tau) + data(1+2*tau:ds )) / (cnt*2))/tau]);
    endif
  endfor
#  retval = [ADEV_x,retval]
endfunction


function plot_adev(p)
ap = adev(p);
loglog(ap(:,1), ap(:,2));
hold on
ap2 = adev(p2);
loglog(ap2(:,1), ap2(:,2));
ap3 = adev(p3);
loglog(ap3(:,1), ap3(:,2));
hold off
title("ADEV")
endfunction




