kalmfliter.m

数字信号处理

%%%%%%%%%%%进行kalman滤波
A=[0  1;-0.81 1.74];
B=[0 ;1];
C=[0 1];
Q=0.04;
R=1;

%%%%%%%%%%%%%%产生序列x
N=200;
x=zeros(1,N);
u=sin(x/5)';%zeros(N,1);
%%%%%%%%%产生v1噪声序列
randn('seed',0)
w = sqrt(Q)*randn(N,1);
w(1)=0;
v = sqrt(R)*randn(N,1);
 Bx= 1;
Ax=[1 -1.74 0.81];
x=filter(Bx,Ax,w+u);
y=x;
yv=y+v;
P = B*Q*B';         % Initial error covariance
x = zeros(2,1);     % Initial condition on the state
xe=zeros(2,N);
ye = zeros(N,1);
errcov=zeros(N,1);
for i=1:N
  % Measurement update
  Mn = P*C'/(C*P*C'+R);
  x = x + Mn*(yv(i)-C*x);   % x[n|n]
  P = (eye(2)-Mn*C)*P;      % P[n|n]
  xe(:,i)=x;
  ye(i) = C*x;
  errcov(i) = C*P*C';

  % Time update
  x = A*x + B*u(i);        % x[n+1|n]
  P = A*P*A' + B*Q*B';     % P[n+1|n]
end
figure
%subplot(311), 
plot(1:N,y,'b--',1:N,ye,'r-')%,1:N,yv,'g')
title('Time-varying Kalman filter response')
xlabel('No. of samples'), ylabel('Output')
%subplot(312),
figure
plot(1:N,y-yv,'b-',1:N,y-ye,'r-')
xlabel('No. of samples'), ylabel('Output')
%subplot(313)
figure
plot(1:N,errcov), ylabel('Error covar')
%xee=xe(2,:)-ye'