卡拉曼算法简答程序

clc;
clear all;
close all;

n = 300;
con = 35;

x = zeros(1,n);
y = sqrt(2) * randn(1,n) + con;
figure(1);
hist(y);
%% kalman filter
x(1) = 35;
p = 10;
Q = cov(randn(1,n));
R = cov(randn(1,n));
for k = 2:n
x(k) = x(k-1);
p = p + Q;
kg = p/(p + R);
x(k) = x(k) + kg * (y(k) -x(k));
p = (1 - kg) * p;
end
%% smoothness filter
filterWid = 10;
smoothRes = 35*ones(1,n);
for i = filterWid+1:n
tempsum = 0;
for j = i - filterWid:i - 1
tempsum = tempsum + y(i);
end
smoothRes(i) = tempsum/filterWid;
end
%% figure
t = 1:n;
expValue = zeros(1,n);
for i = 1:n
expValue(i) = con;
end
figure(2);
plot(t,expValue,'r',t,y,'b');
axis([0,n,20,40]);
xlabel('Sample time');
ylabel('Room temperature');
grid on;
title('actual signal');
legend('expented','measure');

figure(3);
plot(t,smoothRes,'k',t,x,'g','linewidth',2);