function [zHat mHat bHat] = ...
    RangeSmoother( SC_Range, SC_UpperPressure, SC_LowerPressure, m0, b0 )
%
% PURPOSE: Recursive Least Squares Filter that estimates the slope and
%          constant of the piston position (also called range, or z) as a
%          function of gas pressure and low-frequency aperiodic piston
%          position measurments.
%          
%    NOTE: This is prototype/development code.  It's not yet the finished
%          product. 
% 
%    DATE: 10 Oct 2018, rsm, Andy H.
%
  global showPlots
  if( isempty(showPlots) ) 
    showPlots = 0;
  end
  
%
% Only one switch:
%
useForgetting     = 1;        % Use forgetting factor instead of noise:
%
% Let the games begin:
%
nPts  = length(SC_UpperPressure);
nPtsL = length(SC_LowerPressure);
nPtsR = length(SC_Range);

if( nPts ~= nPtsL || nPts ~= nPtsR || nPtsL ~= nPtsR )
  fprintf(['RangeSmoother - ERROR.  SC_UpperPressure, SC_LowerPressure and SC_Range' ... 
	   ' are not the same length; (%d,%d,%d)\n'],nPts, nPtsL, nPtsR);
  return
end

data.SC_Range = SC_Range;
data.SC_UpperPressure = SC_UpperPressure;
data.SC_LowerPressure = SC_LowerPressure;

idx = find( SC_UpperPressure~=NaN & SC_LowerPressure~=NaN );

data.SC_Range = data.SC_Range(idx);
data.SC_UpperPressure = data.SC_UpperPressure(idx);
data.SC_LowerPressure = data.SC_LowerPressure(idx);

maxThrow     = 80;         %Inches
dt           = 0.1;        %Seconds
%
% Areas of the upper and lower cylinders:
Au = pi/4*5^2;
Al = Au - pi/4*1.5^2;

%t = dt*(0:length(data.SC_Range)-1);
t = dt*[0:nPts-1];
%
% Clean up the laser-range-finder data:
%
zoh = 0;
jj = 1;
data.SampledRange = zeros(1,nPts);
for( i=1:nPts )
  %
  % Andy processes the analog measurement in the other data sets:
  value = ParseRange(data.SC_Range(i));
  if( ~isnan(value) & value < maxThrow )
    zoh = value;
    data.Sampledt(jj) = t(i);
    if( jj==1 )
      %
      % Because k starts from 2 in the loop below.
      force(1) = Au*data.SC_UpperPressure(i) - Al*data.SC_LowerPressure(i);
    end
    jj = jj+1;
  end
  data.SampledRange(i) = zoh;
end

nRangeMeas  = length(who);

%
% Assume a linear relation between piston position z and pressure,
%
% z = m*p + z0
%
% This is a line, and we're estimating the constants m and z0, or p0 if
% using a pressure model.
%
% Also beware that conceptually z is the independent variable, yet we are
% estimating the inverse function and treating z as if it depends on p.  
%
% Also beware that I'm using lower case p and p0 for pressure, and upper case
% P and P0 for the covariance matrix so the equations conform to Simon,
% page~86.
%
P = zeros(2,2,nRangeMeas-1);
% 
% P(1,1,k) is the variance of the slope, and P(2,2,k) is the variance of the
% reference pressure (pressure at z=z0).  Both of these additionally include
% modeling error.
%
% Initial guess of the slope and the reference location z0 or pressure p0 and
% standard deviation of piston position measurement noise:

xHat0 = [ m0 b0]';
sigma_z = .5;           % inches, force model
P(:,:,1) = [1e-6 0
	    0   .2];
%
xHat = NaN(2,nRangeMeas-1);
xHat(:,1) = xHat0;

indx = ones(nRangeMeas-1,1);

zHat = zeros(1,nPts);
mHat = zeros(1,nPts);
bHat = zeros(1,nPts);

lastSample = 0;
%
% Use R for noisy position updates, or the forgetting-factor gamma for slowly
% varying constants.
R = sigma_z^2;
%
%
gamma = 1;
a = 1;
if( useForgetting )
  R = 1;
  gamma = .995;
  a = (1 - gamma);
end

j = 2;

%
% RLS Equations with noise covariance R from Simon, p.86.
% RLS with gamma equations from FP&Workman, p.381.
%
% I combined the two.
%
%for k = 2:length(data.SC_Range)
for( k = 2:nPts )
  %
  % Force measurement.
  force(k) = Au*data.SC_UpperPressure(k) - Al*data.SC_LowerPressure(k);
  %
  if( data.Sampledt(j-1) <= t(k) && j < nRangeMeas )
    indx(j) = k;
    %
    % Measurement (or output) matrix:
    H = [ force(k) 1 ];
    %
    PLast = P(:,:,j-1);
    xHatLast = xHat(:,j-1);
    
    K = PLast*H'/gamma*inv(H*PLast*H'/gamma + R/a);
    
    xHat(:,j) = xHatLast + K*(data.SampledRange(k) - H*xHatLast);
    %P(:,:,j)    = (eye(2) - K*H)*PLast*(eye(2) - K*H)' + K*R*K';
    %P(:,:,j)    = (eye(2) - K*H)*PLast/gamma;
    P(:,:,j)    = (eye(2) - K*H)*PLast*(eye(2) - K*H)'/gamma + K*R*K'/a;
    j = j+1;
  end
  zHat(k) = xHat(1,j-1)*force(k) + xHat(2,j-1);
  mHat(k) = xHat(1,j-1);
  bHat(k) = xHat(2,j-1);
end


% Least squares fit:
nSamples = nRangeMeas-1;
%
% This computes range as a fn of pressure or force.
lsqw = [force(indx)' ones(nSamples,1)]\data.SampledRange(indx)';

deltaRLS = zHat(indx) - data.SampledRange(indx);

deltaBLS = ( lsqw(1)*force(indx) + lsqw(2) ) - data.SampledRange(indx);

error = 0;

if( ~showPlots ) return; end

% Plot it:
  MonitorPositions = get(0,'MonitorPositions');
  position = MonitorPositions(end,:);
  %position = get(0,'screensize');
  npposition(1:2) = position(1:2) + round([position(3)*.02 position(4)*.04]);
  npposition(3:4) = round([position(3)*.95 position(4)*.90]);
  position = npposition;

  lstr={};
  if exist( 'buoyFigPtr' ) ~= 1
    buoyFigPtr= figure;
    set( buoyFigPtr, 'Name', 'Estimated Range', 'DeleteFcn', 'clear buoyFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      buoyFontsize=fontsize;
    else
      buoyFontsize=20;
    end
    buoyFontweight='normal';
    set(gca,'fontsize',buoyFontsize,'fontweigh',buoyFontweight)
    set(gcf,'defaultaxesfontsize',buoyFontsize,'defaulttextfontsize', ...
	    buoyFontsize)
    buoyAxesPtr = gca;
  else
    figure(buoyFigPtr);
  end
%idx = 6000:6600;
idx = 1:length(data.SC_Range);
ah(1) = subplot(2,1,1);
plot(t(idx)', data.SC_Range(idx)','b');
lstr(length(lstr)+1,1) = {'Measured'};
if( max(data.SC_Range) > maxThrow )
  ylim0 = get(gca,'ylim');
  set(gca,'ylim',[ylim0(1), maxThrow+5]);
end

hold on;
grid on;

plot(t, data.SampledRange, 'k');
lstr(length(lstr)+1,1) = {'Subsampled'};

plot(t(idx)', zHat','r');
lstr(length(lstr)+1,1) = {'Recursive Least Squares'};

rangeHatBLS = lsqw(1)*force + lsqw(2);

plot(t(idx), rangeHatBLS(idx),'g');
labelStr = sprintf('Batch Least Squares; (m,b) = (%.4f %.2f)', lsqw(1),lsqw(2));
lstr(length(lstr)+1,1) = {labelStr};
%legend('Estimated Position (in)','Measured Position');
legend(lstr, 'location','best')

ylabel('Ram Displacement, Inches')

modelStr = 'Force Model';

if( useForgetting )
  modelStr = strcat( modelStr, '; Forgetting Factor');
else
  modelStr = strcat( modelStr, '; Noisy Measurment');
end

titleStr = sprintf(['Recursive Versus Batch Least Squares' ...
		    ' Estimates \n  %s'], modelStr);
title(titleStr);

rmsRLS = rms(deltaRLS(2000:end));
rmsBLS = rms(deltaBLS(2000:end));

ah(2) = subplot(2,1,2);

plot(t(indx),[deltaRLS' deltaBLS']);
grid on
ylabel('Estimation Error, Inches')
xlabel('Seconds')
rlsStr = sprintf('Recursive Least Squares; RMS Error = %.2f in',rmsRLS);
blsStr = sprintf('Batch Least Squares; RMS Error = %.2f in',rmsBLS);
legend(rlsStr,blsStr);
linkaxes(ah,'x');

%mseKF = trapz(t(idx), deltaRLS(idx)'.^2)
%mseLS = trapz(t(idx), deltaLS'.^2)

scaleGuess = -.5e4;

  lstr={};
  if exist( 'pressFigPtr' ) ~= 1
    pressFigPtr= figure;
    set( pressFigPtr, 'Name', 'Pressure', 'DeleteFcn', 'clear pressFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      buoyFontsize=fontsize;
    else
      buoyFontsize=20;
    end
    buoyFontweight='normal';
    set(gca,'fontsize',buoyFontsize,'fontweigh',buoyFontweight)
    set(gcf,'defaultaxesfontsize',buoyFontsize,'defaulttextfontsize', ...
	    buoyFontsize)
    pressAxesPtr = gca;
  else
    figure(pressFigPtr);
  end

  plot(data.SC_Range(indx), data.SC_UpperPressure(indx));
  lstr(length(lstr)+1,1) = {'Upper Chamber Pressure, PSI'};
  grid on;
  hold on;
  ylabel('Pressure')
  xlabel('Range')
  legend(lstr, 'location','best')

  titleStr = sprintf('Upper Chamber Pressure versus Ram Position');
  title(titleStr);

  lstr={};
  if exist( 'forceFigPtr' ) ~= 1
    forceFigPtr= figure;
    set( forceFigPtr, 'Name', 'Force', 'DeleteFcn', 'clear forceFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      buoyFontsize=fontsize;
    else
      buoyFontsize=20;
    end
    buoyFontweight='normal';
    set(gca,'fontsize',buoyFontsize,'fontweigh',buoyFontweight)
    set(gcf,'defaultaxesfontsize',buoyFontsize,'defaulttextfontsize', ...
	    buoyFontsize)
    pressAxesPtr = gca;
  else
    figure(forceFigPtr);
  end

  plot(data.SC_Range(indx),  force(indx));
  lstr(length(lstr)+1,1) = {'Force, Pounds'};
  grid on;
  hold on;

  plot(zHat,  force);
  legendStr = sprintf('Recursive Least Squares ');
  lstr(length(lstr)+1,1) = {legendStr};
  %
  plot(data.SC_Range(indx), 1/lsqw(1)*data.SC_Range(indx) - lsqw(2)/lsqw(1));
  legendStr = sprintf(['Batch Least Squares (1/m,-b/m) = ',...
		    '(%.4f, %.2f)'],1/lsqw(1), -lsqw(2)/lsqw(1));
  lstr(length(lstr)+1,1) = {legendStr};

  ylabel('Force, lb.')
  xlabel('Range')
  legend(lstr, 'location','best')

  titleStr = sprintf('Force versus Ram Position \n %s');
  title(titleStr);


  lstr={};
  if exist( 'stateFigPtr' ) ~= 1
    stateFigPtr= figure;
    set( stateFigPtr, 'Name', 'States', 'DeleteFcn', 'clear stateFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      buoyFontsize=fontsize;
    else
      buoyFontsize=20;
    end
    buoyFontweight='normal';
    set(gca,'fontsize',buoyFontsize,'fontweigh',buoyFontweight)
    set(gcf,'defaultaxesfontsize',buoyFontsize,'defaulttextfontsize', ...
	    buoyFontsize)
    stateAxesPtr = gca;
  else
    figure(stateFigPtr);
  end
  subplot(2,1,1);
  plot(t(indx),  xHat(1,:) );
  grid on;
  hold on;
  ylabel('Inverse Slope, In/lb');
  titleStr = sprintf('State Estimates Versus Time \n  %s', modelStr);
  title(titleStr);
  subplot(2,1,2);
  plot(t(indx),  xHat(2,:) );
  grid on;
  xlabel('Seconds')
  ylabel('Position Offset, In.');

  linkaxes(get(gcf,'children'),'x')



  lstr={};
  if exist( 'covFigPtr' ) ~= 1
    covFigPtr= figure;
    set( covFigPtr, 'Name', 'Variances', 'DeleteFcn', 'clear covFigPtr' );
    set(gcf,'position',position)
    if(exist('fontsize')) 
      buoyFontsize=fontsize;
    else
      buoyFontsize=20;
    end
    buoyFontweight='normal';
    set(gca,'fontsize',buoyFontsize,'fontweigh',buoyFontweight)
    set(gcf,'defaultaxesfontsize',buoyFontsize,'defaulttextfontsize', ...
	    buoyFontsize)
    stateAxesPtr = gca;
  else
    figure(covFigPtr);
  end

  subplot(3,1,1);
  plot(t(indx), (squeeze(P(1,1,:))').^.5 );
  ylabel('Inverse Slope Estimate Error \sigma, In/lb');
  grid on;
  titleStr = sprintf('Variances Versus Time \n  %s', modelStr);
  title(titleStr);
  subplot(3,1,2);
  plot(t(indx), squeeze(P(1,2,:))' );
  grid on;
  ylabel('Covariance, In*PSI');
  subplot(3,1,3);
  plot(t(indx), (squeeze(P(2,2,:))').^.5 );
  grid on;
  ylabel('Position Offset Est. Error \sigma In');
  xlabel('Seconds');

  linkaxes(get(gcf,'children'),'x')

