采用Simulink进行仿真！！

Kalman算法由M函数实现。控制干扰信号wk)和测量噪声信号v(k)幅值均为0.10的白噪声信号，输入信号幅值为1.0、频率为0.5Hz 的正弦信号。采用卡尔曼滤波器实现信号的滤波,取O=1，R=1。仿真结果如图1和2所示。

图1 原始信号y及滤波后的信号ye

图2 原始信号y及带有噪声的原始信号ye

仿真图：

Kalman滤波子程序：chap1_27f.m

%Discrete Kalman filter

%x=Ax+B(u+w(k));

%y=Cx+D+v(k)

function[u]kalman(ul,u2,u3)

persistent A B C D Q R P x

yv=u2;

if u3==0

x-zeros(2,1);

ts-0.001;

a=25;

b=133;

sys=tf(b,[1,a,0]);

A1=[0 1;0 -a];

B1=[0;b];

C1=[1 0];

D1=[0];

[A,B.C,D]=c2dm(A1,B1,CI,D1,ts,z');

Q=1; %Covariances of w

R=1; %Covariances of v

P=B*Q*B'; %Initial error covariance

end

%Measurement update

Mn=P*C/C*P*C'+R);

x=A*x+Mn*(yv-C*A*x);

P=(eye(2)-Mn*C)*P;

ye=C*x+D; %Filtered value

u(1)=ye; %Filtered signal

u(2)=yv; %Signal with noise

errcov=C*P*C; %Covariance of estimation error

%Time update

X=A*x+B*ul;

P=A*P*A'+B*Q*B';

作图程序：chap1_27plot.m

close all;figure(1);

plot(tyE,1),r,LY(:,2),k:','linewidth',2);

xlabel('time(s)';ylabel(y.ye');

legend('ideal signal','filtered signal');

figure(2);

plot(ty(:,1)T,ty1(:.1).k:'linewidth', 2);

xlabel( 'tim(s));ylabel(y,yv');

legend('ideal signal', 'signal with noise');