sc_imm.h

Go to the documentation of this file.

/*
 * sc_imm.h 
 *
 * Copyright 2011, Princeton Satellite Systems. All rights reserved.
 *
 * Unscented Kalman Filter object.
 *
 *
 */
 
#ifdef AS_OS_WINDOWS
#include "matrixlib.h"
#else
#include <MatrixLib/MatrixLib.h>
#endif
 
#include <SCControl/sc_ukf.h>
#include <SCControl/sc_math.h>
 
#ifndef __SC_IMM__
#define __SC_IMM__
 
 
typedef ml_matrix (*jump_func) (void *context);
 
 
 
class IMM {
public:
        IMM();
        ~IMM();
        void intialize_filter_bank (const int size_bank);
        void set_filter_mode (const int n_filter, UKF_mode mode);
        void set_filter_parameters (const int n_filter, 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 int n_filter,const ml_matrix &Rp, const ml_matrix &Rm);
        void set_state_function (const int n_filter,state_func state_fun);
        void set_jump_function (jump_func jump_fun);
        void set_integrator (const int n_filter,integ_func integ_fun);
        void set_measurement_function (const int n_filter,meas_func meas_fun);
    void set_context(void *context);
        ml_matrix current_state (void) const { return state; }
        ml_matrix current_covariance (void) const { return covariance; }
    ml_matrix current_mode_probability (void) const {return mode_probability; }
        void update_estimate (const ml_matrix &measurement);
 
private:
        IMM (const IMM &u);
        IMM & operator= (const IMM &u);
 
        void mix_estimate(void);
        void combine_estimate(void);
        void update_estimate_bank(ml_matrix measurement);
 
        
        void combine_models (void);
 
        ml_matrix jump_probabilities;
        
        ml_matrix mode_probability;
        
        ml_matrix state;
        
        ml_matrix covariance;
 
        int bank_count;
        
        double current_dt;
        
        jump_func jump_fp;
    void *ctx;
        ml_matrix predicted_mode_probability;
        int max_bank_size;
        UKF ukf_bank[50];
    bool first_step;
    
};
 
#endif