sc_ukf_2.h

//
//  sc_ukf_2.h
//  SCControl
//
//  Created by Eric Ham on 6/12/16.
//
//  Implements an Unscented Kalman Filter using separate predict and update functions
//  Uses the Unscented Transformation to simplify the code.
//  The sigma points are computed using a Cholesky transform.
//
//
 
#ifndef __SCControl__sc_ukf_2__
#define __SCControl__sc_ukf_2__
 
#include <stdio.h>
#ifdef AS_OS_WINDOWS
#include "matrixlib.h"
#else
#include <MatrixLib/MatrixLib.h>
#endif
 
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 (*propagate_func) (const ml_matrix &state, void *context);
typedef ml_matrix (*meas_2_func) (const ml_matrix &state,void *context);
 
 
class UKF2 {
    
    //instance variables
    private:
        ml_matrix r;
        ml_matrix v;
        ml_matrix m;
        ml_matrix p;
        ml_matrix q;
        ml_matrix w;
        ml_matrix wM;
        ml_matrix y;
        double alpha;
        double kappa;
        double beta;
        double nL;
        double c;
        double dT;
        void weight_matrix(void);
        bool has_propagate_function = false;
   
    // methods
    public:
        UKF2(void);
        ~UKF2();
        void initialize(void);
        void predict(void);
        void update(void);
        void set_state_covariance(const ml_matrix & q) { this->q = q; }
        void set_measurement_covariance(const ml_matrix & r) { this->r = r; }
        void set_mean(const ml_matrix & m) { this->m = m; }
        void set_measurement(const ml_matrix & y) { this->y = y; }
        void set_covariance(const ml_matrix & p){ this->p = p;}
        void set_filter_parameters(double alpha, double kappa, double beta){ this->alpha = alpha; this->kappa = kappa; this->beta = beta;}
        void set_integrator_time_step(double dT){this->dT = dT;};
        void set_state_function (state_func state_fun){state_fp = state_fun; has_propagate_function = false;};
        void set_integrator (integ_func integ_fun){integ_fp = integ_fun; has_propagate_function = false;};
        void set_measurement_function (meas_2_func meas_fun){meas_fp = meas_fun;};
        void set_propagate_function (propagate_func propagate_fun){propagate_fp = propagate_fun; has_propagate_function = true;};
        void set_context_rhs(void *context){ctx_rhs = context;};
        void set_context_meas(void *context){ctx_meas = context;};
        ml_matrix get_mean(void) { return m;}
        ml_matrix get_covariance(void) { return p;}
        ml_matrix get_residual(void) { return v;}
        state_func state_fp;
        integ_func integ_fp;
        meas_2_func meas_fp;
        propagate_func propagate_fp;
        void *ctx_rhs;
        void *ctx_meas;
   
};
 
 
#endif /* defined(__SCControl__sc_ukf_2__) */