output.m

（我的课程报告）基本卡尔曼滤波实现的代码 可绘制模型状态、状态预报、模型输出、输出预报。程序中可改变某些参数观察变化

%%利用模型生成X(k),y(k)
R1=1;
R2=1;

e=randn(1,3000);
e=e/std(e);
e=e-mean(e);
a=0;
b=sqrt(R1);
e=a+b*e;


z=randn(1,3000);
z=z/std(z);
z=z-mean(z);
c=0;
d=sqrt(R2);
z=c+d*z;

x=cell(1,3000);
x{1}=[5 ; -5];
A=[0.9 0.1;-0.1 0.8];
B=[1;1];
C=[0 1];
for k=1:1:3000
    x{1,k+1}=A*x{1,k}+B*e(k);
    y(k)=C*x{1,k}+z(k);
end

for i=1:1:3000
    p(i)=x{1,i}(1,1);
    q(i)=x{1,i}(2,1);
end

subplot(3,1,1)
plot(p);
title('xi(1)');

subplot(3,1,2)
plot(q);
title('xi(2)');

subplot(3,1,3)
plot(y);
title('y');


%%求X(k|k-1),y(k)

x_=cell(1,3000);
sigma=cell(1,3000);
K=cell(1,3000);
 x_{1}=[5;-5];
sigma{1}=[5 0;0 5];
K{1}=A*sigma{1}*C'*inv([C*sigma{1}*C'+R2]);

for i=1:1:3000
    x_{i+1}=A*x_{i}+K{i}*[y(i)-C*x_{i}];         %%对状态的提前一步预报
    sigma{i+1}=[A-K{i}*C]*sigma{i}*A'+B*R1*B';
    K{i+1}=A*sigma{i+1}*C'*inv([C*sigma{i+1}*C'+R2]);
    y_(i)=C*x_{i};         %%对y(k+1)的预报 
    err_y(i)=y(i)-y_(i);
end

for i=1:1:3000
    m(i)=x_{i}(1,1);
    n(i)=x_{i}(2,1);
end

for i=1:1:3000
    s1(i)=sigma{i}(1,1);
    s2(i)=sigma{i}(1,2);
    s3(i)=sigma{i}(2,1);
    s4(i)=sigma{i}(2,2);
end

%%绘制X^(k|k-1)及X(k)的第一分量
figure
subplot(4,1,1)
plot(p,':bo');
hold on
plot(m,'--m*');
legend('X(k)','X^-(k|k-1)(1)')
title('R1=1');
title('X(k) and X^-(k|k-1)');

%%绘制X^(k|k-1)及X(k)的第二分量
subplot(4,1,2)
plot(q,':bo');
hold on
plot(n,'--m*');
title('X(k) and X^-(k|k-1)(2)');
legend('X(k)','X^-(k|k-1)')

%%绘制模型输出y及y^(k|k-1)
subplot(4,1,3)
plot(y_,'b');
hold on
plot(y,'r'); 
title('y(k) and y^-(k|k-1)');
legend('y^','y')

%%绘制输出预报误差
subplot(4,1,4)
plot(err_y);
title('Error of y'' forecast');
ylabel('Error');

%%绘制sigma
figure
plot(s1,'r');
hold on
plot(s2,'--mo');
hold on
plot(s3,'-.b*');
hold on
plot(s4,'k');
title('\Sigma');
legend('\Sigma(1,1)','\Sigma(1,2)','\Sigma(2,1)','\Sigma(2,2)')