function [ xhat, Phat ] = kalman_step( xhat, P, y )
%UNTITLED3 Summary of this function goes here
%   Detailed explanation goes here           
     Ts = 1; 
    sigmaW = 0.02;
    sigmaV = 0.01;
    A = [1 Ts 0 0  0 0;
         0  1 0 0  0 0;
         0  0 1 Ts 0 0;
         0  0 0 1  0 0;
         0  0 0 0  1 Ts;
         0  0 0 0  0 1];
    
    C = [1 0 0 0 0 0;
         0 0 1 0 0 0;
         0 0 0 0 1 0];
    
    Q = sigmaW * eye(6);
    R = sigmaV * eye(3);
    
    K = P * ((C')/(C*P*C' + R));
%     y
%     xhat
%     C
    xhat = xhat + K * (y - C*xhat);
    P = P - K*C*P;

    % prediction
    xhat = A*xhat;
    P = A*P*A' + Q;

    Phat=P;
end

