UNIVERSITA' DEGLI STUDI DI PAVIA - Robotica

%
ni = nargin;
no = nargout;
if ni ~= 8
error('Wrong number of parameters')
end
if no ~= 3
error('Wrong number of return values')
end
error(abcdchk(A,B));
if ~length(A) | ~length(B)
error('A and B matrices cannot be empty.')
end
Pk = P0;
for i = 1:nin,
Pk1 = A * Pk * A' + B * Q * B' - A * Pk * C' *
inv(C * Pk * C' + R) * C * Pk * A';
Pk = Pk1;
if (norm(Pk1 - Pk, 2) / norm(Pk, 2)) < eps
break
end
end
nout = i;
P = Pk;
out_eps = eps;
Il programma ha dunque la forma di una funzione, riccati(), che risolve
iterativamente l'equazione omonima.
Questa funzione riceve in input le matrici che descrivono il sistema
dinamico, cioè A, B e C, il valore di partenza per l'iterazione P0, le matrici che
descrivono il rumore sui dati, ovvero Q ed R, la precisione desiderata per
l'iterazione (eps) in termini di residuo della soluzione ed infine il numero massimo
di iterate, specificato da nin. In uscita si ha, invece, il risultato
dell'approssimazione (P), la precisione raggiunta dall'iterazione ed il numero di
cicli effettivamente compiuti, rispettivamente out_eps e nout.
205