% This m file is an extension of ls3.m which calculates a least squares estimate fo the position of a 
% target based on ranges from the target to a grid of receivers at known locations (control points).  
% This m file also caculates a least squares estimate of the sound speed.  It is very important to note
% that this calculation is much more sensitive to geometry than that for
% position alone.  CP should be taken over a range of ranges including
% right over the presumed target position in order to keep the matrices
% well conditioned
% 

% Reference: "Analysis and Adjustment of Survey Measurements"
%            Edward M. Mikhail, Gordon Gracie
%            Van Nostrand Reinhold Company (out of print)
%            reprint acquired from E. M. Mikhail 
%            Purdue Univ, Civil Engr. Dept.

% variables: 
% CP = actual control point location (receiver locations)
% trueP = true position of the target
% trueRange = actual range between each receiver and the target
% range = measured range between each receiver and the target
% trueC = actual speed of sound (m/sec)\
% C = estimated speed of sound (m/sec)
% measuredTime = measured time of flight
% trueT = actual time of flight
% p = position estimate + speed of sound estimate
% W = weighting matrix (set to all ones for this example)
% B = coefficient matrix for the observations
% f = condition equations constant term vector
% N = coefficient matrix of the normal equations
% t = vector of "constants" in the normal equations
% delta = vector of parameter corrections
% v = residuals
% trueC = true sound speed velocity
% numRows = number of CPs to add right over the top of the target
% rangeEst = new estimate of the slant range
% travelTimeEst = new estimate of the one way travel time

% inputs: receiver locations,
%         range between target and each receiver
%         initial guess at target location
%         bias in range measurement
%         noise in range measurement
%         ship attitude - heading, pitch, roll

% outputs: delta (the residuals)
%          new position at each iteration

% Author: Gene Massion

% Revision History:    
% 12/13/04  initial release
%

clear
randn('state',sum(100*clock));

numCP = 200;
trueC = 1485.0;
measuredC = 1500.0;
shipRadius = 300.0;
rangeBias = 0.0;
rangeVar = 1.0;
trueP = [0 0 -300 trueC];

%Calculate control point X, Ys assuming ship is on the surface Z=0
angles = 0:(2*pi)/numCP:2*pi-(2*pi/numCP);
CP = [shipRadius * cos(angles); shipRadius * sin(angles); zeros(1,numCP)];
CP = CP';
%Add some rows with control points right over the target
numRows = 100;
temp = [trueP(1) .* ones(numRows,1) trueP(2) .* ones(numRows,1) zeros(numRows,1)];
CP = [CP; temp];
numCP = numCP + numRows;

%Calculate measured slant range
trueRange = sqrt((CP(:,1) - trueP(1)).^2 + (CP(:,2) - trueP(2)).^2 +(CP(:,3) - trueP(3)).^2)';
rangeNoise = rangeVar * randn(numCP,1);
measuredRange = trueRange + rangeNoise' + rangeBias;

%Calculate measured one way travel time
measuredTime = measuredRange / trueC;
measuredTime = measuredTime';

% Weight matrix = 1.0;
d = ones(1, numCP);
W = diag(d,0);

% guess first position
p = [10 10 -250 measuredC];
travelTimeEst = measuredTime;

iter = 1;
delta = 1000 * ones(1,length(trueP));
  while max(abs(delta)) > 0.1
    B = [(CP(:,1) - p(1))./(travelTimeEst .* p(4)^2) (CP(:,2) - p(2))./(travelTimeEst .* p(4)^2) ...
            (CP(:,3) - p(3))./(travelTimeEst .* p(4)^2) travelTimeEst./p(4)];
    f = ((1/p(4)) .* sqrt((CP(:,1) - p(1)).^2 + (CP(:,2) - p(2)).^2 + (CP(:,3) - p(3)).^2)) - measuredTime;
    f = f';
          
    % the simple inversion
    %delta = B\f'
    
    % the complicated inversion
    N = B' * W * B;
    t = B' * W * f';
    delta = inv(N) * t;
    v = f - (B * delta)';
   
    p = p + delta';
    rangeEst = sqrt((CP(:,1) - p(1)).^2 + (CP(:,2) - p(2)).^2 +(CP(:,3) - p(3)).^2)';
    travelTimeEst = rangeEst / p(4);
    travelTimeEst = travelTimeEst';

    fprintf(1,'iter= %2.0f cond of N = %6.0f  dx= %6.2f dy= %6.2f dz= %6.2f dC= %8.2f estimates= %8.2f %8.2f %8.2f %8.2f\n', ...
            iter, cond(N), delta(1), delta(2), delta(3), delta(4), p(1), p(2), p(3), p(4))
    iter = iter + 1;
  end   
  
  fprintf(1,'\nfinal estimates: x = %8.1f  y = %8.1f  z = %8.1f  C =  %8.1f\n', ...
            p(1), p(2), p(3), p(4));  
  fprintf(1,'x error = %8.6f   y error = %8.6f   z error = %8.6f   C error = %8.6f\n\n', ...
            trueP(1) - p(1), trueP(2) - p(2), trueP(3) - p(3), trueP(4) - p(4));
  
 