function kalman1(L,Ak,Bk,Wk,Vk,Hk,Rw,Rv,Z1,Z2,P0,x0)
Ak=[1 0;0 1];
Bk=[1 0;0 1];
Wk=[1 0;0 1];
Vk=[1 0;0 1];
Hk=[1 0;0 1];
L=200;
x0=[1 1]';
xx0=[1 1]';
p0=[1 0;0 1];
Rw=input('Rw=');
Rv=input('Rv=');
x(:,1)=x0;
for k=2:L
x(:,k)=sin(:,k);
x(:,k)=Ak*x(:,k-1)+Wk*sqrt(Rw)*randn(2,1);
y(:,k)=Hk*x(:,k)+Vk*sqrt(Rv)*randn(2,1);
end
subplot(2,2,1);
n=1:L;
plot(n,x(:,n),'r',n,y(:,n),'b');
title('红线为x(:n),蓝线为y(:,n)');
Rk=Vk*Vk'*Rv;
Qk=Wk*Wk'*Rw;
xx(:,1)=xx0;
p=p0;
for k=2:L
x_x(:,k)=Ak*x(:,k-1);
p_p=Ak*p*Ak'+Qk;
K=p_p*Hk'*inv(Hk*p_p*Hk'+Rk);
I=eye(size(K));
p=(I-K*Hk)*p_p;
xx(:,k)=Ak*x_x(:,k)+K*(y(:,k)-Hk*Ak*x_x(:,k));
yy(:,k)=Hk*xx(:,k);
end
subplot(2,2,2);
plot(n,x(:,n),'r',n,xx(:,n),'b');
title('红线为x(:n),蓝线为xx(:,n)');
subplot(2,2,3);
plot(n,y(:,n),'r',n,yy(:,n),'b');
title('红线为y(:n),蓝线为yy(:,n)');
%Z1=input('Z1=');
%Z2=input('Z2=');
H1k=input('H1k=');
H2k=input('H2k=');
for k=2:L
K1=p_p*H1k'*inv(H1k*p_p*H1k'+Rk);
x1(:,k)=Ak*xx(:,k-1)+K1*(yy(:,k)-H1k*xx(:,k));
p1=(I-K1*H1k)*p_p;
K2=p1*H2k'*inv(H2k*p1*H2k'+Rk);
%x2(:,k)=Ak*xx(:,k-1)+K1*(Z1-H1k*xx(:,k-1))+K2*(Z2-H2k*x1(:,k));
x2(:,k)=Ak*xx(:,k-1)+K1*(H1k*x1(:,k)+Vk*sqrt(Rv)*randn(2,1)-H1k*xx(:,k-1))+K2*(H2k*x1(:,k)+Vk*sqrt(Rv)*randn(2,1)-H2k*x1(:,k));
p2=intersect((I-K1*H1k)*p_p,(I-K2*H2k)*p_p,'rows');
X(:,k)=x2(:,k);
P=p2;
end
subplot(2,2,4);
plot(n,X(:,n),'r',n,x(:,n),'b');
title('红线为X(:n),蓝线为x(:,n)');
