function kalman_filter_demo()
    delta_t = 1.0;

    % Variância do ruído do processo e da medição (constrói R e Q)
    sigma_a2 = 0.1;
    sigma_z2 = 0.5;

    x = [0; 0; 1; 1; 0; 0];
    %comecei P como identidade como foi instruído pelo horie, salve
    %grande Horie :)
    P = eye(6);

    %Matriz que chamamos de A, modelo embarcado nos coeficientes 
    Fk = [1, 0, delta_t, 0, 0.5 * delta_t^2, 0;
          0, 1, 0, delta_t, 0, 0.5 * delta_t^2;
          0, 0, 1, 0, delta_t, 0;
          0, 0, 0, 1, 0, delta_t;
          0, 0, 0, 0, 1, 0;
          0, 0, 0, 0, 0, 1];

    Hk = [1, 0, 0, 0, 0, 0;
          0, 1, 0, 0, 0, 0];

    % Var(kX) = k^2Var(X) bão lembrar disso :)
    Qk = [delta_t^4 / 4, 0, delta_t^3 / 2, 0, delta_t^2 / 2, 0;
          0, delta_t^4 / 4, 0, delta_t^3 / 2, 0, delta_t^2 / 2;
          delta_t^3 / 2, 0, delta_t^2, 0, delta_t, 0;
          0, delta_t^3 / 2, 0, delta_t^2, 0, delta_t;
          delta_t^2 / 2, 0, delta_t, 0, 1, 0;
          0, delta_t^2 / 2, 0, delta_t, 0, 1];
    Qk = sigma_a2 * Qk;

    % Matriz de covariância do ruído da medição
    %Considerei que o erro na medição de X não influi no erro da medição de
    %Y, acho mt razoável isso :|
    Rk = sigma_z2 * eye(2);

    % Simulei aqui posições com ruído Gaussiano
    true_positions = [linspace(0, 20, 40)', linspace(0, 10, 40)'];
    zk = true_positions + randn(40, 2) * sqrt(sigma_z2);

    estimated_positions = zeros(40, 2);
    velocities = zeros(40, 2);
    accelerations = zeros(40, 2);

    for k = 1:40
        [x, P] = kalmanFilter(x, P, Fk, Hk, Qk, Rk, zk(k, :)');
        estimated_positions(k, :) = x(1:2)';
        velocities(k, :) = x(3:4)';
        accelerations(k, :) = x(5:6)';
        fprintf('Iteração %d:\n', k);
        disp('Estado atualizado:');
        disp(x);
        disp('Covariância do estado atualizado:');
        disp(P);
    end
  % Plot das posições medidas e corrigidas
    figure;
    subplot(3, 2, 1);
    plot(zk(:, 1), zk(:, 2), 'ro-', 'DisplayName', 'Medições');
    hold on;
    plot(estimated_positions(:, 1), estimated_positions(:, 2), 'bo-', 'DisplayName', 'Estimativas Kalman');
    legend;
    title('Posições medidas e corrigidas');
    xlabel('X');
    ylabel('Y');
    grid on;

    % Plot das posições x medidas e corrigidas
    subplot(3, 2, 2);
    plot(1:40, zk(:, 1), 'ro-', 'DisplayName', 'Medições X');
    hold on;
    plot(1:40, estimated_positions(:, 1), 'bo-', 'DisplayName', 'Estimativas X Kalman');
    legend;
    title('Posições X medidas e corrigidas');
    xlabel('Iteração');
    ylabel('Posição X');
    grid on;

    % Plot das posições y medidas e corrigidas
    subplot(3, 2, 3);
    plot(1:40, zk(:, 2), 'ro-', 'DisplayName', 'Medições Y');
    hold on;
    plot(1:40, estimated_positions(:, 2), 'bo-', 'DisplayName', 'Estimativas Y Kalman');
    legend;
    title('Posições Y medidas e corrigidas');
    xlabel('Iteração');
    ylabel('Posição Y');
    grid on;

    % Plot da velocidade em x
    subplot(3, 2, 4);
    plot(1:40, velocities(:, 1), 'b-', 'DisplayName', 'Velocidade em X');
    title('Velocidade em X');
    xlabel('Iteração');
    ylabel('Velocidade');
    grid on;

    % Plot da velocidade em y
    subplot(3, 2, 5);
    plot(1:40, velocities(:, 2), 'b-', 'DisplayName', 'Velocidade em Y');
    title('Velocidade em Y');
    xlabel('Iteração');
    ylabel('Velocidade');
    grid on;

    % Plot da aceleração em x
    subplot(3, 2, 6);
    plot(1:40, accelerations(:, 1), 'b-', 'DisplayName', 'Aceleração em X');
    title('Aceleração em X');
    xlabel('Iteração');
    ylabel('Aceleração');
    grid on;

    % Plot da aceleração em y
    figure;
    plot(1:40, accelerations(:, 2), 'b-', 'DisplayName', 'Aceleração em Y');
    title('Aceleração em Y');
    xlabel('Iteração');
    ylabel('Aceleração');
    grid on;
end


function [x, P] = kalmanFilter(x, P, Fk, Hk, Qk, Rk, zk)
    % Predition 
    x_pred = Fk * x;
    P_pred = Fk * P * Fk' + Qk;

    % Ganho de Kalman
    K = P_pred * Hk' / (Hk * P_pred * Hk' + Rk);

    % Estimation (Após medida - Kalman Gain definido Bão)
    x = x_pred + K * (zk - Hk * x_pred);
    P = (eye(size(P)) - K * Hk) * P_pred;
end
