fsw_attitude_determination_multi_sensor.h

/*
 * Header file for object of class fsw_attitude_determination_multi_sensor.
 * 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_MULTI_SENSOR
#define FSW_ATTITUDE_DETERMINATION_MULTI_SENSOR
 
 
 
class fsw_attitude_determination_multi_sensor : public cd_control_module
{
    public:
        fsw_attitude_determination_multi_sensor (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 od_state_ref;
        CD_REQUEST_REF sun_sensor_pitch_ref;
        CD_REQUEST_REF sun_sensor_roll_ref;
        CD_REQUEST_REF magnetometer_ref;
        CD_REQUEST_REF sun_sensor_valid_ref;
        CD_REQUEST_REF ephem_sun_vector_ref;
        CD_REQUEST_REF q_eci_to_body_sim_ref;
        CD_REQUEST_REF b_field_meas_ref;
                CD_REQUEST_REF jd_ref;
        CD_REQUEST_REF meas_stars_ref;
        CD_REQUEST_REF u_star_catalog_ref;
    
        CD_CREATE_REF ad_q_body_to_camera_ref;
        CD_CREATE_REF ad_planet_data_ref;
        CD_CREATE_REF ad_q_eci_to_body_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_camera_ref;
    
                CD_CREATE_REF camera_data_command_ref;
        CD_CREATE_REF ad_q_body_to_magnetometer_ref;
        CD_CREATE_REF ad_q_body_to_sun_sensor_ref;
        CD_CREATE_REF ad_q_body_to_star_sensor_ref;
        CD_CREATE_REF ad_use_single_frame_command_ref;
                CD_CREATE_REF ad_reset_command_ref;
        CD_CREATE_REF ad_on_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_star_noise_covariance_command_ref;
        CD_CREATE_REF ad_meas_sun_noise_covariance_command_ref;
        CD_CREATE_REF ad_meas_magnetometer_noise_covariance_command_ref;
                CD_CREATE_REF ad_initial_state_command_ref;
        CD_CREATE_REF ad_use_star_data_command_ref;
        CD_CREATE_REF ad_use_magnetometer_data_command_ref;
        CD_CREATE_REF ad_use_sun_data_command_ref;
        CD_CREATE_REF ad_b_field_meas_noise_covariance_command_ref;
        CD_CREATE_REF ad_sun_meas_noise_covariance_command_ref;
        
    private:
        UKF ukf_camera;
   
        UKF2 ukf;
        
                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 meas_fun_ukf_2 (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_body;            
                        ml_matrix u_meas_cat;               
                        ml_matrix body_rate;                
                        ml_matrix body_rate_bias;           
            ml_matrix spacecraft_r_v;           
            ml_matrix q_body_to_magnetometer;   
            ml_matrix q_body_to_sun_sensor;     
            ml_matrix q_body_to_star_sensor;    
            ml_matrix sun_sensor_id;            
            ml_matrix u_sun_eci;                
                        double dT;                          
            int use_star_data;                  
            int use_sun_data;                   
            int use_magnetometer_data;          
            double julian_date;                 
            double row_origin;                  
            double col_origin;                  
                } EstimationData;
        
        star_sun_mag_meas_data est_meas_data;
 
};
 
extern "C"
{
    cd_control_module *fsw_attitude_determination_multi_sensor_builder(const char *name);
}
 
#endif