//
// Script to read and process sorted text files from 140ft telescope logs
// This is to be used for many actions.

//
// Open the file
// slurp in the data
// chop off any cruft
// extract the header row
// sort it by column 2 then column 1
cd('C:\Documents and Settings\jford\My Documents\scilab');
[a,head] = fscanfMat('pol2007_1_29_18_38_50.txt');
head = head($,1:$);
// sort it, in some obtuse way
col = 1;
[s,k]=gsort(a,'r','i');
b = a(k(:,col),:);
//plot(a(1:$,3)) // plot 3rd column of file
//clf('reset');plot(b(1:$,3:4)) // plot columns 3 and 4
//clf('reset');plot(b(1:$,13:14)) // plot columns 13 and 14
clf('reset');
// Calculate velocity from position change
vel=(b(2:$,13)-b(1:$-1,13))/0.020 ;
// Get the points that are non-zero
vel = vel;
velnotzero=find(vel(1:$,1)<>0);
// plot 'em
//plot(vel(velnotzero,1)) ;
// Now run the velocity estimator and plot it out
// in another color.
// The state vector for this is 3 columns, ordered X,V,A
sv=zeros(size(b,'r'),6);
alp=0.5;
bet = (alp*alp/(2-alp));
//bet=0.1;
gam= 0.08;
dtp=0.020;
dtv=0.002;

//
// find the first reading of the encoder
j = 1;
Xmeas = b(j,13);
sv(j,1) = Xmeas;
sv(j,2) = vel(velnotzero(1),1);//(b(j,13) - b(j-1,13))/0.020;
//sv(j,3) = 0.001;
j=j+1;
while b(j,13)-b(j-1,13) == 0
// prime the pump
Xmeas = b(j,13);
sv(j,1) = Xmeas;
sv(j,2) = vel(velnotzero(1),1);//(b(j,13) - b(j-1,13))/0.020;
//sv(j,3) = 0.0010;
j=j+1;
end

for i = j:size(b,1)
// if the position command updated, then use it and update the filter, 
// otherwise just propagate the estimated values
  if b(i-1,13) - b(i,13) <> 0 then
    Xmeas = b(i,13);
    sv(i,6) = (Xmeas - b(i-1,13))/dtp;
    sv(i,5) = sv(i-1,1) + sv(i-1,2)*dtv + dtv*dtv*0.5*sv(i-1,3);
    sv(i,4) = Xmeas - sv(i,5);
    // update the filter's measurements using measured position
    sv(i,1) = sv(i,5) + alp*sv(i,4);
    sv(i,2) = sv(i-1,2) + bet/dtp*sv(i,4)+ dtp*sv(i-1,3);
  //sv(i,2) = sv(i,1)- sv(i-1,1) + bet*sv(i,4) + dtp*sv(i-1,3);
    sv(i,3) = sv(i-1,3) + gam/dtp*sv(i,4);
  else // just propagate the state vector values forward
    sv(i,1) = sv(i-1,1) + sv(i-1,2)*dtv + dtv*dtv*0.5*sv(i-1,3);
    sv(i,2) = sv(i-1,2) + dtv*sv(i-1,3);
    sv(i,3) = sv(i-1,3);
    sv(i,4) = sv(i-1,4);
    sv(i,6) = sv(i-1,6);
  end //end else
end // end for loop

clf('reset');
//plot(sv(13000:1:20000,1));
//plot(b(1:1:20000,13),'cyan');
plot(sv(13000:1:20000,6),'magenta');;
plot(sv(13000:1:20000,2),'red');
plot(sv(13000:1:20000,3),'green');
//plot(vel(velnotzero,1),'cya');
//plot(sv(1:10:20000,2),'red');//plot(sv(5001:5051,1));
//plot(b(5001:5051,13),'cya');;
//plot(sv(1:$,1));
//plot(b(11:$,13),'cya');;