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
%Z=rowfun(measurement,X); %doesn't work like this...

[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)-pred_mean)*(Z(:,i)-expZ)';
end

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



% YOUR CODE ENDS HERE:

end