MATLAB CONTROL SYSTEM TOOLBOX 9 Podręcznik Użytkownika Pobierz pdf (Strona 367)

Kalman Filtering

you can implement the time-varying filter with the following for loop.

P = B*Q*B'; % Initial error covariance

x = zeros(3,1); % Initial condition on the state

ye = zeros(length(t),1);

ycov = zeros(length(t),1);

for i=1:length(t)

% Measurement update

Mn = P*C'/(C*P*C'+R);

x = x + Mn*(yv(i)–C*x); % x[n|n]

P = (eye(3)–Mn*C)*P; % P[n|n]

ye(i) = C*x;

errcov(i) = C*P*C';

% Time update

x = A*x + B*u(i); % x[n+1|n]

P = A*P*A' + B*Q*B'; % P[n+1|n]

MATLAB CONTROL SYSTEM TOOLBOX 9 Podręcznik Użytkownika Strona 367