function [ mu, cov ] = ukf_estimate_normal( S, w )
%UKF_ESTIMATE_NORMAL Estimate mean, covariance from set of sigma points

% Dimension (3 for 2D localization)
dim = size(S,1);

% Number of sigma points (= number of weights):
n = size(w,1); 

mu = zeros(dim,1);
cov = zeros(dim,dim);

% YOUR CODE STARTS HERE:
for i=1:n
    mu=mu+w(i)*S(i);
end

for i=1:n
   cov=cov+w(i)*(S(i)-mu)*(S(i)-mu)'; 
end
w
S
mu 
cov
% YOUR CODE END HERE:

end