function [Mean,Var] = houf(trans,moments,unscented)
% HOUF - performing the unscented transformation through the
% nonlinear function defined by 'trans', where the input moments
% are defined in 'moments'. The sigma selection algorithm is
% defined in 'unscented'.
%   
%  trans     - string defining the nonlinear transformation
%  moments   - 4th order moment vector
%  unscented - sigma point selection algorithm
%              ['symmetric','minimumskew','gut2b']
%
%  MEAN      - mean of the transformed random variable
%  VAR       - variance of the transformed random variable
%
  
% 27. November 2002
% Dirk Tenne
% CoDE - University at Buffalo
% http://code.eng.buffalo.edu/tracking


  %-------------------------
  % sigma set
  %-------------------------
  [xsi_k,w_k]=getSigmaSet(moments,unscented);

  %-------------------------
  % transformation
  %-------------------------
  for i = 1:length(xsi_k)
    eta_k(:,i) = feval(trans,xsi_k(:,i));
  end

  %-------------------------
  % extract moments
  %-------------------------
  [Mean,Var] = getMoments(eta_k,w_k);



function [xsi,w] = getSigmaSet(moments,selector,varargin)
% GETSIGMASET - returns a sigma set based on the specified method
% and the moments. 
%   
%  moments  - 4th order moment vector
%  selector - sigma point selection algorithm
%             ['symmetric','minimumskew','gut2b']
%
%  xsi      - sigma set
%  w        - weights of the sigma set

% 27. November 2002
% Dirk Tenne
% http://code.eng.buffalo.edu/tracking
%
  
  % for UNCORRELATED n-dimensional case put the moments into the
  % correct matrices:
  Mean = moments(:,1);
  Var  = diag(moments(:,2));
  skew = moments(:,3);
  curt = diag(moments(:,4));
  nd = length(Mean); % number of uncorrelated parameters
  
back_transform = 'on'; % some methods might not need the affine
                       % transformation, so turn this to 'off'
	       
switch selector
 case 'symmetric'
  % symmetric sigma set for 1-D
    n = length(Mean);
    if nargin > 2
      kappa = varargin{:};
    else
      kappa = 3-n; % matching Gaussian fourth order central moment
      %kappa = 9/5-n; % matching the fourth moment for uniform
                     % distr.
    end
    
    % calculate the weights
    w0 = kappa/(n+kappa);
    w1 = 1/2/(n+kappa);
    %w2 = w1;
    %w  = [w0 w1 w2];
    w  = [w0 w1*ones(1,2*n)];
    % calculate the sigma set
    L = sqrt((n+kappa)*Var);
    sigma1 = -L;
    sigma2 =  L;
    xsi = [zeros(n,1) sigma1 sigma2] + diag(Mean)*ones(n,2*n+1);
    back_transform = 'off';
  
 case 'minimumskew'
  % minimum skew sigma set for 1-D
  
  % calculate the weights
    w0 = sqrt(2)/2;
    w1 = (1-w0)/2;
    w2 = w1;
    w  = [w0 w1 w2];
  % calculate the sigma set
    sigma1 = -1/sqrt(2*w1);
    sigma2 = 1/sqrt(2*w1);
    xsi = [0 sigma1 sigma2];
 
 case 'gut2b'
  % gaussian unscented transform with two sigmas
  % this is a more efficient way of calucating the sigma set,
  % espacially the weights don't depend on Px,S1,S2.
    if nd > 1
      Var = diag(Var);
    end

    sigma1 = diag(sqrt((5+sqrt(10))*Var));
    sigma2 = diag(sqrt((5-sqrt(10))*Var));
    xsi = [zeros(nd,1) -sigma1 sigma1 -sigma2 sigma2];
    xsi = xsi + diag(Mean)*ones(size(xsi));
    wr = (sqrt(10)-2)/(sqrt(10)+2);
    w1 = wr/20;
    w2 = 1/wr/20;
 
    w0 = 1-2*nd*(w1+w2);
    w = [w0 w1*ones(1,2*nd) w2*ones(1,2*nd)];
    % turn the back transformation off
    back_transform = 'off';
 
 otherwise
  error(['Unknown sigma point selector''',selector,'''!'])
end
  
  % affine back transformation
  if strcmp(back_transform,'on')
    s = sqrt(Var);
    xsi = Mean + s*xsi;
  end

  
function [Mean,Var] = getMoments(eta,w)
% GETMOMENTS - returns the moments based on the sigma set eta with
% weights w. 
%    
%  eta      - sigma set
%  w        - weights of the sigma set
%
%  moments  - 4th order moment vector
%  dmoments - gradient of moments [grad(mean) grad(var)...]

% 27. November 2002
% Dirk Tenne
% http://code.eng.buffalo.edu/tracking
%

  ne = size(eta,1);
  if ne == 1
    Mean = sum(w.*eta);
    eta0 = eta-Mean;
    Var  = sum(w.*eta0.^2);
  else
    wr = repmat(w,ne,1);
    Mean = sum(wr.*eta,2);
    eta0 = eta-diag(Mean)*ones(size(eta));
    Var = eta0*(eta0.*wr)';
  end   
    
