correlated.m

《卡曼滤波基础-------实用方法》

function Correlated(M, MFilter)

% Kalman filter simulation using correlated process and measurement noise.
% This illustrates the improvement in filter results that can be attained
% when the correlation is taken into account.
% M = correlation between process noise and measurement noise.
% MFilter = value of M used in Kalman filter.

kf = 50; % number of time steps in simulation
phi = 0.8; % system matrix
H = 1; % measurement matrix
Q = 1; % process noise covariance
R = 0.1; % measurement noise covariance

% Compute the eigendata of the covariance matrix so that we can simulate correlated noise.
Q1 = [Q M; M' R];
[d, lambda] = eig(Q1);
if (lambda(1) < 0) || (lambda(2) < 0)
    disp('Q1 is not positive semidefinite');
    return;
end
ddT = d * d';
if (norm(eye(size(ddT)) - ddT) > eps)
   disp(['d is not orthogonal. d = ',d]);
   return;
end
   
x = 0; % initial state
xhatplus = x; % initial state estimate
Pplus = 0; % initial uncertainty in state estimate

xArray = [];
xhatArray = [];
KArray = [];
PArray = [];
zArray = [];

randn('state', 0); % initialize random number generator

for k = 1 : kf
   % Generate correlated process noise (w) and measurement noise (n)
   v = [sqrt(lambda(1,1))*randn; sqrt(lambda(2,2))*randn];
   Dv = d * v;
   w = Dv(1);
   n = Dv(2);
   % Simulate the system dynamics and the measurement
   x = phi * x + w;
   z = H * x + n;
   % Simulate the Kalman filter
   Pminus = phi * Pplus * phi' + Q;
   K = inv(H * Pminus * H' + H * MFilter + MFilter' * H' + R);
   K = (Pminus * H' + MFilter) * K;
   xhatminus = phi * xhatplus;
   xhatplus = xhatminus + K * (z - H * xhatminus);
   Pplus = Pminus - K * (H * Pminus + MFilter');
   % Save data for plotting
   xArray = [xArray x];
   xhatArray = [xhatArray xhatplus];
   KArray = [KArray K];
   PArray = [PArray Pplus];
   zArray = [zArray z];
end

% Plot the data
k = 1 : kf;
close all;

figure;
plot(k, zArray - xArray, 'r-', k, xhatArray - xArray, 'b:');
set(gca,'FontSize',12); set(gcf,'Color','White');
title(['M = ',num2str(M),', MFilter = ',num2str(MFilter)]);
xlabel('Time');
legend('Measurement Error', 'Estimation Error');

figure;
plot(k, KArray);
set(gca,'FontSize',12); set(gcf,'Color','White');
title(['Kalman Gain for M = ',num2str(M),', MFilter = ',num2str(MFilter)]);
xlabel('Time');

figure;
plot(k, PArray);
set(gca,'FontSize',12); set(gcf,'Color','White');
title(['Estimation Error Covariance for M = ',num2str(M),', MFilter = ',num2str(MFilter)]);
xlabel('Time');

% Compute statistics
err = zArray - xArray;
err = norm(err)^2 / kf;
disp(['Measurement Error Variance = ',num2str(err)]);
err = xhatArray - xArray;
err = norm(err)^2 / kf;
disp(['Estimation Error Variance = ',num2str(err)]);
disp(['Analytical Variance = ',num2str(Pplus)]);