function [mu, sigma] = kf(mu, sigma, u, z)

% get the matrices for the plant model
[A, B] = slam_plant_model();
R = slam_plant_cov(u);

% prediction
muBar = A * mu + B * u;
sigmaBar = A * sigma * A' + R;

% are there any valid measurements?
if ~any(z)
    mu = muBar;
    sigma = sigmaBar;
    return
end

% get the matrices for the measurement model
C = slam_meas_model(muBar);
Q = slam_meas_cov(muBar);

% correction
K = sigmaBar * C' * inv(C * sigmaBar * C' + Q);
mu = muBar + K * (z - C * muBar);
sigma = (eye(size(sigma)) - K * C) * sigmaBar;
