KFilter:

--------------------------------------------------------------------------
	  Mechanizes one step of a Kalman Filter with a forgetting factor

     x[k+1] = phi*x[k] + gamma*u[k]
     y[k]   = x[k] + k[k]*(z - h*x[k])

     The forgetting factor scales the covariance factor in the covariance
     update. If it were zero the covariance update would be the state
     covariance matrix q.
  
     p = s*phi*p*phi' + q;

     The function automatically forces symmetry on p by averaging p
     and p'.

     The function handles deterministic inputs as shown above. gamma
     is optional so it will also handle x[k+1] = phi*x[k] + u[k].

     Note that the matrices are for a discrete time system.

     Since version 1.
--------------------------------------------------------------------------
     Form:
     [x, p, k, y, pY] = KFilter( r, phi, q, h, x, z, p, gamma, u, s )
--------------------------------------------------------------------------

     ------
     Inputs
     ------
     r         (m,m)  Measurement Covariance
     phi       (n,n)  State transition matrix
     q         (n,n)  Process noise covariance
     h         (m,n)  Measurement matrix
     x         (n)    Previous value of the state
     z         (m)    Measurement vector
     p         (n,n)  Covariance of xold
     gamma     (n,l)  Input matrix
     u         (n)    Deterministic input
     s                Forgetting factor 1 = don't, � = 1 stage filter

     -------
	  Outputs
     -------

     x         (n)    Updated state this is at time k+1
     p         (n,n)  Covariance of x 
     k         (n,m)  Kalman Gain matrix
     y         (m,1)  Output at time k
     pY        (n,n)  Covariance after the measurement update

--------------------------------------------------------------------------