kalmanfilter.m

This programe is kaman-filter s code,which is sample,but very usefull for new study-student.

% kalman-filter
% kalman-filter.m, for the DSP(II) homework 001, pp.61
% using the Burg method to fit a Nth order autoregressive(AR) model
% Author : sunbaoyu
% Date :  March.20th,2009
function kalmnfilter 
clc;
clear;
% measured signals
 y = [-3.2,-0.8,-14,-16,-17,-18,-3.3,-2.4,-18,-0.3,-0.4,-0.8,-19,-2.0,-1.2,-11,-14,-0.9,0.8,10,0.2,0.5,-0.5,2.4,-0.5,0.5,-13,0.5,10,-12,0.5,-0.6,-15,-0.7,15,0.5,-0.7,-2.0,-19,-17,-11,-14];
% Ak,Ck,Qk,Rk 
 Ak = 1/exp(0.02);
 Ck = 1;
%Qk = 1-exp(-0.02).^2;
 Qk = 1;
 Rk = 1;
 x(1) = 0;
 p(1) = 1;
for k = 2:1:42
    ps(k)= Ak^2*p(k-1)+Qk
    H(k) = ps(k)*inv((ps(k)+Rk))
    p(k) = (1-H(k))*ps(k)
    x(k) = Ak*x(k-1)+ H(k)*(y(k)-Ak*x(k-1))
end
figure(1);
plot([0.02:0.02:42*0.02],x(1:1:42),'g',[0.02:0.02:42*0.02],y(1:1:42),'r');
ylabel('Amplitue');
xlabel('simple');
grid on;
legend('x is Estimated','y is Measured')
figure(2);
hold on
plot([0.02:0.02:42*0.02],H(1:1:42),'r');
axis([0 42*0.02 0 1]);
ylabel('gain');
xlabel('simple');
legend('H is gain')
hold off
grid on;
disp('The estimated x:');
disp(x);
disp('The measured y:');
disp(y);