ukf.m

自己写的UKF滤波程序

function X=UKF(Y,X_init,P_init,f_state,h_measure,Q,R,kappa)
%
%      X=UKF(Y,X_init,f_state,h_measure,P_init,Q,R,kappa)
%
% Suppose there is no input to the system
%
% This is UKF filter program-----2*n+1 Sigma points
%      X is the filtered state , every row is a state
%      Y is the measurement , every row is a measurement
%      X_init is the initialization of the mean of X , nX*1
%      P_init is the initialization of the covariance of X , nX*nX
%      f_state is the state tranform function
%      h_measure is the measure function
%      Q is the covariance of process noise , nX*nX
%      R is the covariance of measure noise , nY*nY
%      kappa is a tune parameter , nX+kappa != 0
%

if (nargin < 8)
    disp('Input arguments ERROR!!');
    return ;
end

nX=size(X_init,1);     % Get the dimension of the state
[mY,nY]=size(Y);       % Get the dimension of the measurement
X=zeros(mY,nX);

X_post   = zeros(nX,mY+1);
PX_post  = zeros(nX,nX,mY+1);
X_prior  = zeros(nX,mY);
PX_prior = zeros(nX,nX,mY);
Y_post   = zeros(nY,mY);
PY_post  = zeros(nY,nY,mY);

YSigma_point=zeros(nY,2*nX+1);

X_post(:,1)=X_init;                     % Initialization
PX_post(:,:,1)=P_init;

for k=1:mY

% Time update    
     % Sigma point 
     [Sigma_point,W] = Sigma(X_post(:,k),PX_post(:,:,k),kappa);

     % Transformed Sigma point
     for i=1:2*nX+1
         Sigma_point(:,i) = feval(f_state,Sigma_point(:,i),k);
%          Sigma_point(:,i) = ffun(Sigma_point(:,i),k);
%          Sigma_point(:,i) = f_state(Sigma_point(:,i),k);
         % compute  the a prior state estimation
         X_prior(:,k)=X_prior(:,k)+W(i)*Sigma_point(:,i);
     end

     for i=1:2*nX+1
         PX_prior(:,:,k)=PX_prior(:,:,k)+W(i)*(Sigma_point(:,i)-X_prior(:,k))*(Sigma_point(:,i)-X_prior(:,k))';
     end
     PX_prior(:,:,k)=PX_prior(:,:,k)+Q;
     
% Measure update     
     % Sigma point------this step can be omitted if you want to sacrifice
     % performance
%      [Sigma_point,W] = Sigma(X_prior(:,k),PX_prior(:,:,k),kappa);
     
     % Transformed Sigma point
     for i=1:2*nX+1
         YSigma_point(:,i) = feval(h_measure,Sigma_point(:,i),k);
%          YSigma_point(:,i) = hfun(Sigma_point(:,i),k);
%          YSigma_point(:,i) = h_measure(Sigma_point(:,i),k);
         % compute  the a prior state estimation        
         Y_post(:,k)=Y_post(:,k)+W(i)*YSigma_point(:,i);
     end     
     
     for i=1:2*nX+1
         PY_post(:,:,k)=PY_post(:,:,k)+W(i)*(YSigma_point(:,i)-Y_post(:,k))*(YSigma_point(:,i)-Y_post(:,k))';
     end
     PY_post(:,:,k)=PY_post(:,:,k)+R;
     
     % Compute the cross covariance 
     PXY=zeros(nX,nY);
     for i=1:2*nX+1
         PXY=PXY+W(i)*(Sigma_point(:,i)-X_prior(:,k))*(YSigma_point(:,i)-Y_post(:,k))';
     end
     
     K=PXY*inv(PY_post(:,:,k));
     X_post(:,k+1)=X_prior(:,k)+K*((Y(k,:))'-Y_post(:,k));
     PX_post(:,:,k+1)=PX_prior(:,:,k)-K*PY_post(:,:,k)*K';
     X(k,:)=(X_post(:,k+1))';
     
end