inamkalman.m

来自「simplst kalman filter estimation results」· M 代码 · 共 39 行

%**************************************
%%%%       KALAMAN FILTER         %%%%%
%**************************************
clear all
close all
A=[1];
H=[1];
Q=input('process noise covarinace = ');
R=input('measurement noise covariance = ');
Iteration=input('No. of iteration = ');
Z=-0.37727;
X_initial=0;
P_initial=1;
for k=1:Iteration
    Z_hat(k)=Z+sqrt(0.01)*randn(1,1);
    %%%%%%%%%%% PRIDICTOR%%%%%%%%%%%%%%%
    if (k==1)
        X_predict(1)=X_initial;
        P_predict(1)=P_initial+Q;
    else
       X_predict(k)=X_hat(k-1);
       P_predict(k)=P(k-1)+Q;  
      end
    %%%%%%%%%%%% CORRECTOR  %%%%%%%%%%%%%%
    Kalm_gain(k)=P_predict(k)*(inv(P_predict(k)+R));
    X_hat(k)=X_predict(k)+Kalm_gain(k)*(Z_hat(k)-X_predict(k));
    P(k)=(1-Kalm_gain(k))*P_predict(k);  
end 
k=0:49;
plot(k,Z,'linewidth',5)
xlabel('iteration');
ylabel('Voltage');
hold on
plot(k,X_hat,'linewidth',2)
hold on
plot(k,Z_hat,'+')