function [ S w] = ukf_sigma_points( mu, cov, w0 )
%UKF_SIGMA_POINTS Compute sigma points from mean and covariance matrix.

% Use default value for w0 if it is not provided as a parameter:
if nargin < 3
    w0 = 1/3;
end

nx = size(mu,1); % Dimension (3 for 2D localization)

n = 2*nx + 1;    % n sigma points

% Store sigma points in (nx x n) matrix
% Sigma point i is S(:,i)
S = zeros(nx, n);

% Store weights in (n x 1) matrix:
% Weight of sigma point i is w(i);
w = zeros(n,1);

% YOUR CODE STARTS HERE

S(:,1)=mu;
w(1)=1/3;
w(2:n)=(1-w0)/(2*nx);

if (sqrtm(cov)^2 ~= cov)
    cov
    sqrtm(cov)^2
end

RootN= sqrt(nx/(1-w0))*sqrtm(cov);

for i =1:nx
    S(:,i+1) = mu+RootN(:,i);
    S(:,i+nx+1) = mu-RootN(:,i);
end  


% YOR CODE ENDS HERE

end