function [x,P]=ukf(fstate,x,P,hmeas,y,Q,R)
% UKF   Unscented Kalman Filter for nonlinear dynamic systems
% [x, P] = ukf(f,x,P,h,y,Q,R) returns state estimate, x and state covariance, P 
% for nonlinear dynamic system (for simplicity, noises are assumed as additive):
%           x_k+1 = f(x_k) + w_k
%           y_k   = h(x_k) + v_k
% where w ~ N(0,Q) meaning w is gaussian noise with covariance Q
%       v ~ N(0,R) meaning v is gaussian noise with covariance R
% Inputs:   f: function handle for f(x)
%           x: "a priori" state estimate
%           P: "a priori" estimated state covariance
%           h: function handle for h(x)
%           y: current measurement
%           Q: process noise covariance 
%           R: measurement noise covariance
% Output:   x: "a posteriori" state estimate
%           P: "a posteriori" state covariance
%
% Example:
%{
n=3;      %number of state
q=0.1;    %std of process 
r=0.1;    %std of measurement
Q=q^2*eye(n); % covariance of process
R=r^2;        % covariance of measurement  
f=@(x)[x(2);x(3);0.05*x(1)*(x(2)+x(3))];  % nonlinear state equations
h=@(x)x(1);                               % measurement equation
s=[0;0;1];                                % initial state
x=s+q*randn(3,1); %initial state          % initial state with noise
P = eye(n);                               % initial state covraiance
N=20;                                     % total dynamic steps
xV = zeros(n,N);          %estmate        % allocate memory
sV = zeros(n,N);          %actual
yV = zeros(1,N);
for k=1:N
  y = h(s) + r*randn;                     % measurements
  sV(:,k)= s;                             % save actual state
  yV(k)  = y;                             % save measurment
  [x, P] = ukf(f,x,P,h,y,Q,R);            % ekf 
  xV(:,k) = x;                            % save estimate
  s = f(s) + q*randn(3,1);                % update process 
end
for k=1:3                                 % plot results
  subplot(3,1,k)
  plot(1:N, sV(k,:), '-', 1:N, xV(k,:), '--')
end
%}
%
% Reference: Julier, SJ. and Uhlmann, J.K.. Unscented Filtering and
%  Nonlinear Estimation. Proceedings of the IEEE, Vol. 92, No. 3,
%  pp.401-422, 2004. 
% Henrique M. T. Menegaz, João Y. Ishihara, Geovany A. Borges, and 
%  Alessandro N. Vargas. A Systematization of the Unscented Kalman Filter 
%  Theory. IEEE TRANS. ON AUTOMATIC CONTROL, VOL. 60, NO. 10, OCTOBER 2015
% James B. Rawlings, David Q. Mayne, and Moritz M. Diehl. 
%  Model Predictive Control: Theory, Computation, and Design. 
%  Ebook provided online: https://sites.engineering.ucsb.edu/~jbraw/mpc/
%
% Adapted from Yi Cao's work 04/01/2008
% https://de.mathworks.com/matlabcentral/fileexchange/18217-learning-the-unscented-kalman-filter
%
n=numel(x);                                 %numer of states
m=numel(y);                                 %numer of measurements
alpha=1;                                    %default, tunable
ki=0.1;                                     %default, tunable
beta=2;                                     %default, tunable
lambda=alpha^2*(n+ki)-n;                    %scaling factor
c=n+lambda;                                 %scaling factor
W=[lambda/c 0.5/c+zeros(1,2*n)];            %weights
c_sqrt=sqrt(c);
X=sigmas(x,P,c_sqrt);                       %sigma points around x
[x_pred,X1,P_pred,X2]=ut(fstate,X,W,n,Q);   %unscented transformation of process
[y_pred,Z1,P2,Z2]=ut(hmeas,X1,W,m,R);       %unscented transformation of measurements
PCT=X2*diag(W)*Z2';                         %transformed cross-covariance
Kf=PCT*inv(P2);
x=x_pred+Kf*(y-y_pred);                     %state update
P=P_pred-Kf*PCT';                           %covariance update
function [y,Y,P,Y1]=ut(f,X,W,n,R)
%Unscented Transformation
%Input:
%        f: nonlinear map
%        X: sigma points
%        W: weights
%        n: numer of outputs of f
%        R: additive covariance
%Output:
%        y: transformed mean
%        Y: transformed sampling points
%        P: transformed covariance
%       Y1: transformed deviations
L=size(X,2);     % number of points
y=zeros(n,1);
Y=zeros(n,L);
d=chol(R)*randn(n,L); % sample disturbances/noises
for k=1:L                   
    Y(:,k)=f(X(:,k)) + d(:,k); % in MPC book of Rawlings, Mayne, and Diehl      
    %Y(:,k)=f(X(:,k));        % in Julier and Ulhmann 2004 paper
    y=y+W(k)*Y(:,k);       
end
Y1=Y-y(:,ones(1,L));
P=Y1*diag(W)*Y1'+R;
eig(P)
function Z=sigmas(x,P,c)
%Sigma points around reference point
%Inputs:
%       x: reference point
%       P: covariance
%       c: coefficient for 2n points
%Output:
%       Z: Sigma points
A = c*sqrtm(P);
%A = c*chol(P)'; % using Cholesky factorization also works
Y = x(:,ones(1,numel(x)));
Z = [x Y+A Y-A];