% Kalman filtering of speech signals
% This code implements a Kalman filter on the acoustic samples of speech.
% Authors: |- Saurabh Prasad
%          |- Sanjay Patil
% There is a small indexing bug that needs to be resolved.

clear;
[y fs] = wavread('bb_1315a.wav');
p = 8;  % Define the AR order
l = 0;
frame_length = 150; % Define the frame size
num_frames = floor(length(y)/frame_length);

x_corrupted = y + normrnd(0,0.2,length(y),1); % Corrupt the acoustic space 
                                              % with AWGN (0 mean, 0.2 var.)

% Perform Kalman Filtering per frame

for frame_index = 1 : num_frames
    
    % The following extracts AR-coefficients for the pseudo-stationary
    % analysis-frame
    frame = x_corrupted(frame_index*frame_length+1:frame_index*frame_length+frame_length);
    [C Lags] = xcorr(frame,frame);
    mid = (length(C)+1)/2;
    C_new = C(mid : mid+2*p+l-1);
    R_column = C_new(p+1:2*p+l);
    for i = 1 : p + l
        for j = 1 : p
            R(i,j) = C_new(i+j-1);
        end;
    end;            
    a = pinv(R)*R_column;   % a(i) is the i'th AR-coefficient

    % The a(i)'s are then used to generate the state-transition matrix 
    % representing state evolution in that frame
    F_ = eye(p-1,p-1);
    F = zeros(p,p);
    F(:,1) = zeros(p,1);
    F(p,:) = a';
    % F is now the state-transition matrix for the Kalman Filter
    F(1:p-1,2:p) = eye(p-1,p-1);

    % Defining G (Process noise gain) and H (observation matrix)
    G = zeros(p,1);
    G(p,1) = 1;
    H = G';

    % Initialize the initial state-vector prediction to the first 'p' data
    % samples
    X(:,1) = y(1:p);

    % Computing the observation noise variance using the relation described in
    % "Estimation of Noise Variance from Noisy AR signal and it's application
    % in speech enhancement", K.K. Paliwal, IEEE transactions on ASSP, vol. 36,
    % number 2, Feb., 1998, pp292-294
    [C Lags] = xcorr(frame,frame);
    mid = (length(C)+1)/2;
    C_new = C(mid : mid+2*p+l-1);
    sum1 = 0;
    for i = 1 : p    
        sum2 = C_new(i+1);
        for j = 1 : p
            sum2 = sum2 + a(j)*C_new(abs(i-j)+1);
        end;    
        sum2 = a(i)*sum2;
        sum1 = sum1 + sum2;
        sum1 = sum1/sum(a.^2);
    end;
    sigma_v = sum1;

    % Defining Process noise variance
    % Note, this must be modified to automatically compute the noise
    % variance from the data in the analysis frame (using modified Yule-Walker
    % equations).. This initialization of sigma_u is only for a sanity
    % check of Kalman Filtering
    % (NOTE: Refer to the Adaptive Signal Processing book by Haykin...
    % there is an explanation of why Kalman Filters MAY DIVERGE in the
    % absence of driving noise.)
    sigma_u = .25;

    P_old = eye(p,p);
    Q = sigma_u*eye(p,p);
    R = sigma_v;
    if frame_index == 1;
        s(1:p) = x_corrupted(1:p); % not filtering the first p data points in the analysis frame
        for i = p+1 : frame_length
            z = x_corrupted(i);
            x_state = s(i-p:i-1)'; % The state vector passed to the Kalman filter must be px1
            s(i) = kalman(x_state,z,Q,R,F,G,H);     
        end;
    else
        s(frame_index*frame_length+1:frame_index*frame_length+p) = x_corrupted(frame_index*frame_length+1:frame_index*frame_length+p); % not filtering the first p data points in the analysis frame
        for i = (p+1) : frame_length
           z = x_corrupted(frame_index*frame_length*+i);
           x_state = s(frame_index*frame_length - p : frame_index*frame_length -1 )'; % The state vector passed to the Kalman filter must be px1
           s((frame_index*frame_length)+1) = kalman(x_state,z,Q,R,F,G,H);   
        end;
    end;    
end;    
    
s = s - mean(s);
hold on; plot(x_corrupted./(max(x_corrupted)-min(x_corrupted)),'r')
hold on; plot(s./(max(s)-min(s)));
hold on; plot(x./(max(x)-min(x)),'g');