UKFP:

-------------------------------------------------------------------------------
   Implement an Unscented Kalman Filter for parameter estimation.
   The filter uses numerical integration to propagate the state.
   The filter propagates sigma points, points computed from the
   state plus a function of the covariance matrix. For each parameter
   there are two sigma parameter. The current estimated state must be
   input each step.

   The filter appends internal data to the datastructure

   To initialize call

   d = UKFP( 'initialize', d );

   To update

   d = UKFP( 'update', d, y );

-------------------------------------------------------------------------------
   Form:
   d = UKFP( action, d, y )
-------------------------------------------------------------------------------

   ------
   Inputs
   ------
   action    (1,:)  'initialize' or 'update'
   d         (1,1)  UKFP data structure
                    .rHSFun      (1,:) Name of RHS function
                    .rHSFunData  (1,1) RHS function data structure
                    .measFun     (1,:) Name of measurement function
                    .measFunData (1,1) Measurement function data 
                    .x           (:,1) State vector
                    .p           (n,n) Covariance matrix for w   
                    .rP          (n,n) Plant noise covariance 
                    .rY          (m,m) Measurement noise covariance
                    .alpha       (1,1) Scaling parameter (1e-4 to 1)
                    .beta        (1,1) Scaling parameter (usually 2)
                    .kappa       (1,1) Scaling parameter (usually 0) 
                    .w           (n,1) Parameters      
   y         (m,1)  Measurement vector

   -------
   Outputs
   -------
   d         (1,1)  UKFP data structure with the following appended
                    .n           (1,1)   Twice the number of states + 1
                    .y           (m,1)   Measurements based on the states
                    .wA          {n}     Sigma points
                    .wC          (1,n+1) Weights
                    .wM          (1,n+1) Weights
                    .gamma       (1,1)   Sigma points scaling parameter
                    .lambda      (1,1)   Sigma points scaling parameter
                    .pDD         (m,m)   Measurement covariance
                    .pWD         (n,m)   Parameter, measurement cross
                                         covariance
                    .D           (n,2*n) Measurements at sigma points
                    .dY          (n,1)   Residuals
                    .L           (1,1)   Number of parameters

-------------------------------------------------------------------------------
   References: Van der Merwe, R. and Wan, E., "Sigma-Point Kalman Filters for
               Probabilistic Inference in Dynamic State-Space Models".
               Matthew C. VanDyke, Jana L. Schwartz, Christopher D. Hall,
               "UNSCENTED KALMAN FILTERING FOR SPACECRAFT ATTITUDE STATE AND
               PARAMETER ESTIMATION,"AAS-04-115.
-------------------------------------------------------------------------------

Children:

Common: Math/DupVect
Common: Math/RK4