function [ mean, cov ] = ukf_correction( pred_mean, pred_cov, z, Q, measurement )
%UKF_CORRECTION UKF correction step

% YOUR CODE STARTS HERE:
[X,w]=ukf_sigma_points(pred_mean,pred_cov);
%Sm= rowfun(motion,S) % By default, rowfun returns the first output of func
%wie bring ich hier das u unter?
Z=zeros(size(z,1),size(X,2));
for i =1:size(X,2)
    Z(:,i)= measurement(X(:,i));
end
expX=pred_mean;
covX=pred_cov;
%[expX,covX]=ukf_estimate_normal( X, w );
[expZ,CovOfIn]=ukf_estimate_normal( Z, w );
CovOfIn = CovOfIn+Q;

CrossCov = zeros(size(X,1),size(z,1));
for i =1:size(X,2)
    CrossCov = CrossCov+ w(i)*(X(:,i)-expX)*(Z(:,i)-expZ)';
end

K= CrossCov*inv(CovOfIn);
mean= expX+K*(z-expZ);
cov= covX * K*CovOfIn*K';



% YOUR CODE ENDS HERE:

end