UKFP:

Path: Common/Estimation

% 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 parameters. 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          (.)   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          (.)   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:

Math: Integration/RK4
Math: Linear/DupVect

Back to the Common Module page