1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42
| %% Adaptive Kalman filter
clear all;clc;format short;
z=[0.39, 0.5, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45,0.39, 0.5, 0.48, 0.29, 0.25, 0.32, 0.34, 0.48, 0.41, 0.45];
%% First Order Adaptive Kalman Filter
x_inter=zeros(1,20);
x_current=zeros(1,20);
P_current=1;
Q=0.382;
q_t=0.1;
R=0.1;
A=1;
B=1;
u=0;
for t=2:20
% **************Predict**************
x_inter(t)=A*x_current(t-1)+B*u;
P_inter=P_current+Q;
% **************Update**************
K=P_inter/((P_inter+R));
x_current(t)=x_inter(t)+K*(z(t)-x_inter(t));
%x_current(t)=(1-K)*x_inter(t)+K*A(t)+(1-K)*q_t;
P_current=(1-K)*P_inter;
end
real_value=z
estimated=x_current
mse = (sum(x_current.^2))/20
figure();
hold on;
plot(z,'b');
plot(x_inter,'r');
plot(x_current,'g');
hold off; |
Partager