%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  EKF algorithm for the demodulation of monocomponent 
%  AM--FM signals
%  AUTHOR  : Santhanam , Balasubramaniam
%  USAGE   : [env,ifreq,theta] = ...
%  ekf1(sig,wc,wm,sigmav,fs);
%  Parameter Description:
%  env     : Amplitude estimate
%  theta   : phase estimate 
%  ifreq   : Instantaneous frequency estimate
%  wc      : Carrier frequency
%  sigmav  : std of measurement noise
%  fs      : sampling frequency
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [env,ifreq,theta] = ekf1(sig,wc,wm,sigmav,fs)
format long
N = length(sig);
T = (1/fs);
%%   Generate unit variance zero mean AWGN (measurement noise)
v = randn(1,N); v = v - mean(v)*ones(1,N);
v = (sigmav/std(v))*v;
%%  State transition matrix
alpha_a = 0.995; alpha_f = 1;
phi1 = [alpha_a 0 0];
phi2 = [0 alpha_f 0];
phi3 = [0 1 1];
Phi  = [phi1;phi2;phi3];
%  Model noise covariance parameters
qa = sqrt(20); qf = sqrt(0.0085); 
qp = 0.0001;
Q_1 = [qa 0 0]; 
Q_2 = [0 qf 0]; 
Q_3 = [0 0 qp];
Q = [Q_1;Q_2;Q_3];
%  Initial state estimates
xmin = [1 wc pi/2].';
%  Error covariance matrices
pf = 0.001 ; pth = 0.001 ; 
pa = qa.^2/(1-alpha_a.^2); 
pmod = qf.^2/(1-alpha_f.^2);
Pmin = diag([pa pf pth]);
%  Measurement and model noise covariances
R  = (sigmav).^2; 
%
for i = 1:1:N
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  Generate h function at predicted state
    h = xmin(1)*cos(xmin(3)/fs);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Generate H via linearization
    H_1 = cos(xmin(3)/fs);
    H_2 = 0;
    H_3 = -xmin(1)*sin(xmin(3)/fs)/fs;
    H = [H_1;H_2;H_3];
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Compute Kalman gain needed for new data
%   from previous estimates (reliability)
    K = Pmin*H*inv(H.'*Pmin*H + R);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   State update using new data
    xplus = xmin + K*(sig(i) - h);
%   State prediction from previous state
    xmin = Phi*xplus;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Error covariance update after new data
    Pplus = (eye(3,3) - K*H.')*Pmin;
%   Error covariance of predicted state
    Pmin = Phi*Pplus*Phi.' + Q;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%   Save state varaibles
    x = xplus;
    theta(i) = x(3); % IP output
    ifreq(i) = x(2); % IF output    
    env(i) = x(1); % IA output
end
%**********************************************************