% Kalman filter for a robot moving along a train track.
%
% This is the model developed in EE3901/EE5901 lectures. The state space
% for this system is:
%  
%  x = [ position
%        velocity ];
% 

deltaT = 0.05; % time between measurements (seconds)
time = 0:deltaT:20; % run for 20 seconds 

% For demonstration purposes, calculate the exact trajectory of the
% vehicle. Suppose that the velocity is given by:
velocity = 2*sin(2*pi*time/40);
% The integral of the velocity is position:
position = -40/pi*(cos(2*pi*time/40) - 1);
%
% Of course in real life you wouldn't know the true velocity and position.
% We include them in this simulation only so we can assess the accuracy of
% the filter.

% Suppose that there are 3 sensors. These are:
% Define the measurement matrix:
H = [1      0   % GPS position giving answer in metres
     1000   0   % Rangefinder position giving answer in millimetres
     0      1]; % Wheel encoder velocity 
 
% Define the measurement covariance.
% In real life you need to measure this or obtain sensor specifications
% from their datasheets. If the sensors are uncorrelated then there will be
% zeros in the off-diagonal positions. 
R = [0.5^2      0           0
     0          1000^2      0
     0          0           2^2];

% Simulate noisy sensors.
% Here we sample from a multivariate normal distribution with expected
% value given by the true measurements and covariance given by R.
% In real life you would just read from the actual sensors but this is a
% simulation so we use random numbers to simulate sensor noise.
measurements = mvnrnd((H*[position;velocity]).', R).';

% Specify the initial state vector and covariance
xhat = [0
        0];
P = [0  0 
     0  0];
 
% Specify the process covariance. 
% Here we assume a diagonal matrix (i.e. neglect covariance terms). We
% assume a standard deviation proportional to deltaT.
Q = [(0.1*deltaT)^2         0                 % for position, assume we accumulate 0.1m of standard deviation per second 
     0                      (0.7*deltaT)^2];  % for velocity, assume we accumulate 0.7m/s of standard deviation per second
% ^ these numbers chosen by experimentation to achieve good performance.
 
% Specify state prediction model
F = [1   deltaT         % p(k+1) = p(k) + deltaT*v(k)
     0   1      ];      % v(k+1) =               v(k)

% Preallocate some memory for storing the output of the filter.
kalman_filtered_position = zeros(size(position));
kalman_filtered_position(1) = xhat(1); % save the initial position

% Run the Kalman filter algorithm
for i = 2:length(time)
    % Run the prediction step 
    xhat_prediction = F * xhat;
    P_prediction = F * P * F' + Q;
    
    % Get a measurement
    z = measurements(:, i);
    
    % Run the update step
    y = z - H*xhat_prediction; % measurement residual
    S = H*P*H' + R; % measurement residual covariance
    K = P * H' / S; % Kalman gain
    xhat = xhat_prediction + K*y;
    P = (eye(2) - K*H)*P_prediction;
    
    % Save the position for plotting
    kalman_filtered_position(i) = xhat(1);
end

% Plot the sensors
figure();
plot(time, position, 'DisplayName', 'Exact');
hold on;
plot(time, measurements(1,:), 'DisplayName', 'Simulated noisy GPS');
plot(time, measurements(2,:)/1000, 'DisplayName', 'Simulated noisy rangefinder');
plot(time, kalman_filtered_position, 'DisplayName', 'Kalman filtered position');
xlabel('Time (s)');
ylabel('Position (m)');

legend show;
