UNIVERSITA' DEGLI STUDI DI PAVIA - Robotica

% Usato per eliminare il doppione alla frontiera dei due insiemi
low_part = logspace(-10, 2, 39);
rho = [low_part(1, 1:38) logspace(2, 12, 82)];
nrho = max(size(rho));
outfile = fopen(fname, 'w');
fprintf(outfile, '%d\n', nA);
fprintf(outfile, '%d\n', nrho);
tic
for i = 1:nrho,
% Imposta il parametro che controlla il predittore
Q = 1;
R = rho(i);
% 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 * A' + B * Q * B');
% Calcola il guadagno del filtro di Kalman
K = A * P * C' * inv(C * P * C' + R);
fprintf(outfile, '%f ', rho(i));
fprintf(outfile, '%f ', K);
fprintf(outfile, '\n');
end
fprintf(outfile, '\n');
toc
fclose(outfile);
213