理论部分见以前的博客。
代码：

%kalman filter
%x=Ax+B(u+w)
%y=cx+D+v

clear all;
close all;

%对象的连续模型（传递函数）
sys=tf([133],[1,25,0]);

%对象的连续模型离散化
ts=0.001;
dsys=c2d(sys,ts,'z');
[num,den]=tfdata(dsys,'v');

%对象的连续状态空间
A1=[0 1;0 -25];
B1=[0;133];
C1=[1 0];
D1=[0];
ss1= ss(A1,B1,C1,D1);%对象的连续状态空间
dss=c2d(ss1,ts,'z');%将对象的连续状态空间离散化
[A,B,C,D]=ssdata(dss);%获取对象的离散状态空间的abcd

%输入(包含噪声)
for i=1:1:3000
    u(i)=0.1*rands(1)+1.0*sin(2*pi*1.5*i*ts);
end

for i=1:1:3000
    v(i)=0.1*rands(1);
end

%数据初始化
y_1=0; %上一时刻对象的输出
y_2=0;%上一上一时刻对象的输出
u_1=0;%上一时刻对象的输入
u_2=0;%上一上一时刻对象的输入
x_1=zeros(2,1);%上一时刻对象的状态
Q=1;
p_1=B*Q*B';
R=1;
y=zeros(1,3000);
yv=zeros(1,3000);
y_filter=zeros(1,3000);

%滤波开始
for k=1:1:3000
    time(k)=k*ts;
    
    Q=1;
    y(k)=-den(2)*y_1-den(3)*y_2+num(2)*u_1+num(3)*u_2;%对象输出
    yv(k)= y(k)+v(k);%对象输出加上测量噪声，形成观测值，就是对他进行滤波校正
    
    x=A*x_1+B*u(k);  %注意这里易错：因为等号右边已经为数组形式，所以不能像以前那样将其写成：x（k）=A*x_1+B*u(k)，因为不能把一个数组保存到另外一个数组里面，所以等号左边只能为一个变量，不能为一个数组。
                                %今后也要注意职能将数字保存到数组中，不能将数组保存到数组中。
    p=A*p_1*A'+B*Q*B';
    K=p*C'/(C*p*C'+R);
    x= x+K*(yv(k)-C*x);
    y_filter(k)=C*x;
    p=(eye(2)-K*C)*p;
    
    u_2=u_1;
    u_1=u(k);
    
    y_2=y_1;
    y_1=y(k);
    
    x_1= x;
    p_1=p;
    
end

figure(1);
plot(time,y,'r',time,yv,'k:','linewidth',2);
figure(2);
plot(time,y,'r',time,y_filter,'k:','linewidth',2);

结果：

总结：这部分代码需要注意的地方：
1.状态空间的表达形式以及离散化方法。
2.在进行矩阵运算时要注意等号左右两边的数组的形式是否一样，并且要注意不能把一个数组保存到另外一个数组里面，这些细节会影响代码的书写效率。

simulink仿真

需要注意：卡尔曼滤波模块的输入包括两部分：控制对象的观测值y以及对象的输入u，这两部分都包含相应的干扰。

其中matlab function模块的代码：

function y_filter = fcn(u)

persistent A B C D Q p R  x1 p_1

    A=[1.000 0.0010;0 0.9753];
    B=[0.0001;0.1314];
    C=[1 0];
    D=[0];
    R=1;
    Q=1;


if isempty(x1)
    x1=zeros(2,1);%上一时刻对象的状态
end
   
if isempty(p_1)
    p_1=B*Q*B';
end    
 
    x1=A*x1+B*u(1);  %注意这里易错：因为等号右边已经为数组形式，所以不能像以前那样将其写成：x（k）=A*x_1+B*u(k)，因为不能把一个数组保存到另外一个数组里面，所以等号左边只能为一个变量，不能为一个数组。
                                %今后也要注意职能将数字保存到数组中，不能将数组保存到数组中。
    p=A*p_1*A'+B*Q*B';
    K=p*C'/(C*p*C'+R);
    x1= x1+K*(u(2)-C*x1);
    y_filter=C*x1;
    p=(eye(2)-K*C)*p;

    
    x1= x1;
    p_1=p;

y_filter = y_filter ;

运行结果：