KALMAN.m

function [xhatOut, yhatOut] = KALMAN(u,meas)
    % This Embedded MATLAB Function implements a very simple Kalman filter.
    %
    % It implements a Kalman filter for estimating both the state and output
    % of a linear, discrete-time, time-invariant, system given by the following
    % state-space equations:
    %
    % x(k) = 0.914 x(k-1) + 0.25 u(k) + w(k)
    % y(k) = 0.344 x(k-1) + v(k)
    %
    % where w(k) has a variance of 0.01 and v(k) has a variance of 0.1.

    % Author: Phil Goddard (phil@goddardconsulting.ca)
    % Date: Q2, 2011.

    % Define storage for the variables that need to persist
    % between time periods.
    persistent P xhat A B C Q R
    if isempty(P)
    % First time through the code so do some initialization
       xhat = 0;
       P = 0;
       A = 0.914;
       B = 0.25;
       C = 0.344;
       Q = 0.01^2;
       R = 0.1^2;
    end
    % Propagate the state estimate and covariance matrix:
    xhat = A*xhat + B*u;
    P = A*P*A' + Q;
    % Calculate the Kalman gain
    K = P*C'/(C*P*C' + R);
    % Calculate the measurement residual
    resid = meas - C*xhat;
    % Update the state and error covariance estimate
    xhat = xhat + K*resid;
    P = (eye(size(K,1))-K*C)*P;
    % Post the results
    xhatOut = xhat;
    yhatOut = C*xhatOut;

end