%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