UNIVERSITA' DEGLI STUDI DI PAVIA - Robotica

A' + B * Q * B')
% Calcola il guadagno del filtro di Kalman
K = A * P * C' * inv(C * P * C' + R)
abs(eig(A - K * C))
xk = zeros(nA, nk);
xk1 = zeros(nA, nk);
xk_pred = zeros(nA, nk);
pow = zeros(1, nstep + 1);
pow(1) = 1;
An = polyvalm(pow, A)
for i = 1:(nk - nstep)
xk(:,i + 1) = A * xk(:,i) + K * (zk(i) - C * xk(:,i));
xk_pred(:,i + nstep) = An * xk(:,i + 1);
zk_pred(i + nstep) = C * xk_pred(:,i + nstep);
end
% Stampa il grafico
axis xy
grid on
plot(k, zk_real, 'g.', k, zk, 'b.', k, xk_pred(1,:), 'r.');
legend('Trajectory', 'Noisy measurement', 'Prediction', 0);
title('Functions');
xlabel('k - Discrete time');
Il programma utilizza il predittore per stimare lo stato di un sistema
dinamico che evolve nel tempo secondo una legge nota, che è codificata
all'interno del programma stesso: è quindi molto semplice variare la funzione che
determina l'evoluzione del sistema, in modo da testare il filtro nelle più diverse
condizioni operative. Nel listato precedente, e negli esempi che seguono, è
utilizzata una funzione sinusoidale. E' anche possibile aggiungere del rumore al
segnale, in modo da verificare il comportamento del filtro aumentando
progressivamente i disturbi sugli ingressi.
La simulazione è codificata in MATLAB come una funzione che accetta i
seguenti parametri:
- nA, il numero di integratori del modello;
- rho, ovvero il parametro che controlla la sensibilità al rumore del filtro;
207