%UT_TRANSFORM  Perform unscented transform
%
% Syntax:
%   [mu,S,C,X,Y,w] = UT_TRANSFORM(M,P,g,g_param,tr_param)
%
% In:
%   M - Random variable mean (Nx1 column vector)
%   P - Random variable covariance (NxN pos.def. matrix)
%   g - Transformation function of the form g(x,param) as
%       matrix, inline function, function name or function reference
%   g_param - Parameters of g               (optional, default empty)
%   tr_param - Parameters of the transformation as:       
%       alpha = tr_param{1} - Transformation parameter      (optional)
%       beta  = tr_param{2} - Transformation parameter      (optional)
%       kappa = tr_param{3} - Transformation parameter      (optional)
%       mat   = tr_param{4} - If 1 uses matrix form         (optional, default 0)
%       X     = tr_param{5} - Sigma points of x
%       w     = tr_param{6} - Weights as cell array {mean-weights,cov-weights,c}
%
% Out:
%   mu - Estimated mean of y
%    S - Estimated covariance of y
%    C - Estimated cross-covariance of x and y
%    X - Sigma points of x
%    Y - Sigma points of y
%    w - Weights as cell array {mean-weights,cov-weights,c}
%
% Description:
%   ...
%   For default values of parameters, see UT_WEIGHTS.
%
% See also
%   UT_WEIGHTS UT_MWEIGHTS UT_SIGMAS

% Copyright (C) 2006 Simo S�rkk�
%               2010 Jouni Hartikainen
%
% $Id: ut_transform.m 482 2010-10-18 07:53:23Z jmjharti $
%
% This software is distributed under the GNU General Public 
% Licence (version 2 or later); please refer to the file 
% Licence.txt, included with the software, for details.

function [mu,S,C,X,Y,w] = ut_transform(M,P,g,g_param,tr_param)

  if nargin < 4
     g_param = [];
  end
  
  if nargin < 5
     tr_param = []; 
  end

  %
  % Apply defaults
  %
  if isempty(tr_param) 
     alpha = [];
     beta = [];
     kappa = [];
     mat = [];
     X = [];
     w = [];
  else 
      alpha = tr_param{1}; 
      if length(tr_param) >= 2
          beta = tr_param{2};
      else 
          beta = [];
      end
      if length(tr_param) >= 3          
          kappa = tr_param{3};
      else
          kappa = [];
      end
      if length(tr_param) >= 4
          mat = tr_param{4};
      else
          mat = [];
      end
      if length(tr_param) >= 5
          X = tr_param{5};
      else
          X = [];
      end
      if length(tr_param) >= 6
          w = tr_param{6};
      else
          w = [];
      end
  end
    
  if isempty(mat)
     mat = 0;
  end  
  
  %
  % Calculate sigma points
  %
  if isempty(w) == 0
      WM = w{1};
      c  = w{3};
      if mat
        W  = w{2};
      else
        WC = w{2};
      end
  elseif mat
    [WM,W,c] = ut_mweights(size(M,1),alpha,beta,kappa);
    X = ut_sigmas(M,P,c);
    w = {WM,W,c};
  else
    [WM,WC,c] = ut_weights(size(M,1),alpha,beta,kappa);
    X = ut_sigmas(M,P,c);
    w = {WM,WC,c};
  end
  
  %
  % Propagate through the function
  %
  if isnumeric(g)
    Y = g*X;
  elseif ischar(g) | strcmp(class(g),'function_handle')
    Y = [];
    for i=1:size(X,2)
      Y = [Y feval(g,X(:,i),g_param)];
    end
  else
    Y = [];
    for i=1:size(X,2)
      Y = [Y g(X(:,i),g_param)];
    end
  end
  
  if mat
    mu = Y*WM;
    S  = Y*W*Y';
    C  = X*W*Y';
  else
    mu = zeros(size(Y,1),1);
    S  = zeros(size(Y,1),size(Y,1));
    C  = zeros(size(M,1),size(Y,1));
    for i=1:size(X,2)
      mu = mu + WM(i) * Y(:,i);
    end
    for i=1:size(X,2)
      S = S + WC(i) * (Y(:,i) - mu) * (Y(:,i) - mu)';
      C = C + WC(i) * (X(1:size(M,1),i) - M) * (Y(:,i) - mu)';
    end
  end