kalmanvswiener.m

对比卡尔曼滤波和维纳滤波对一阶gaussian-markov过程的滤波预测。

clear all;
close all;

N = 100;            % signal length
sigu = 0.1;          % excitation noise standard deviation
sigw = 0.5;          % observation noise std
a=0.95;              % state transition matrix (1 x 1)
mu0 = 5;            % initial state mean
sig0 = 0;           % initial state standard deviation


% first order gauss-markov 
s(1) = a*(sig0*randn + mu0) + sigu*randn;   
for n=2:N
   s(n) = a*s(n-1)+sigu*randn;
   if 0 
     s(n) = s(n) + 10;    
   end
end
s = s';     

% observation
x = s+sigw*randn(size(s));

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% Kalman filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

sp(1) = a*mu0;
Mp(1) = a^2*sig0^2 + sigu^2;
K(1) = Mp(1)/(Mp(1) + sigw^2);   
se(1) = sp(1) + K(1)*(x(1) - sp(1)); % estimate of s(1)
Me(1) = (1-K(1))*Mp(1);          

% Kalman recursions
for n=2:N
   sp(n) = a*se(n-1);               % prediction 
   Mp(n) = a^2*Me(n-1)+sigu^2;      % covariance associated with predictor
   K(n) = Mp(n)/(Mp(n) + sigw^2);   % kalman gain
   se(n) = sp(n) + K(n)*(x(n) - sp(n)); % estimate 
   Me(n) = (1-K(n))*Mp(n);          % covariance associated with estimator
end

se = se';   % convert to column vector

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% compare with Wiener filter
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

% steady state autocorrelcation: rss[k+1] = E[s[n]*s[n-k]], k=0,1,...,N
rss=sigu^2*a.^(0:N)/(1-a^2);
rss=rss';   

% noise 
rww = [sigw^2; zeros(N,1)];    

% observed signal 
rxx = rss + rww;


g{1} = rxx(2)/rxx(1);     
h{1} = rss(1)/rxx(1);      % Wiener filter
se_wiener(1) = h{1}*x(1);         

% generalized LD algorithm
for n=2:N
  k = (rxx(n+1)-rxx(n:-1:2)'*g{n-1})/(rxx(1) - rxx(2:n)'*g{n-1});
  d = -k*g{n-1}(end:-1:1);
  g{n} = [g{n-1}; 0] + [d; k];      
  k2 = (rss(n)-rxx(n:-1:2)'*h{n-1})/(rxx(1) - rxx(2:n)'*g{n-1});
  d2 = -k2*g{n-1}(end:-1:1);
  h{n} = [h{n-1}; 0] + [d2; k2];
  se_wiener(n) = h{n}'*x(n:-1:1);
end

se_wiener = se_wiener';     % convert to column vector


figure(1);
plot(x)
title('observation');
xlabel('time step');
ylabel('state');
grid on
ax=axis;
figure(2);
plot(s)
title('true');
xlabel('time step');
ylabel('state');
grid on
axis(ax);
figure(3);
plot(se)
title(['Kalman filter estimate, MSE =' num2str(norm(s - se)^2)]);
xlabel('time step');
ylabel('state');
grid on
axis(ax);
figure(4);
plot(se_wiener)
title(['Wiener filter estimate, MSE =' num2str(norm(s - se_wiener)^2)]);
xlabel('time step');
ylabel('state');
axis(ax);
grid on
orient tall