%% Kalman Filter for uint8 images

% grayI = imread(img);                % Read in a grayscale image
noisyI = imnoise(grayI,'gaussian',0,0.01); % add gaussian noise

[numRows, numCols] = size(noisyI);

% System parameter
A = 1; B = 0; u = 0; C = 1;

% Process noise variance
Q = (20)^2;                    % Estimated Value

% Measurement noise variance
R = (25.5)^2;                  % for noise variance of 0.01 in double precision results a variance of (25.5)^2 in uint8 precision

% Apriori state estimates of one row
s_apriori = zeros(1,numCols);

% Aposteriori state estimates of one row
x_aposteriori = zeros(1,numCols);

% Apriori error covariance estimates
P_apriori = zeros(1,numCols);

% Aposteriori error covariance estimates
P_aposteriori = zeros(1,numCols);

% Initial aposteriori state estimate
x_aposteriori(1) = maxPix/2;

% Initial aposteriori error covariance estimate
P_aposteriori(1) = 1;

% Observed image, measurements:
r = double(noisyI);

% size of filtered Image
kalFilImg = zeros(numRows, numCols);

%--------------------------------------------------------------------------
% Perform Kalman filtering
%--------------------------------------------------------------------------

for i = 1:numRows
    for j = 2:numCols
        
        % Update Kalman filter
        [x_aposteriori(j), P_aposteriori(j)] = kalmanFunc(x_aposteriori(j-1), A, B, u, P_aposteriori(j-1), Q, R, C, r(i,j));
    end
    kalFilImg(i,:) = x_aposteriori; 
end

kalFilImg = uint8(kalFilImg);

% Plot of filtered image by Kalman Filter
figure
imshow(kalFilImg);
