robustkalman.m

对输入的信号进行卡尔曼滤波。内插信号

%The compare of kalman filter with robust kalman filter designed by stochasic use the algorithm of Isrial 



[a1 b1 c1 d1]=tf2ss([0 0 1],[1 0 0]);
%[a2 b2 c2 d2]=tf2ss([18 18],[1 10]);
%[a3 b3 c3 d3]=series(a1,b1,c1,d1,a2,b2,c2,d2);
%[a0 b0 c0 d0]=cloop(a3,b3,c3,d3);
a0=[0 -0.5;1 1];
b0=[-6;1];
c0=[-100 10];
d0=0;
dimf=size(b0); %output dimention x*y of b0
noinp=dimf(2);
dimc=size(c0);
noout=dimc(1);
adelta=[0+0.11 -0.5;1 1+0.3];
t=[0:0.01:1.5]';
q0=1;
w=sqrt(q0)*randn(length(t),noinp);
r0=1;
nu=sqrt(r0)*rand(length(t),noout);



[y,x]=lsim(a0,b0,c0,d0,w,t);
y=x*c0'+nu;%y is the real estimate value


[L P E]=lqe(a0,b0,c0,q0,r0);
[a3 ,b3,c3,d3]=estim(a0,b0,c0,d0,L);
[y1,xh]=lsim(a3,b3,c3,d3,y,t);

%k=[-0.0088 0.0079]';
%k=[-0.0066;0.0045];
k=[-0.0053;0.0016];

%a=[0.1349 -0.4813;1.4169 1.2367];
a=[0 -0.5861;1 1.1895];
xestimate=zeros(length(t),2);
for i=1:length(t)
  xestimate(i,:)=(a0*(xestimate(i,:))'+k*(y(i)-c0*(xestimate(i,:))'))';
    
%xestimate(i,:)=(a*(xestimate(i,:))'+k*(y(i)-[100 0]*(xestimate(i,:))'))';
end

subplot(221);
plot(t,x(:,1),'b+',t,xh(:,1),'g',t,xestimate(:,1),'r-');
title('第一个状态变量的估计');
xlabel('蓝色,真实值,绿色,卡尔曼值,红色,鲁棒估计值');
subplot(222);
plot(t,x(:,2),'b+',t,xh(:,2),'g',t,xestimate(:,2),'r-');
title('第二个状态变量的估计');
subplot(223);
plot(t,abs(x(:,1)-xh(:,1)),'g',t,abs(x(:,1)-xestimate(:,1)),'r');
title('第一个状态变量的估计的差值');
xlabel('绿色,卡尔曼估计差值,红色,鲁棒估计差值');
subplot(224);
plot(t,abs(x(:,2)-xh(:,2)),'g',t,abs(x(:,2)-xestimate(:,2)),'r');
title('第二个状态变量的估计的差值');
xlabel('绿色,卡尔曼估计差值,红色,鲁棒估计差值');