fsw_attitude_determination.h

/*
 * Header file for object of class fsw_attitude_determination.
 * Copyright 2010, Princeton Satellite Systems, Inc.
 * All Rights Reserved.
 */
 
#include <ControlDeck2/ControlDeck2.h>
#import <SCControl/SCControl.h>
#include "sc_events.h"
#include "sc_data_names.h"
 
#ifndef FSW_ATTITUDE_DETERMINATION
#define FSW_ATTITUDE_DETERMINATION
 
 
 
class fsw_attitude_determination : public cd_control_module
{
    public:
        fsw_attitude_determination (const char * mod_name);
                void initialize_data();
        void initialize_data_requests();
        void initialize();
        void handle_event(int code);
 
    protected:
        
                CD_REQUEST_REF star_id_camera_ref;
                CD_REQUEST_REF star_data_ref;
                CD_REQUEST_REF unit_vector_ref;
                CD_REQUEST_REF u_star_camera_ref;
                CD_REQUEST_REF camera_rate_meas_ref;
        CD_REQUEST_REF jd_ref;
    
        CD_CREATE_REF ad_gyro_bias_camera_ref;
        CD_CREATE_REF ad_covariance_camera_ref;
        CD_CREATE_REF ad_q_eci_to_camera_single_frame_ref;
        CD_CREATE_REF ad_q_eci_to_body_ref;
        CD_CREATE_REF ad_q_eci_to_camera_ref;
 
                CD_CREATE_REF camera_data_command_ref;
        CD_CREATE_REF ad_use_single_frame_command_ref;
                CD_CREATE_REF ad_reset_command_ref;
                CD_CREATE_REF ad_parameter_command_ref;
                CD_CREATE_REF ad_initial_covariance_command_ref;
                CD_CREATE_REF ad_plant_noise_covariance_command_ref;
                CD_CREATE_REF ad_meas_noise_covariance_command_ref;
                CD_CREATE_REF ad_initial_state_command_ref;
        CD_CREATE_REF ad_q_camera_to_body_command_ref;
 
private:
        UKF ukf_camera;
        
                ml_matrix       u_meas_catalog(ml_matrix id, ml_matrix u_catalog);
 
                ml_matrix q_eci_to_body;
                ml_matrix gyro_bias_camera;
                double julian_date_old;
        
                static ml_matrix meas_fun_ukf (const ml_matrix &x, void *context);
                static ml_matrix att_prop( ml_matrix x, double t, void *context);
        
                void reset_ukf();
        
                ml_matrix q_eci_to_camera;
                ml_matrix q_eci_to_camera_single_frame;
    
        int reset_old;
        
        
                typedef struct estimation_struct
                {
                        double f;                                               
                        ml_matrix q_eci_to_camera;              
                        ml_matrix u_meas_cat;                   
                        ml_matrix body_rate;                    
                        ml_matrix body_rate_bias;               
                        double dT;                                              
                } EstimationData;
        
 
};
 
extern "C"
{
    cd_control_module *fsw_attitude_determination_builder(const char *name);
}
 
#endif