sc_ukf.h

Go to the documentation of this file.

/*
 * sc_ukf.h 
 *
 * Copyright 2007, Princeton Satellite Systems. All rights reserved.
 *
 * Unscented Kalman Filter object.
 *
 * Based on UKF.m and UKFP.m in Toolboxes/Aerospace/Estimation.
 *
 */
 
#ifdef AS_OS_WINDOWS
#include "matrixlib.h"
#else
#include <MatrixLib/MatrixLib.h>
#endif
 
/*
        Note:  I'm not crazy about the way parameter mode operates; it currently mimics the
 Matlab version in the toolbox.  But the way that there are several versions
 of the parameters, and how these need to be carefully maintained across
 calls to the RHS function, is an area of concern; it's not nearly as clean
 as I'd like it.
 */
 
#ifndef __SC_UKF__
#define __SC_UKF__
 
typedef ml_matrix (*state_func) (ml_matrix x, double t,void *context);
typedef ml_matrix (*integ_func) (state_func state_fun, ml_matrix &x, double h, double t,void *context);
typedef ml_matrix (*meas_func) (const ml_matrix &state,void *context);
 
typedef enum {
        UKF_mode_unset = -1,
        UKF_mode_state,
        UKF_mode_parameter
} UKF_mode;
 
class UKF {
public:
        UKF();
        ~UKF();
 
        void set_filter_mode (UKF_mode mode);
        void set_filter_parameters (const double alpha, const double kappa, const double beta);
        void set_integrator_time_info (double t, double dt);
        void set_initial_values (const ml_matrix &state0, const ml_matrix &covariance0, const ml_matrix &parameter0 = ml_matrix());
        void set_noise_covariances (const ml_matrix &Rp, const ml_matrix &Rm);
        void set_state_function (state_func state_fun);
        void set_integrator (integ_func integ_fun);
        void set_measurement_function (meas_func meas_fun);
    void set_context(void *context);
    void set_state( const ml_matrix &state0 );
        ml_matrix current_state (void) const { return *state; }
        ml_matrix current_residual (void) const { return *residual; }
        ml_matrix current_parameter (void) const { return *parameter; }
        ml_matrix current_covariance (void) const { return *covariance; }
        ml_matrix current_residual_covariance (void) const {return * residual_covariance; }
        ml_matrix current_rhs_params (void) const { return rhs_params; }
        void update_estimate (const ml_matrix &measurement);
 
private:
        UKF (const UKF &u);
        UKF & operator= (const UKF &u);
 
        ml_matrix calculate_sigma_points (const ml_matrix &x, const ml_matrix &P);
 
        void update_estimate_state (const ml_matrix &measurement);
 
        void update_estimate_parameter (const ml_matrix &measurement);
 
        UKF_mode mode;
 
        unsigned int n;
        ml_matrix *state;
        ml_matrix *parameter;
        ml_matrix *covariance;
 
        ml_matrix *process_noise_cov;
        ml_matrix *meas_noise_cov;
 
        state_func state_fp;
        integ_func integ_fp;
        meas_func meas_fp;
    void *ctx;
        
        ml_matrix rhs_params;
 
        double alpha;
        double kappa;
        double beta;
        double t;
        double dt;
 
        // internal
 
        double lambda;
        ml_matrix *Wm;
        ml_matrix *Wc;
        ml_matrix *residual;
        ml_matrix *residual_covariance;
};
 
#endif