function Reduced % Reduced order Kalman filter simulation. % Estimate the first state of a two-state system. F = [.9 .1; .2 .7]; % System matrix %F = [1.1 -.1; .2 .7]; % System matrix Lambda1 = 1; Lambda2 = 0; Q = 0.1; % Process noise covariance H1 = 0; H2 = 1; R = 1; % Measurement noise covariance Lambda = [Lambda1; Lambda2]; % Noise input matrix H = [H1 H2]; % Measurement matrix Ktol = 0.00001; % tolerance for convergence of gain to steady state NumSteps = 50; % number of simulation steps close all; % close all figures % Iteratively compute the steady state reduced filter Kalman gain P = 0; PIt = 0; PItOld = 0; PItt = 0; PIttOld = 0; Ptt = 0; Pt = 0; Sigma = 0; K = 0; x = [0; 0]; % Initial state vector xhat = [0; 0]; % Initial state estimate xhatReduced = 0; % Initial reduced order state estimate Pplus = [0 0; 0 0]; % Initial Kalman filter estimation error covariance I = eye(2); % Identity matrix ErrArray = [x - xhat]; % Array of Kalman filter estimation errors ErrReducedArray = [x(1) - xhatReduced]; % Array of reduced order filter estimation errors x1Array = [x(1)]; xhatReducedArray = [x(1)]; KArray = []; PttArray = []; PtArray = []; PArray = []; SigmaArray = []; PfullArray = []; randn('state', sum(100*clock)); % initialize the random number generator % Try to find a steady state reduced order gain. temp1 = H1 * F(1,2) + H2 * F(2,2); temp2 = H1 * Lambda1 + H2 * Lambda2; for k = 1 : 200 A = H1 * F(1,1) * P * F(1,1)' * H1'; A = A + H1 * F(1,1) * Sigma * temp1'; A = A + temp1 * Sigma' * F(1,1) * H1'; A = A - H1 * F(1,1) * PItt * temp1'; A = A - temp1 * PItt' * F(1,1) * H1'; A = A + H1 * F(1,1) * Pt * F(2,1) * H2'; A = A + H2 * F(2,1) * Pt * F(1,1) * H1'; A = A - H1 * F(1,1) * PIt * F(2,1) * H2'; A = A - H2 * F(2,1) * PIt * F(1,1) * H1'; A = A + temp1 * Ptt * temp1'; A = A + temp1 * Sigma' * F(2,1) * H2'; A = A + H2 * F(2,1) * Sigma * temp1'; A = A + H2 * F(2,1) * Pt * F(2,1) * H2'; A = A + R; A = A + temp2 * Q * temp2'; B = F(1,1) * P * F(1,1) * H1'; B = B + F(1,2) * Sigma' * F(1,1) * H1'; B = B + F(1,1) * Sigma * temp1'; B = B - F(1,2) * PItt' * F(1,1) * H1'; B = B - F(1,1) * PItt * temp1'; B = B + F(1,1) * Pt * F(2,1) * H2'; B = B - F(1,1) * PIt * F(2,1) * H2'; B = B + F(1,2) * Ptt * temp1'; B = B + F(1,2) * Sigma' * F(2,1) * H2'; B = B + Lambda1 * Q * temp2'; KOld = K; K = B * inv(A); KArray = [KArray K]; if (k > 3) & (abs((K - KOld) / K) < Ktol) break; end PIttOld = PItt; PItt = (1 - K * H1) * F(1,1) * (PIt * F(2,1) + PItt * F(2,2)); PItt = PItt + K * (H1 * F(1,1) + H2 * F(2,1)) * (Pt * F(2,1) + Sigma * F(2,2)); PItt = PItt + K * temp1 * (Sigma' * F(2,1) + Ptt * F(2,2)); PItt = PItt + K * temp2 * Q * Lambda2'; PItOld = PIt; PIt = (1 - K * H1) * F(1,1) * (PIt * F(1,1) + PIttOld * F(1,2)); PIt = PIt + K * (H1 * F(1,1) + H2 * F(2,1)) * (Pt * F(1,1) + Sigma * F(1,2)); PIt = PIt + K * temp1 * (Sigma' * F(1,1) + Ptt * F(1,2)); PIt = PIt + K * temp2 * Q * Lambda1'; temp3 = F(1,2) - K * H1 * F(1,2) - K * H2 * F(2,2); P = (1 - K * H1)^2 * F(1,1)^2 * P; P = P + 2 * (1 - K * H1) * F(1,1) * Sigma * temp3; P = P - 2 * (1 - K * H1) * F(1,1) * PIttOld * temp3; P = P - 2 * (1 - K * H1) * F(1,1) * Pt * F(2,1) * H2' * K'; P = P + 2 * (1 - K * H1) * F(1,1) * PItOld * F(2,1) * H2' * K'; P = P + temp3^2 * Ptt; P = P - 2 * temp3 * Sigma' * F(2,1) * H2' * K; P = P + K^2 * H2^2 * F(2,1)^2 * Pt; P = P + K^2 * R; P = P + (Lambda1 - K * H1 * Lambda1 - K * H2 * Lambda2)^2 * Q; PttOld = Ptt; PtOld = Pt; SigmaOld = Sigma; Ptt = F(2,1)^2 * Pt + 2 * F(2,1) * Sigma * F(2,2) + F(2,2)^2 * Ptt + Lambda2^2 * Q; Pt = F(1,1)^2 * Pt + 2 * F(1,1) * Sigma * F(1,2) + F(1,2)^2 * PttOld + Lambda1^2 * Q; Sigma = F(1,1) * PtOld * F(2,1) + F(1,1) * SigmaOld * F(2,2); Sigma = Sigma + F(1,2) * SigmaOld * F(2,1) + F(1,2) * PttOld * F(2,2); Sigma = Sigma + Lambda1 * Q * Lambda2; PttArray = [PttArray Ptt]; PtArray = [PtArray Pt]; PArray = [PArray P]; SigmaArray = [SigmaArray Sigma]; end figure; plot(KArray); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('Iteration Number'); ylabel('Reduced Order Gain'); if abs((K - KOld) / K) > Ktol disp('Reduced order Kalman gain did not converge to a steady state value'); return; end for k = 1 : NumSteps % System simultion x = F * x + Lambda * sqrt(Q) * randn; z = H * x + sqrt(R) * randn; % Full order Kalman filter simulation (time varying) Pminus = F * Pplus * F' + Lambda * Q * Lambda'; KStd = Pminus * H' * inv(H * Pminus * H' + R); xhat = F * xhat; xhat = xhat + KStd * (z - H * xhat); Pplus = (I - KStd * H) * Pminus * (I - KStd * H)' + KStd * R * KStd'; % Reduced order Kalman filter simulation (steady state) xhatReduced = F(1,1) * xhatReduced + K * (z - H1 * F(1,1) * xhatReduced); % Save data for plotting x1Array = [x1Array x(1)]; xhatReducedArray = [xhatReducedArray xhatReduced ]; ErrArray = [ErrArray x-xhat]; ErrReducedArray = [ErrReducedArray x(1)-xhatReduced]; PfullArray = [PfullArray Pplus(1,1)]; end % Compute estimation errors Err = sqrt(norm(ErrArray(1,:))^2 / size(ErrArray,2)); disp(['Full order estimation std dev (analytical and experimental) = ',num2str(sqrt(Pplus(1,1))),', ',num2str(Err)]); ErrReduced = sqrt(norm(ErrReducedArray(1,:))^2 / size(ErrReducedArray,2)); disp(['Reduced order estimation std dev (analytical and experimental) = ',num2str(sqrt(P)),', ',num2str(ErrReduced)]); % Plot results k = 1 : NumSteps; figure; plot(PttArray); title('Ptt', 'FontSize', 12); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('Time Step'); figure; plot(PtArray); title('Pt', 'FontSize', 12); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('Time Step'); figure; plot(k, PArray(1:max(k)), 'r:'); hold on; plot(k, PfullArray(1:max(k)), 'b'); title('P', 'FontSize', 12); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('Time Step'); ylabel('Estimation Error Variance'); legend('Reduced order variance', 'Full order variance'); figure; plot(SigmaArray); title('Sigma', 'FontSize', 12); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('Time Step'); figure; plot(k,ErrArray(1,1:max(k)),'r:'); hold on; plot(k,ErrReducedArray(1:max(k)), 'b'); set(gca,'FontSize',12); set(gcf,'Color','White'); xlabel('Time Step'); ylabel('Estimation Error'); legend('Std Kalman filter error', 'Reduced filter error');