UKF 无迹卡尔曼滤波源程序 matlab

function [xEstPEstxPredPPredzPredinovationSK]=ukf（xEstPEstUQffunzRhfundtalphabetakappa）;

% title    :  UNSCENTED KALMAN FILTER  
%
% PURPOSE  :  This function performs one complete step of the unscented Kalman filter.
%
% SYNTAX   :  [xEstPEstxPredPPredzPredinovation]=ukf（xEstPEstUQffunzRhfundtalphabetakappa）
%
% INPUTS   :  - xEst             : state mean estimate at time k  
%             - PEst             : state covariance at time k
%             - U                : vector of control inputs
%             - 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）   
%	      - alpha （optional） : sigma point scaling parameter. Defaults to 1.
%             - beta  （optional） : higher order error scaling parameter. Default to 0.  
%             - kappa （optional） : scalar tuning parameter 1. Defaults to 0.  
%
% OUTPUTS  :  - xEst             : updated estimate of state mean at time 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
%  
%
% NOTES    :  The process model is of the form x（k+1） = ffun[x（k）v（k）dtu（k）]
%             where v（k） is the process noise vector. The observation model is 
%             of the form z（k） = hfun[x（k）w（k）dtu（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.
%
  

% Process defaults

if （nargin < 10）
  alpha=1;
end;

if （nargin < 11）
  beta=0;
end;

if （nargin < 12）
  kappa=0;
end;


% Calculate the dimensions of the problem and a few useful
% scalars

states       = size（xEst（:）1）;
observations = size（z（:）1）;
vNoise       = size（Q2）;
wNoise       = size（R2）;

noises       = vNoise+wNoise;

% Augment the state vector with the noise vectors.
% Note: For simple additive noise models this part
% can be done differently to save on c