卡尔曼滤波器在simulink 中的实现

自己在MATLAB中构造的卡尔曼滤波器，挺好用的，分享给大家。

%Discrete Kalman filter
%x=Ax+B（u+w（k））;
%y=Cx+D+v（k）
function [u]=kalman（u1u2u3）
persistent A B C D Q R P x

yv=u2;
if u3==0
   x=zeros（21）;
   ts=0.001;
   a=25;b=133;
   sys=tf（b[1a0]）;
   A1=[0 1;0 -a];
   B1=[0;b];
   C1=[1 0];
   D1=[0];
   [ABCD]=c2dm（A1B1C1D1ts‘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*u1;
P=A*P*A‘+B*Q*B‘;

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件        293  2012-03-30 11:04  kalman_plot.m

     文件        744  2012-03-30 11:16  kalman_fun.m

     文件      31413  2012-03-30 11:20  kalman_mdl.mdl

----------- ---------  ---------- -----  ----

                32450                    3