function [scorrected spredict K minPredictMSE minMSE] = KalmanFilter (s, x, a, varu, varn, ms, vars) N = length(x); spredictInit = ms; minMSEInit = vars; for k=1:N % PREDICTION if k>1 spredict(k) = a*scorrected(k-1); else spredict(k) = a*spredictInit; end; % Min Prediction MSE if k>1 minPredictMSE(k) = (a^2)*minMSE(k-1) + varu; else minPredictMSE(k) = (a^2)*minMSEInit + varu; end; % Kalman Gain K(k) = minPredictMSE(k)/(minPredictMSE(k)+varn(k)); % Correction scorrected(k) = spredict(k) + K(k)*(x(k)-spredict(k)); % Min MSE minMSE(k) = (1-K(k))*minPredictMSE(k); clf; subplot(3,1,1); plot(s(1:k),'.-');hold on; plot(x(1:k),'c.-'); plot(spredict(1:k),'k.-');plot(scorrected(1:k),'ro-'); legend('true signal','observed','predicted','corrected'); subplot(3,1,2); plot(K(1:k),'.-');legend('K[n]'); subplot(3,1,3); plot(minPredictMSE,'.-');hold on; plot(minMSE,'r.-');legend('Prediction MSE','MSE'); pause(0.2); end;