kalman.m

k卡尔曼滤波器

% 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

% 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;
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')

%plot the change of P with time

%figure,
%plot(w,P);
%title('the change of the cov');
%xlabel('Iteration')
%ylabel('P(k)')

%figure();
%valid_iter = range(1,sz); % Pminus not valid at step 0
%plot(valid_iter,Pminus(valid_iter));
%legend('a priori error estimate');
%xlabel('Iteration');
%ylabel('$(Voltage)^2$');
%setp(pylab.gca(),'ylim',[0,.01]);
%show();