mykalman.m

卡尔曼滤波器matlab源代码。 function [Y,PY,KC]=myKalman(x,A,B,Q,H,R,y0,P0) 这是我课程设计时做的。

function [Y,PY,KC]=myKalman(x,A,B,Q,H,R,y0,P0)
%myKalman function
   
   % get the order of the state vector
   nlenorder=length(A(:,1));
   % get the length of the observed signal
   nlensig=length(x);
  
   nlenu=length(B(:,1));
   u=zeros(nlenu,1);
   
   % inilize the output buffer
   outy=y0;
   outpy=zeros(nlenorder);
   outkc=zeros(length(R(:,:,1)));
   
   Py=outpy;
   Kc=outkc;
   
   % This is the code which implements the discrete Kalman filter:   
   for i=1:1:nlensig-2
       ytemp=outy(i);
       xtemp=x(i);
       for j=1:1:nlenorder-1
          ytemp=[ytemp;0]+[zeros(length(ytemp),1);outy(i+j)];
          xtemp=[xtemp;0]+[zeros(length(xtemp),1);x(i+j)];
       end
       nlenpy=length(outpy);

       % Prediction for state vector and covariance:
       ytemp = A*ytemp + B*u;
       Py = A * Py * A' + Q;

       % Compute Kalman gain factor:
       Kc = Py*H'*inv(H*Py*H'+R);

       % Correction based on observation:
       ytemp = ytemp + Kc*(xtemp-H*ytemp);
       Py = Py - Kc*H*Py;
       
       %updata the filtering results
       outy=[outy;0]+[zeros(length(outy),1);ytemp(1)];
       outpy=[outpy;zeros(nlenorder)]+[zeros(length(outpy(:,1)),nlenorder);Py];
       outkc=[outkc;zeros(length(R(:,:,1)))]+[zeros(length(outkc(:,1)),length(R(:,1)));Kc];
      
   end
   cw=outpy(:,1)+outpy(:,2);
   dw=outkc(:,1)+outkc(:,2);
   %subplot(1,1,1);
   %plot(cw);
   %title('The estimatin error covariance');
   %subplot(1,1,1);
   %plot(dw);
   %title('The Kalman filter gain ');
   
   % Note that the desired result, which is an improved estimate
   % of the sytem state vector x and its covariance P, was obtained
   % in only five lines of code, once the system was defined. (That's
   % how simple the discrete Kalman filter is to use.) Later,
   % we'll discuss how to deal with nonlinear systems.
   
   Y=outy;
   PY=outpy;
   KC=outkc;
  
   return
   
% x=ones(1,100);
% A=[1.8,0;0,-0.81];
% B=[0,0;0,0];
% Q=[0.2,0;0,0.2];
% H=[1,0;0,1];
% R=[0.1,0;0,0.1];
% y0=[0;0];
% P0=[0,0];