kalman例子

% Kalman filter example demo in Python
% A Python implementation of the example given in pages 11-15 of "An
% Introduction to the Kalman Filter" by Greg Welch and Gary Bishop,
% University of North Carolina at Chapel Hill, Department of Computer
% Science, TR 95-041,
% http://www.cs.unc.edu/~welch/kalman/kalmanIntro.html
% by Andrew D. Straw






% intial parameters
clear,close all
sz = 50; % size of array
x = -0.37727; % truth value (typo in example at top of p. 13 calls this z)
z = 0.1*randn(1,50)+x; % observations (normal about x, sigma=0.1)


Q = 1e-5; % process variance
R =0.1^2 ;% estimate of measurement variance, change to see effect  0.1^2


% allocate space for arrays
xhat=zeros(sz);      % a posteri estimate of x
P=zeros(sz);         % a posteri error estimate
xhatminus=zeros(sz); % a priori estimate of x
Pminus=zeros(sz);    % a priori error estimate
K=zeros(sz);        % gain or blending factor




% intial guesses
xhat(1) = 0.0;
P(1) = 1.0;


for k=2:sz
    % time update
    xhatminus(k) = xhat(k-1);
    Pminus(k) = P(k-1)+Q;


    % measurement update
    K(k) = Pminus(k)/( Pminus(k)+R );
    xhat(k) = xhatminus(k)+K(k)*(z(k)-xhatminus(k));
    P(k) = (1-K(k))*Pminus(k);
end
x=zeros(50)-0.37727 ;%每个值都是0.37727
w=1:50;
plot(w,x,' ',w,z,'+',w,xhat,'-');
legend('true value','noisy measurements','a posteri estimate')
axis([1 50 -0.7 0]);%define the range of the axises
xlabel('Iteration')

ylabel('Voltage')