ukf_predict3.m

%UKF_PREDICT3  Augmented (state, process and measurement noise) UKF prediction step
%
% Syntax:
%   [M,P,X,w] = UKF_PREDICT3(M,P,f,Q,R,f_param,alpha,beta,kappa)
%
% In:
%   M - Nx1 mean state estimate of previous step
%   P - NxN state covariance of previous step
%   f - Dynamic model function as inline function,
%       function handle or name of function in
%       form a([x;w],param)
%   Q - Non-singular covariance of process noise w
%   R - Measurement covariance.
%   f_param - Parameters of f               (optional, default empty)
%   alpha - Transformation parameter      (optional)
%   beta  - Transformation parameter      (optional)
%   kappa - Transformation parameter      (optional)
%   mat   - If 1 uses matrix form         (optional, default 0)
%
% Out:
%   M - Updated state mean
%   P - Updated state covariance
%   X - Sigma points of x
%   w - Weights as cell array {mean-weights,cov-weights,c}
% 
% Description:
%   Perform augmented form Unscented Kalman Filter prediction step
%   for model
%
%    x[k+1] = a(x[k],w[k],param)
%
%   Function a should be such that it can be given
%   DxN matrix of N sigma Dx1 points and it returns 
%   the corresponding predictions for each sigma
%   point. 
%
% See also:
%   UKF_PREDICT1, UKF_UPDATE1, UKF_PREDICT2, UKF_UPDATE2, UKF_UPDATE3
%   UT_TRANSFORM, UT_WEIGHTS, UT_MWEIGHTS, UT_SIGMAS 

% Copyright (C) 2003-2006 Simo S�rkk�
% Copyright (C) 2007 Jouni Hartikainen
%
% $Id: ukf_predict3.m 480 2010-10-18 07:45:48Z 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 [M,P,X,w,C] = ukf_predict3(M,P,f,Q,R,f_param,alpha,beta,kappa,mat)

  %
  % Check which arguments are there
  %
  if nargin < 2
    error('Too few arguments');
  end
  if nargin < 3
    f = [];
  end
  if nargin < 4
    Q = [];
  end
  if nargin < 5
    R = [];
  end
  if nargin < 6
    f_param = [];
  end
  if nargin < 7
    alpha = [];
  end
  if nargin < 8
    beta = [];
  end
  if nargin < 9
    kappa = [];
  end
  if nargin < 10
    mat = [];
  end

  %
  % Apply defaults
  %
  if isempty(mat)
    mat = 0;
  end

  %
  % Do transform
  % and add process and measurement noises
  %
  MA = [M;zeros(size(Q,1),1);zeros(size(R,1),1)];
  PA = zeros(size(P,1)+size(Q,1)+size(R,1));
  i1 = size(P,1);
  i2 = i1+size(Q,1);
  PA(1:i1,1:i1) = P;
  PA(1+i1:i2,1+i1:i2) = Q;
  PA(1+i2:end,1+i2:end) = R;
  
  tr_param = {alpha beta kappa mat};
  [M,P,C,X_s,X_pred,w] = ut_transform(MA,PA,f,f_param,tr_param);
    
  % Save sigma points
  X = X_s;
  X(1:size(X_pred,1),:) = X_pred;