
function [filteredOutput] = kalataFilter(inputImages,channelIndexList,timeArray,sigma_w,sigma_n,filt_order,burn_in)
%kalataFilter.m Apply Kalata filters to an input time-series of images
%acquired at single channels.
%
%
% 
% INPUTS:
%     inputImages : An Ny*Nx*Nt array of images, with each Ny*Nx image acquired at a specific channel
%                   recorded in channelIndexList, such that inputImages(:,:,k) is the image recorded at 
%                   channelIndexList(k)
%     channelIndexList: An Nt*1 array of doubles, one entry for each image, with a unique linear index for each channel.
%                   Values are expected to lie between 1 and NChannels.
%     timeArray:    An Nt*1 array of doubles, corresponding to the time points at which each image was acquired.
%     sigma_w:      Process noise variance
%     sigma_n:      Measurement noise variance
%     filt_order:   Order of the Kalata filter, with values:
%                     0 - Sliding Window (SW) filter
%                     1 - Alpha filter
%                     2 - Alpha-Beta filter
%     burn_in:      Number of frames of each channel to record before beginning to apply the kinematic model.
%                   Integer-valued
%                   
% OUTPUTS:
%     filteredOutput: An Ny*Nx*Nc*Nt array of filtered multispectral images, such that each Ny*Nx*Nc multi-channel image is the estimate of the true multi-channel image at timepoint k.
%     
%     
%     author      - Devin O'Kelly
%     date        - 28 Nov 2019
%     last update - 19 May 2020
%     
    
    % Preparing for buffer creation.
    Nw = numel(unique(channelIndexList));
    [Ny,Nx,NFrames] = size(inputImages);
    
    filteredOutput = ones(Ny,Nx,Nw,NFrames);
    lastTime = 0;
    channelTimeList = zeros(Nw,1);
    nMeas    = zeros(Nw,1);
    
    switch filt_order
        case 0 % Sliding window
            est.state = zeros(Ny,Nx,Nw);
        case 1 % Alpha
            est.state = zeros(Ny,Nx,Nw);
        case 2 % Alpha-beta
            est.state = zeros(Ny,Nx,Nw);
            est.d_state = zeros(Ny,Nx,Nw);
        otherwise
            error('Invalid filter order!');
    end
    
    
    %%
    for k = 1:NFrames
        %%% Simulate data acquisition, along with channel index and timestamp
        frameData = inputImages(:,:,k);
        channelIndex = channelIndexList(k);
        curTime = timeArray(k);
        
        
        dt = curTime - lastTime; % Time update for kinematic component.
        dT = curTime - channelTimeList(channelIndex); % Sampling time since the last time this channel was observed.
        
        
        % Kinematic update / predict step
        if nMeas(channelIndex) < burn_in
            switch filt_order
                case 0
                    est.state = est.state;
                case 1
                    est.state = est.state;
                case 2
                    est.state = est.state;
                    est.d_state = est.d_state.*0;
            end
        else
            switch filt_order
                case 0
                    est.state = est.state;
                case 1
                    est.state = est.state;
                case 2
                    est.state = est.state + dt.*est.d_state;
                    est.d_state = est.d_state;
            end
            
        end
        
        % Calculate tracking index based on time since last observation.
        trackingIndex = calcLambda(dT,sigma_w,sigma_n);
        
        switch filt_order
            case 0
                alpha = 1;
            case 1
                italpha = calcABG_from_lambda(trackingIndex);
                [alpha] = calc_scheduling(nMeas(channelIndex),italpha);
            case 2
                [italpha,itbeta] = calcABG_from_lambda(trackingIndex);
                [alpha,beta] = calc_scheduling(nMeas(channelIndex),italpha,itbeta);
        end
        
        
        % Take measurement residual.
        channelDiff = frameData - est.state(:,:,channelIndex);
        
        
        if nMeas(channelIndex) == 0 % Let the first estimate take on the first value.
            est.state(:,:,channelIndex) = frameData;
            switch filt_order
                case {2}
                    est.d_state(:,:,channelIndex) = est.d_state(:,:,channelIndex).*0;
                otherwise
            end
        else
            switch filt_order
                case 0
                    est.state(:,:,channelIndex) = frameData;
                case 1
                    est.state(:,:,channelIndex) = est.state(:,:,channelIndex) + alpha.*channelDiff;
                case 2
                    est.state(:,:,channelIndex) = est.state(:,:,channelIndex) + alpha.*channelDiff;
                    est.d_state(:,:,channelIndex) = est.d_state(:,:,channelIndex) + (beta./dT).*channelDiff;
            end
        end
        
        channelTimeList(channelIndex) = curTime;
        lastTime = curTime;
        nMeas(channelIndex) = nMeas(channelIndex) + 1;
        
        filteredOutput(:,:,:,k) = est.state;
       
        
        if rem(k,50) == 0
            disp([num2str(k) ' completed']);
        end
        
    end
end







% calculating tracking index
% Expression from:
%     Paul R. Kalata: The tracking index: A generalized parameter for alpha-beta and alpha-beta-gamma target trackers.
%     IEEE Transactions on Aerospace and Electronic Systems, AES-20(2):174-181, March 1984.
function lambda = calcLambda(dt,process_var,noise_var)
    
    sw = sqrt(process_var);
    sn = sqrt(noise_var);
    
    lambda = sw.*(dt.^2)./sn;
    
end

% Calculation of alpha, alpha-beta, and alpha-beta-gamma coefficients.
%
% Expressions for A and A-B coefficients from:
%     Paul R. Kalata: The tracking index: A generalized parameter for alpha-beta and alpha-beta-gamma target trackers.
%     IEEE Transactions on Aerospace and Electronic Systems, AES-20(2):174-181, March 1984.
%
% Expressions for A-B-G coefficients from:
%     J. E. Gray and W. J. Murray: A derivation of an analytic expression for the tracking index for the alpha-beta-gamma filter.
%     IEEE Trans. on Aerospace and Electronic Systems, 29:1064-1065, 1993.
%
% Input: A single scalar value (the tracking index) output from
% calcLambda()
%
% Output: Coefficients for 0th, 1st, or 2nd-order Kalata filters depending
% on the number of outputs denoted.

function [alpha, beta, gamma] = calcABG_from_lambda(lambda)
    
    switch nargout
        case 1
            alpha = (-(lambda.^2) + sqrt(lambda.^4 + 16.*(lambda.^2)))./8;
        case 2
            r = (4 + lambda - sqrt(8.*lambda + lambda.^2))./(4);
            alpha = 1 - r.^2;
            beta = 2.*(2-alpha) - 4.*sqrt(1-alpha);
            
        case 3
            b = lambda/2 - 3;
            c = lambda/2 +3;
            d = -1;
            p = c - (b.^2)/3;
            q = (2.*b.^3)./27 - (b.*c)/3 + d;
            v = sqrt(q.^2 + (4.*p.^3)/27);
            z = -(q + v/2).^(1/3);
            s = z - p./(3.*z)-b./3;
            alpha = 1 - s.^2;
            beta = 2.*(1-s).^2;
            gamma = (beta.^2)./(2.*alpha);
        otherwise
            disp('Number of output arguments specifies filter order.');
    end
    
end


% Calculating the scheduling based on the sample index.
% Functions based on expressions from:
%     Fixed-Gain, Two-Stage Estimators For Tracking Maneuvering Targets, W.D.
%     Blair, NSWC Dahlgren Division: https://apps.dtic.mil/dtic/tr/fulltext/u2/a255832.pdf
%     Link current as of 2020 May 19
function [varargout] = calc_scheduling(k,varargin)
    
    nvarg = numel(varargin);
    switch nvarg
        case 1
            [alpha] = varargin{:};
            if isnan(alpha)
                alpha = -Inf;
            end
            
            itValue_alpha = 1./(k+1);
            alpha_k = max(itValue_alpha,alpha);
            varargout = {alpha_k};
        case 2
            [alpha, beta] = varargin{:};
            if isnan(alpha)
                alpha = -Inf;
            end
            if isnan(beta)
                beta = -Inf;
            end
            
            itValue_alpha = (2.*(2.*k + 1)./((k+1).*(k+2)));
            itValue_beta = (6./((k+1).*(k+2)));
            
            alpha_k = max(itValue_alpha,alpha);
            beta_k = max(itValue_beta,beta);
            varargout = {alpha_k beta_k};
        case 3
            [alpha, beta, gamma] = varargin{:};
            if isnan(alpha)
                alpha = -Inf;
            end
            if isnan(beta)
                beta = -Inf;
            end
            if isnan(gamma)
                gamma = -Inf;
            end
            
            itValue_alpha = (3.*(3.*k.^2 + 3.*k + 2)./((k+1).*(k+2).*(k+3)));
            itValue_beta = (18.*(2.*k + 1)./((k+1).*(k+2).*(k+3)));
            itValue_gamma = (60)./((k+1).*(k+2).*(k+3));
            
            alpha_k = max(itValue_alpha,alpha);
            beta_k = max(itValue_beta,beta);
            gamma_k = max(itValue_gamma,gamma);
            varargout = {alpha_k beta_k gamma_k};
        otherwise
    end
end

