UNIVERSITA' DEGLI STUDI DI PAVIA - Robotica

C.2 Simulazione del predittore di Kalman
La simulazione che viene effettuata dal seguente programma è stata
sviluppata per testare il funzionamento del predittore di Kalman.
function [zk, xk, zk_pred, xk_pred, An] = tk(nA, rho, nstep)
% tk.m
% Simulazione del filtro di Kalman
%
% [zk, xk, zk_pred, xk_pred, An] = tk(nA, rho, nstep)
%
error(nargchk(3, 3, nargin))
close all
% Varianza del rumore
max_noise = 0;
% Passo di campionamento
dt = 0.04;
% Generazione dei valori di test
k = [0:dt:2*pi];
nk = max(size(k));
noise = 2 * (rand(size(k)) - 0.5) * max_noise;
x0 = 0;
v0 = 170;
a0 = 10;
zk_real = round(100 * sin(1 * k));
zk = round(zk_real) + round(noise);
% Matrici caratteristiche del sistema in esame
A = diag(ones(1, nA), 0) + diag(ones(1, nA - 1),1);
B = zeros(1, nA);
B(nA) = 1;
B = B';
C = zeros(1, nA);
C(1) = 1;
% Imposta il parametro che controlla il predittore
Q = 1;
R = rho;
% Calcola la soluzione dell'equazione di Riccati con metodo
% iterativo
P0 = eye(size(A));
[P, eps, nout] = riccati(P0, A, B, C, Q, R, -inf, 100);
norm(-P + A * P * A' - A * P * C' * inv(C * P * C' + R) * C * P *
206