Nome: Plácido Capp Campos RA: 157024

Filtro de Kalman
Nesse trabalho irei reconhecer as equações do filtro de Kalman segundo o livro do
Bar Shalom e do código kamandemo.
No código iremos primeiro escolher o sistema e acrescentar os ruídos nele:
close all;
clear all;
clc

%% Inicialização do Sistema

% Planta do sistema
F = [ 1.1269 -0.494 0.1129; 1 0 0; 0 1 0 ];
G = [ -0.3832; 0.5919; 0.5191 ];
H = [0 0 1];
I = [1];

%Monta sistema
sys = ss(F,G,H,I,-1);

%% Inserindo ruídos

% Convariância do ruído do sistema

Q = 1;

% Convariância do ruído do sensor

R = 2;

% Generate a sinusoidal input vector (known)

t = [0:100]';
u = sin(t/5);

% Generate process noise and sensor noise vectors

randn('seed',0);
w = sqrt(Q)*randn(length(t),1);
v = sqrt(R)*randn(length(t),1);

% Now simulate the response using LSIM

% Generate the noisy plant response
y = lsim(sys,u+w); % w = process noise
yv = y + v; % v = meas. noise

Com a simulação do sistema e os dados adquiridos a cada instante podemos simular um

filtro de kalman em funcionamento:
1) %% Filtro de Kalman

2) % Now implement the filter recursions in a FOR loop:

3) P=G*Q*G'; % Initial error covariance
4) x=zeros(3,1); % Initial condition on the state
5) ye = zeros(length(t),1);
6) ycov = zeros(length(t),1);
 7) for i=1:length(t)
8) % Measurement update
9) Mn = P*H'/(H*P*H'+R);
10) x = x + Mn*(yv(i)-H*x); % x[n|n]
11) P = (eye(3)-Mn*H)*P;
12) ye(i) = H*x;
13) errcov(i) = H*P*H';
14) % Time update
15) x = F*x + G*u(i); % x[n+1|n]
16) P = F*P*F' + G*Q*G';
17) end

Podemos ver acima as equações do Bar Shalon:

• Na linha 9 temos a união das equações (5.2.3-9) e (5.2.3-11). Mn representa o
ganho W na equação:

E o S da equação acima é dado por:

• Na linha 10 temos as equações (5.2.3-12) e (5.2.3-13). De forma que nosso x futuro é

dado por:

E nosso erro v é dado pelo erro entre a estimativa de z futuro e o novo valor medido de
z:

• No cálculo da covariância da linha 11 temos a fórmula (5.2.3-14):

• Na linha 12 temos (5.2.3-7):

• Na linha 13 é feita a covariância da estimação dada por (5.2.3-9), o termo R é
desprezado nessa equação, creio que seja porque queremos a variância apenas do
erro do algoritmo e não aquele causado por eventos aleatórios, no caso R.

• Nas linhas 15 e 16 temos respectivamente as equações (5.2.3-2) e (5.2.3-5),

obtendo o próximo estado do sistema:

Finalmente teremos então os resultados do sistema plotados e podemos observar o

desempenho do sistema, essa parte foi feita exatamente como nos slides então irei
omitir para tornar o relatório mais enxuto.