%    LBLRENAV
%
%    B. A. Moran, 11-Sep-1998

%    Copyright (C) 1995-1998, Massachussetts Institute of Technology
%    Sea Grant Autonomous Underwater Vehicles Laboratory
%    $Id: lblrenav.m,v 1.1 2000/01/10 19:08:06 rob Exp $	

% set up lbl fixes (advance if available)

Z_LBL = data(:,[m_lbl_northfix m_lbl_eastfix]);

if exist('advance') == 1
    Z_LBL = Z_LBL + advance;
end

% provide default value for gain that is applied to LBL fix

if exist( 'K_LBL' ) ~= 1
    K_LBL = 0.5;
end

% initialize first position

M_POS = zeros(size(data,1),2);
M_POS(1,:) = data(1,[m_pos_n m_pos_e]);

for ii = 2:size(data,1)

    % calculate forward model using DR only
    
    dX = diff(data(ii-1:ii,[m_dr_north m_dr_east]));
    
    % update the position using appropriate dX
    
    M_POS(ii,:) = M_POS(ii-1,:) + dX;
    
    % adjust using feedback if LBL fix is valid
    
    if ( data(ii,lbl_fixstat) == 1 )
	
	dX = K_LBL * (Z_LBL(ii,:) - M_POS(ii,:));

%	disp(sprintf('\t%.1f', dX))

	M_POS(ii,:) = M_POS(ii,:) + dX;
    end

end
