function results = function_KalmanFilter(measures,type,acel, KalmanConfig)
    % Creation of Kalman Filter Results Variable and atribution of time 
    % results = time | theta_x | theta_y | theta_z | .theta_x | .theta_y | .theta_z
    results = zeros(7,1);
    results(1,1) = measures(1,1);

    % Number of Features (size2 minus 1 (time))
    nbrM = size(measures,2)-1;

    % Initial Definitions
    % Initial Trust (zero) size(Pk) = 6x6
    Pk = ones(6)*KalmanConfig(5);
    % Model Error (basically is acceleration) size(Q) = 6x6
    Q  = eye(6);
        % Model error regarding angular position
        Q(1,1) = KalmanConfig(1);     %  x
        Q(2,2) = KalmanConfig(1);     %  y
        Q(3,3) = KalmanConfig(1);     %  z
        % Model error regarding angular velocity
        Q(4,4) = KalmanConfig(2);     % .x
        Q(5,5) = KalmanConfig(2);     % .y
        Q(6,6) = KalmanConfig(2);     % .z

       
    % Begin Cycle (Kalman Filter Calculation)
    for i = 2:size(measures,1)
        % Measure Error size(R) = nxn
        R = zeros(nbrM);
        RconstV = 1;
        RconstI = 1;
        if(size(measures,2) > 7)
            if(measures(i,8) == -60140)
                if(type == 'v'); continue; end
                if(type == 'b'); RconstI = 1e10; end
            end
        end
        if(type == 'i'); RconstI = 1; end
        if(type == 'v'); RconstV = 1; end
            % Measure error regarding angular position from sensors
            R(1,1) = KalmanConfig(3)*RconstV;     %  x
            R(2,2) = KalmanConfig(3)*RconstV;     %  y
            R(3,3) = KalmanConfig(3)*RconstV;     %  z
            % Measure error regarding angular velocity from sensors
            R(4,4) = KalmanConfig(4)*RconstV;     % .x
            R(5,5) = KalmanConfig(4)*RconstV;     % .y
            R(6,6) = KalmanConfig(4)*RconstV;     % .z
            % Measure error regarding camera values
            for j = 1:(nbrM-6)/2
                R(5+2*j,5+2*j) = 1*RconstI;     %  y
                R(6+2*j,6+2*j) = 1*RconstI;     % .y
            end



        % Calculation of time iteration plus definiton of A,B,C and u
        dt = measures(i,1)-results(1,end);
        A = [1  0  0 dt  0  0
             0  1  0  0 dt  0
             0  0  1  0  0 dt
             0  0  0  1  0  0
             0  0  0  0  1  0
             0  0  0  0  0  1];
        B = [dt^2/2      0      0
                  0 dt^2/2      0
                  0      0 dt^2/2
                 dt      0      0
                  0     dt      0
                  0      0     dt];
        % C is of this type:
        % C = [1 0 0 0 0 0]
        %     [0 1 0 0 0 0]
        %     [0 0 1 0 0 0]
        %     [0 0 0 1 0 0]
        %     [0 0 0 0 1 0]
        %     [0 0 0 0 0 1]
        %     [...........]
        %     [0 1 0 0 0 0]
        %     [0 0 0 0 1 0]
        C = eye(6);
        C2 = zeros(2,6); C2(1,2) = 1; C2(2,5) = 1;
        for a = 1:(nbrM-6)/2
            C = [C; C2];
        end
        if(acel); u = [(measures(i,5)-results(5,end))/dt;    % ..x
                       (measures(i,6)-results(6,end))/dt;    % ..y
                       (measures(i,7)-results(7,end))/dt;];  % ..z
        else      u = [0;0;0];
        end


        % State Prediction
        temp_guess = A*results(2:7,end) + B*u;
        % Update
        Gk = Pk*C'*(C*Pk*C'+R)^(-1);

        tempGk = Gk;
        
        % We will always send the same data. If the type is:
        %       % INERTIAL - Gk(:,7:end) = 0;
        %       % VISION   - Gk(:,1:  6) = 0;
        %       % VISION   - continue (if there are no vision data, since it
        %                             will be based only on the model which is
        %                             not recomended)
        %       % BOTH     - Gk always valid (except for measure == -60140)
        if    (type == 'i');                            Gk(:,7:end) = 0;
        elseif(type == 'v' && measures(i,8) ~= -60140); Gk(:,1:  6) = 0;
        elseif(type == 'v' && measures(i,8) == -60140); continue;
        elseif(type == 'b' && measures(i,8) == -60140); Gk(:,7:end) = 0;
        end

        temp_var = temp_guess+(Gk*(measures(i,2:end)'-C*temp_guess));
    %     Gk = tempGk;

        Pk = A*(eye(6)-Gk*C)*Pk*A'+Q;

        temp = [measures(i,1);temp_var];
        results = [results temp];
    end
    results = results';
end