KFilter:

Path: Common/Estimation

% 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.

--------------------------------------------------------------------------
     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

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

---

Back to the Common Module page