function edm
%
% function to simulate the processing of EDM measurements through
% a Kalman filter.  

%==========================================================================
% Function:  edm
%
% Author:
%  Rod Deakin, 
%  BONBEACH, VIC 3196,
%  AUSTRALIA
%  email: randm.deakin@gmail.com
%
% Date:
%  Version 1.0  15 April  2006
%  Version 1.1  29 August 2015
%
% Remarks:
%  This function simulates the processing of EDM measurements
%  through a Kalman filter.  
%
% 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)
%  corrn     - array of corrections to State vector, dimensions (n,epochs)
%  H         - coefficient matrix of System Driving Noise, dimensions (n,u)
%  I         - Identity matrix, dimensions (n,n)
%  K         - Gain matrix, dimensions (n,m)
%  meas      - vector of measurements, dimensions (epochs,1)
%  noise     - vector of normally distributed measurement "noise" with
%              a mean of zero and a standard deviation of sigma (epochs,1)
%  Q         - cofactor matrix of measurements, dimensions (n,n)
%  Qmm       - cofactor matrix of Secondary Model, dimensions (n,n)
%  Qww       - cofactor matrix of System Driving Noise, dimensions (n,n)
%  Qxx       - cofactor matrix of State vector, dimensions (n,n)
%  std_xhat  - vector of standard deviations of the filtered State
%  T         - Transition matrix, dimensions (n,n)
%  U         - cofactor update matrix, dimensions (n,n)
%  xhat      - array containing the State vector at each epoch,
%              dimensions (n,epochs)
%
% Variables:
%  const     - a constant value
%  epochs    - number of epochs
%  i,j,k     - integer counters
%  m         - number of measurements at particular epoch
%  n         - number of parameters in the State vector 'xhat'
%  u         - number of parameters in the System Driving Noise vector
%  std_noise - st dev of measurement noise
%==========================================================================

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% define the Kalman filter matrices: %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
n = 1;  % number of elements in State vector
m = 1;  % number of measurements at each epoch
u = 1;  % number of elements in System Driving Noise vector
% Set the total number of measurement epochs
epochs = 250;
% 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,1) = 1.0;
% Set the cofactor matrix of the System Driving Noise Qww
Qww = zeros(u,u);
Qww(1,1) = 0.0;
% Set the coefficient matrix of System Driving Noise H
H = zeros(n,u);
H(1,1) = 1.0;
% Compute cofactor matrix of Dynamic Model by propagation of variances
Qmm = H*Qww*H';

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Generate some measurements with noise %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the state of the random number generator (rng).  
% Use rng('default') to ensure that you get the same result as Ref [1].  
% Use rng('shuffle') for other results
rng('default');
% rng('shuffle');
% Set the st. dev. of measurement noise, initialise the measurement vector
% then generate the noise
std_noise = 0.010;
meas      = zeros(epochs,1);
noise     = std_noise .* randn(epochs,1);
% Generate the measurement vector
const = 355.420;
for k = 1:epochs
  meas(k) = const + noise(k);
end  

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set Cofactor matrices and State vector to initial values %%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Set the cofactor matrix of the measurements Q
Q = zeros(m,m);
Q(1,1)  = std_noise^2;
% Set the State cofactor matrix Qxx.  This will be the filtered 
% State cofactor matrix at epoch t2.
Qxx = zeros(n,n);
Qxx(1,1) = std_noise^2;  
% Set the starting estimate of the State vector for Epoch 1
% This will be the filtered Sate at epoch t2
xhat(1,1) = meas(1,1);
% 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;
  % Set the elements of the design matrix B
  B = zeros(m,n);
  B(1,1) = -1;
  % Compute the Gain matrix K
  K = Qxx*B'/(Q + B*Qxx*B');
  % Compute corrections to predicted State
  corrn(:,k) = K*(-meas(k) - B*xhat(:,k));
  % 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;
  std_xhat(k) = sqrt(Qxx(1,1));
  % Print the State vector, corrections and filtered State cofactor matrix
  fprintf('\n\nEpoch = %3d, measurement = %8.4f',k,meas(k));
  fprintf('\nFiltered State  Corrn         Filtered State cofactor matrix Qxx');
  for i=1:u
      fprintf('\n  %9.4f %10.4f      ',xhat(i,k),corrn(i,k));
      for j=1:u
          fprintf('%12.9f',Qxx(i,j));
      end  
  end  
end
fprintf('\n\n');
%%%%%%%%%%%%%%%%%%%%%%%%%
% End of Kalman filter %%
%%%%%%%%%%%%%%%%%%%%%%%%%

% Figure 1: create a figure window with two plots
figure(1);
clf;
subplot(2,1,1);
hold on;
grid on;
box on;
plot(meas,'k.');
plot(xhat,'k');
title('Kalman Filter estimate');
ylabel('state [m]');

subplot(2,1,2);
hold on;
grid on;
box on;
plot(std_xhat,'k');
title('Standard deviation of estimate');
xlabel('epoch [s]');
ylabel('std [m]');

% Figure 2: Plot of Kalman Filter estimate
figure(2);
clf;
hold on;
grid on;
plot(meas,'k.');
plot(xhat,'k');
hold off;
title('Kalman Filter estimate');
xlabel('epoch [s]');
ylabel('state [m]');

% Figure 3: Plot of st. dev. of Kalman Filter estimate
figure(3);
clf;
hold on;
grid on;
plot(std_xhat,'k');
hold off;
title('Standard deviation of estimate');
xlabel('epoch [s]');
ylabel('std [m]');

