%%%%%%卡爾曼濾波同滑動平均法濾波的比較
clear
clc;
N=300;
CON=25;%房間溫度，假定溫度是恒定的
%%%%%%%%%%%%%%%kalman filter%%%%%%%%%%%%%%%%%%%%%%
x = zeros(1,N);
y = 2^0.5 * randn(1,N) + CON;%加過程噪聲的狀態(tài)輸出

%x(1) = 25;% 可變
x(1) = mean(y);% 可變
p = 1;

Q = 0.0001; %過程噪聲協(xié)方差， 可變
R = 2^0.5; %觀測噪聲協(xié)方差， 可變
for k = 2 : N
 x(k) = x(k - 1);%預估計k時刻狀態(tài)變量的值
 p = p + Q;%對應于預估值的協(xié)方差
 kg = p / (p + R);%kalman gain
 x(k) = x(k) + kg * (y(k) - x(k));
 p = (1 - kg) * p;
end


%%%%%%%%%%%Smoothness Filter%%%%%%%%%%%%%%%%%%%%%%%%

Filter_Wid = 10;
smooth_res = zeros(1,N);
for i = Filter_Wid + 1 : N
 tempsum = 0;
 for j = i - Filter_Wid : i - 1
 tempsum = tempsum + y(j);
 end
 smooth_res(i) = tempsum / Filter_Wid;
end
% figure(1);
% hist(y);
t=1:N;
figure(1);
expValue = zeros(1,N);
for i = 1: N
 expValue(i) = CON;
end
plot(t,expValue,'r',t,x,'g',t,y,'b',t,smooth_res,'k');
legend('expected','estimate','measure','smooth result');
axis([0 N 20 30])
xlabel('Sample time');
ylabel('Room Temperature');
title('Smooth filter VS kalman filter');