|
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
|
