------------------------------------------------------------------------
   A standard discrete Kalman Filter. 
   Automatically forces the covariance matrix to be symmetric. 
   This does not include the state update.
------------------------------------------------------------------------
   Form:
   [dx, p, k] = EKFSSG( phi, q, h, r, p, dz, s )
------------------------------------------------------------------------

   ------
   Inputs
   ------
   phi     (n,n)     The state transition matrix
   q       (n,n)     The state noise covariance matrix
   h       (m,n)     The measurement matrix = ?h/?x
   r       (m,m)     The measurement covariance matrix
   p       (n,n)     The state covariance matrix
   dz      (m)       The measurement residuals
   s       (1,1)     Forgetting factor >= 1

       (n is the # of states, m is the # of measurements)

   -------
   Outputs
   -------
   dx      (n)       The state vector update
   p       (n,n)     The state covariance matrix
   k       (n,m)     The Kalman gain matrix
   

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