kcode.txt

kalman filter coding

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];