trykalman.m

“假设我们要研究的对象是一个房间的温度。根据你的经验判断

%现在我们模拟一组测量值作为输入。假设房间的真实温度为25度，我模拟了200个测量值，这些测量值的平均值为25度，但是加入了标准偏差为几度的高斯白噪声（在图中为蓝线）。

%为了令卡尔曼滤波器开始工作，我们需要告诉卡尔曼两个零时刻的初始值，是X(0|0)和P(0|0)。他们的值不用太在意，随便给一个就可以了，因为随着卡尔曼的工作，X会逐渐的收敛。但是对于P，一般不要取0，因为这样可能会令卡尔曼完全相信你给定的X(0|0)是系统最优的，从而使算法不能收敛。我选了X(0|0)=1度，P(0|0)=10。

%该系统的真实温度为25度，图中用黑线表示。图中红线是卡尔曼滤波器输出的最优化结果（该结果在算法中设置了Q=1e-6，R=1e-1）。
%2006-9-26

clc
clear all
N=200;
x0=1;
p0=10;
Q=10^-6;
R=10^-1;
t=1:N;
for i=1:N
    z(i)=25+randn*3;
end
mean(z),std(z)
plot(t,z,'k');hold on

    y(1)=x0;
    p1(1)=p0+Q;
    kg(1)=p1(1)/(p1(1)+R);
    x(1)=y(1)+kg(1)*(z(1)-y(1));
    p(1)=(1-kg(1))*p1(1);

for i=2:N
    y(i)=x(i-1);
    p1(i)=p(i-1)+Q;
    kg(i)=p1(i)/(p1(i)+R);
    x(i)=y(i)+kg(i)*(z(i)-y(i));
    p(i)=(1-kg(i))*p1(i);
end

plot(t,x,'r');hold on
plot([0,200],[25,25],'g');