function kalship3(filename)
%
% kalship3   This function implements a Kalman Filter to estimate the 
%   POSITION and VELOCITY of a ship in a navigation channel.  
%   The observations are distances to the ship at regular time intervals
%   from three known locations.
%
% e.g., kalship3('d:\projects\kalman\exercise\kalshipdata3.txt');

%==========================================================================
% Function:  kalship3
%
% Author:
%  Rod Deakin, 
%  BONBEACH, VIC 3196,
%  AUSTRALIA
%  email: randm.deakin@gmail.com
%
% Date:
%  Version 1.0  17 April  2006
%  Version 1.1  29 August 2015
%
% Remarks:
%  This function implements a Kalman Filter to estimate the POSITION and
%  VELOCITY of a ship moving at a near constant velocity in a channel.  
%  The observations are horizontal distances to three fixed stations
%  taken at regular time intervals.
%  The observations are contained in an ASCII data file, an example of 
%  which is shown below (d:\projects\kalman\Kalshipdata3.txt)
%
%-----start of data file d:\projects\kalman\exercise\Kalshipdata3.txt------
%
% Kalman Filter Navigation problem
%
% A ship is moving at a constant velocity with distance measurements  
% every 60 seconds to three shore-based beacons A, B and C.
%
% Data
%       Distances to beacons
% Epoch   A       B       C
%   1	4249.7	7768.6	7721.1
%   2	3876.1	7321.4	7288.5
%   3	3518.4	6872.2	6857.6
%   :       :       :       :
%   :       :       :       :
%   :       :       :       :
%   19	5366.4	1959.6	2819.7
%   20	5785.0	2182.8	3023.5
%----------------------end of data file--------------------------------
%
% References:
%  [1] Deakin, R.E., 2015, Least Squares and Kalman Filtering,
%        Lecture Notes, September 2015.
%  [2] Deakin, R.E., 2006, The Kalman Filter and Surveying Applications,
%        Presented at the Victorian Regional Surveying Conference, Mildura, 
%        23-25 June 2006.
%
% Arrays:
%  B         - Design matrix, dimensions (m,n)
%  Distance  - vector of distances from start at each epoch
%  Ef        - vector of east coords of fixed stations (beacons)
%  epoch     - vector of epoch numbers (integers)
%  H         - coefficient matrix of System Driving Noise, dimensions (n,u)
%  Heading   - vector of headings (bearings from north) at each epoch
%  I         - Identity matrix, dimensions (n,n)
%  K         - Gain matrix, dimension (n,m)
%  meas      - array of measured distances from the ship to the beacons (metres)
%  Nf        - vector of north coords of fixed stations (beacons)
%  Q         - cofactor matrix of measurements, dimension is (m,m)
%  Qmm       - cofactor matrix of Dynamic Model, dimensions (n,n)
%  Qww       - cofactor matrix of System Driving Noise, dimensions (u,u)
%  Qxx       - cofactor matrix of State vector, dimensions (n,n)
%  T         - Transition matrix, dimensions (n,n)
%  U         - cofactor Update matrix, dimensions (n,n)
%  Velocity  - vector of velocities at each epoch
%  xhat      - State vector, dimension is (n,epochs)
%
% Variables
%  cj,dj     - distance  coefficients for observation j
%  d         - distance
%  d2        - distance squared
%  dt        - time difference in seconds between epochs
%  dE,dN     - difference in east and north coordinates
%  j,k       - integer counters
%  m         - number of measurements at each epoch
%  n         - number of parameters in the State vector 'xhat'
%  s2_aE     - variance of east  acceleration (m^2/s^4)
%  s2_aN     - variance of north acceleration (m^2/s^4)
%  s2_Dist   - variance of observed distance(m^2)
%  u         - number of elements in System Driving Noise vector
%  vE,vN     - east and north velocities
%==========================================================================

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the coordinates of the fixed stations, the beacons A, B and C. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
Ef = [10000 13880 15550];
Nf = [10000 11250 7160];

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Read in the data from a file using function textread. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Read the epoch number into array 'epoch' and the distances to the
% three beacons into matrix 'meas'.  Distances to beacons A, B and C 
% are in matrix 'meas' in columns 1, 2 and 3
[epoch,meas(:,1),meas(:,2),meas(:,3)]=textread(filename,'%d %f %f %f','headerlines',8);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set estimates of variances of measurements and System Driving Noise %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the estimate of the variance of measurements.
s2_meas = 1.0;  % variance of distance (m2)
% Set the estimates of variances of System Driving Noise.
s2_aE = 0.017; 
s2_aN = 0.017; 

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the Epoch update rate. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
dt = 60;  % update rate in seconds of time

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Define the Kalman filter matrices. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n = 4;  % number of elements in State vector
m = 3;  % number of measurements at each epoch
u = 2;  % number of elements in System Driving Noise vector

% The State vector has 4 elements; the east and north cordinates E,N, and
% the east and north velocities vE,vN.
% There are three measurements at each epoch (to beacons A, B and C).
% There are two elements in the system driving noise vector (accel. East
% and accel. North).

% Determine the number of measurement epochs.
epochs = length(epoch);
% Initialize the State vector.
xhat = zeros(n,epochs);
% Initialize the corrections to State vector.
corrn = zeros(n,epochs);
% Set the State Transition matrix T
T = eye(n);
T(1,3) = dt;
T(2,4) = dt;
% Set the cofactor matrix of the measurements Q
Q = zeros(m,m);
for m = 1:m
  Q(m,m) = s2_meas;  % diagonal elements of Q = variance of distance
end
% Set the cofactor matrix of the System Driving Noise Qww
Qww = zeros(u,u);
Qww(1,1) = s2_aE;
Qww(2,2) = s2_aN;
% Set the coefficient matrix of System Driving Noise H
H = zeros(n,u);
H(1,1) = (dt^2)/2;
H(2,2) = H(1,1);
H(3,1) = dt;
H(4,2) = H(3,1);
% Compute cofactor matrix of Dynamic Model by propagation of variances
Qmm = H*Qww*H';
% Set the starting estimate of the State vector.  
% This will be the filtered Sate at epoch t2.
E1 = 7875.0;
N1 = 6319.392;
vE = 7;
vN = 3;

% Epoch t1
xhat(1,1) = E1;
xhat(2,1) = N1;
xhat(3,1) = vE;
xhat(4,1) = vN;
% Set the State cofactor matrix Qxx.  
% This will be the filtered State cofactor matrix at epoch t2.
Qxx = zeros(n,n);
Qxx(1,1) = 20.0;  
Qxx(2,2) = 20.0;
Qxx(3,3) = 0.5;
Qxx(4,4) = 0.5;
% Initialize the Gain matrix
K  = zeros(n,m);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Start the Kalman Filter at epoch t2. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
for k = 2:epochs
  % Compute the predicted State vector.
  xhat(:,k) = T*xhat(:,k-1);
  % Compute the predicted State cofactor matrix
  Qxx = T*Qxx*T' + Qmm;
  % Compute the Design matrix B
  B  = zeros(m,n);
  f  = zeros(m,1);
  for j = 1:m
    % coordinate differences Pi to Pk
    dE = xhat(1,k)-Ef(j);
    dN = xhat(2,k)-Nf(j);
    % Computed distance Pi to Pk
    d2 = dE^2 + dN^2;
    d  = sqrt(d2);
    % Distance coefficients
    cj =  dN/d;
    dj =  dE/d;
    % Set the elements of B
    B(j,1) = -dj;
    B(j,2) = -cj;
    % Compute the numeric terms (computed - observed)
    f(j,1) = d - meas(k,j);
  end
  % Compute the Gain matrix K
  K = Qxx*B'/(Q + B*Qxx*B');
  % Compute corrections to predicted State
  corrn(:,k) = K*f;
  % Compute the filtered State vector
  xhat(:,k) = xhat(:,k) + corrn(:,k);
  % Compute the cofactor update matrix
  I = eye(n);
  U = I - K*B;
  % Compute the filtered State cofactor matrix
  Qxx = U*Qxx;
  % Print the State vector, corrections and filtered State cofactor matrix
  fprintf('\n\nepoch = %3d',k);
  fprintf('\nFiltered State  Corrns        Filtered State cofactor matrix Qxx');
  for i=1:n
    fprintf('\n  %9.3f %10.3f      ',xhat(i,k),corrn(i,k));
    for j=1:n
      fprintf('%10.6f',Qxx(i,j));
    end  
  end  
end  
%%%%%%%%%%%%%%%%%%%%%%%%%%
% End of Kalman filter. %%
%%%%%%%%%%%%%%%%%%%%%%%%%%

fprintf('\n\n');

% Compute distance from start and Velocity
% and get corrections to east, north, velocity east and
% velocity north from the array of corrections.

Distance = zeros(epochs,1);
Velocity = zeros(epochs,1);
Heading  = zeros(epochs,1);
corrnE   = zeros(epochs,1);
corrnN   = zeros(epochs,1);
corrnvE  = zeros(epochs,1);
corrnvN  = zeros(epochs,1);

d2r = 180/pi; % degree to radian conversion factor d2r = 57.29577951..
for k=1:epochs
    if k >1
        % coordinate differences P(k-1) to P(k)
        dE = xhat(1,k)-xhat(1,k-1);
        dN = xhat(2,k)-xhat(2,k-1);
        % Computed distance P(k-1) to P(k)
        d = sqrt(dE^2 + dN^2);
        Distance(k,1) = Distance(k-1,1) + d;
    end
    angle = atan2(xhat(3,k),xhat(4,k))*d2r;
    if angle < 0
        angle = angle + 360;
    end
    Velocity(k,1) = sqrt(xhat(3,k)^2 + xhat(4,k)^2);
    Heading(k,1)  = angle; 
    corrnE(k,1)   = corrn(1,k);
    corrnN(k,1)   = corrn(2,k);
    corrnvE(k,1)  = corrn(3,k);
    corrnvN(k,1)  = corrn(4,k);
end

% Print the filtered values

fprintf('Filtered Values');
fprintf('\nEpoch  Distance  Velocity  Heading');
for k=1:epochs
  fprintf('\n%3d %11.3f %8.3f %8.3f',k,Distance(k,1),Velocity(k,1),Heading(k,1));
end

fprintf('\n\n');

%--------------------------
% create plot of velocities
%--------------------------
figure(1);
clf;
grid on;
plot(epoch,Velocity,'bo-');
title('Kalman Filter Velocities');
xlabel('Epoch');
ylabel('Velocity (m/s)');

%-------------------------------------------
% create plot of East coordinate corrections
%-------------------------------------------
figure(2);
clf;
grid on;
plot(epoch,corrnE,'bo-');
title('Kalman Filter corrections to East coords');
xlabel('Epoch');
ylabel('Corrections (m)');

%-------------------------------------------
% create plot of North coordinate corrections
%-------------------------------------------
figure(3);
clf;
grid on;
plot(epoch,corrnN,'bo-');
title('Kalman Filter corrections to North coords');
xlabel('Epoch');
ylabel('Corrections (m)');
