iekf.m

介绍了kf,ekf,ukf ,pf ,upf 的程序代码

function [xEst,PEst,xPred,PPred,zPred,inovation,S,K]=iekf(xEst,PEst,Q,ffun,z,R,hfun,dt);

% TITLE    :  ITERATED EXTENDED KALMAN FILTER  
%
% PURPOSE  :  This function performs one complete step of the unscented Kalman filter.
%
% SYNTAX   :  [xEst,PEst,xPred,PPred,zPred,inovation]=iekf(xEst,PEst,U,Q,ffun,z,R,hfun,dt,alpha,beta,kappa)
%
% INPUTS   :  - xEst             : state mean estimate at time k  
%             - PEst             : state covariance at time k
%             - Q                : process noise covariance at time k  
%             - z                : observation at k+1  
%             - R                : measurement noise covariance at k+1  
%             - ffun             : process model function  
%             - hfun             : observation model function  
%             - dt               : time step (passed to ffun/hfun)   
%
% OUTPUTS  :  - xEst             : updated estimate of state mean at time k+1
%             -xEst1             : updated estimate of state mean at time
%             (the former circular)
%             -zPred           prediction of the measurement at the k+1
%	          - PEst                 : updated state covariance at time k+1
%             - xPred            : prediction of state mean at time k+1
%             - PPred            : prediction of state covariance at time k+1
%	          - inovation            : innovation vector
%  
% AUTHORS  : Li LiangQun
%
% DATE     :  24 May 2005
%
% NOTES    :  The process model is of the form, x(k+1) = ffun[x(k),v(k),dt,u(k)]
%             where v(k) is the process noise vector. The observation model is 
%             of the form, z(k) = hfun[x(k),w(k),dt,u(k)], where w(k) is the 
%             observation noise vector.
%
%             This code was written to be readable. There is significant
%             scope for optimisation even in Matlab.
%

%begin
xPred=feval('ffun',xEst,dt);
Jx=0.5;
PPred=Q+Jx*PEst*Jx';
xEst1=xPred;

%
for i=1:2
    zPred=feval('hfun',xEst1,dt);
    if dt<=30
        Jy=2*0.2*xEst1;
    else
        Jy=0.5;
    end
    M =Jy*PPred*Jy'+R;                 % Innovations covariance.
    PEst=PPred-PPred*Jy'*inv(M)*Jy*PPred;
    xEst=xEst1+PEst*Jy'*inv(R)*(z-zPred)-PEst*PPred*(xEst1-xPred); 
    xEst1=xEst;
end
% update the state covariance  
    zPred=feval('hfun',xEst1,dt);
    if dt<=30
        Jy=2*0.2*xEst1;
    else
        Jy=0.5;
    end
    M =Jy*PPred*Jy'+R;                 % Innovations covariance.
    PEst=PPred-PPred*Jy'*inv(M)*Jy*PPred;