function Colored(flag, corr)

% Optimal State Estimation, by Dan Simon
%
% Kalman filter simulation with colored (time correlated) measurement noise.
% INPUTS:
%    flag = 0 means ignore the time correlation of the measurement noise
%           1 means augment the system state
%           2 means use the Bryson/Henrikson approach
%    corr = magnitude of measurement noise correlation <= 1

kf = 500; % Number of time steps in simulation
phi = [0.7 -0.15; 0.03 0.79];
H = [1 0; 0 1];
psi = [corr 0; 0 corr];
L = [0.15; 0.21];
Q = L * [1] * L';
Qv = [0.05 0; 0 0.05];

phi1 = [phi zeros(2,2); zeros(2,2) psi];
Q1 = [Q zeros(2,2); zeros(2,2) Qv];
R1 = zeros(2,2);
H1 = [H eye(2,2)];

if (flag == 0)
   n = 2; % number of states
   R = Qv;
   sTitle = 'Correlation Ignored';
elseif (flag == 1)
   n = 4; % number of states
   sTitle = 'Augmented State';
elseif (flag == 2)
   n = 2; % number of states
   R = H * Q * H' + Qv;
   M = Q * H';
   D = H * phi - psi * H;
   sTitle = 'Bryson and Henrikson';
else
   disp('illegal input argument');
end

% Assume xhat(0) = x(0) = 0.
x = zeros(4,1); xhatplus = zeros(n,1); xhatminus = zeros(n,1);
z = zeros(2,1);
% Assume P(0) = 0.
Pplus = zeros(n,n); Pminus = zeros(n,n);

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

randn('state', 0);
for k = 1 : kf
   % Simulate the system
   StateNoise = randn;
   n(1,1) = L(1,1) * StateNoise;
   n(2,1) = L(2,1) * StateNoise;
   n(3,1) = sqrt(Qv(1,1)) * randn;
   n(4,1) = sqrt(Qv(2,2)) * randn;
   x = phi1 * x + n;
   zold = z;
   z = H1 * x;
   % Run the Kalman filter
   if (flag == 0)
       % Ignore the time correlation
      Pminus = phi * Pplus * phi' + Q;
      K = inv(H * Pminus * H' + R);
      K = (Pminus * H') * K;
      xhatminus = phi * xhatplus;
      xhatplus = xhatminus + K * (z - H * xhatminus);
      Pplus = Pminus - K * H * Pminus;
   elseif (flag == 1)
       % Use the augmented state approach
      Pminus = phi1 * Pplus * phi1' + Q1;
      K = inv(H1 * Pminus * H1' + R1);
      K = (Pminus * H1') * K;
      xhatminus = phi1 * xhatplus;
      xhatplus = xhatminus + K * (z - H1 * xhatminus);
      Pplus = Pminus - K * H1 * Pminus;
   elseif (flag == 2)
       % Use the Bryson/Henrikson approach
      zeta = z - psi * zold;
      C = M * inv(D * Pminus * D' + R);
      K = Pminus * D' * inv(D * Pminus * D' + R);
      xhatplus = xhatminus + K * (zeta - D * xhatminus);
      %xhatminus = phi * xhatplus + C * (zeta - D * xhatplus); WRONG
      xhatminus = phi * xhatplus + C * (zeta - D * xhatminus); % Corrected January 10, 2009
      Pplus = (eye(2) - K * D) * Pminus * (eye(2) - K * D)' + K * R * K';
      Pminus = phi * Pplus * phi' + Q - C * M' - phi * K * M' - M * K' * phi';
      xhatplus = xhatminus;
   end
   % Save data for plotting.
   xArray = [xArray x];
   xhatArray = [xhatArray xhatplus];
   KArray = [KArray K];
   PArray = [PArray Pplus];
   zArray = [zArray z];
end

% Plot.
k = 1 : kf;
close all;
figure;
plot(k, zArray(1,:) - xArray(1,:), '-', k, xArray(1,:) - xhatArray(1,:), ':');
title([sTitle,' - x1 - Solid = Measurement Error, Dotted = Estimation Error']);
xlabel('time');

figure;
plot(k, zArray(2,:) - xArray(2,:), '-', k, xArray(2,:) - xhatArray(2,:), ':');
title([sTitle,' - x2 - Solid = Measurement Error, Dotted = Estimation Error']);
xlabel('time');

% Compute estimation error statistics.
err1 = xArray(1,:) - xhatArray(1,:);
err1 = sqrt(norm(err1)^2 / kf);
meas = xArray(1,:) - zArray(1,:);
meas = sqrt(norm(meas)^2 / kf);
disp(['x1 RMS Meas / Est Error Variance = ',num2str(meas),' / ',num2str(err1)]);
err2 = xArray(2,:) - xhatArray(2,:);
err2 = sqrt(norm(err2)^2 / kf);
meas = xArray(1,:) - zArray(1,:);
meas = sqrt(norm(meas)^2 / kf);
disp(['x2 RMS Meas / Est Error Variance = ',num2str(meas),' / ',num2str(err2)]);
disp(['RMS Est Error Variance Sum = ',num2str(err1+err2)]);