ModularStateEstimator::ModularStateEstimator(core::ConfigNode config, core::EventHubPtr eventHub, vehicle::IVehiclePtr vehicle) : StateEstimatorBase(config,eventHub), dvlEstimationModule(EstimationModulePtr()), imuEstimationModule(EstimationModulePtr()), depthEstimationModule(EstimationModulePtr()), m_vehicle(vehicle::IVehiclePtr(vehicle)) { // Connect the event listeners to their respective events if(eventHub != core::EventHubPtr()){ updateConnection_IMU = eventHub->subscribeToType( vehicle::device::IIMU::RAW_UPDATE, boost::bind(&ModularStateEstimator::rawUpdate_IMU,this, _1)); updateConnection_DepthSensor = eventHub->subscribeToType( vehicle::device::IDepthSensor::RAW_UPDATE, boost::bind(&ModularStateEstimator::rawUpdate_DepthSensor,this, _1)); updateConnection_DVL = eventHub->subscribeToType( vehicle::device::IVelocitySensor::RAW_UPDATE, boost::bind(&ModularStateEstimator::rawUpdate_DVL,this, _1)); initConnection_IMU = eventHub->subscribeToType( vehicle::device::IIMU::INIT, boost::bind(&ModularStateEstimator::init_IMU,this,_1)); initConnection_DVL = eventHub->subscribeToType( vehicle::device::IVelocitySensor::INIT, boost::bind(&ModularStateEstimator::init_DVL,this,_1)); initConnection_DepthSensor = eventHub->subscribeToType( vehicle::device::IDepthSensor::INIT, boost::bind(&ModularStateEstimator::init_DepthSensor,this,_1)); } // Construct the estimation modules dvlEstimationModule = EstimationModulePtr( new BasicDVLEstimationModule(config["DVLEstimationModule"], eventHub)); imuEstimationModule = EstimationModulePtr( new BasicIMUEstimationModule(config["IMUEstimationModule"], eventHub)); depthEstimationModule = EstimationModulePtr( new BasicDepthEstimationModule(config["DepthEstimationModule"], eventHub)); }
void ModularStateEstimator::init(core::ConfigNode config, core::EventHubPtr eventHub) { //Removed 7/13/2013 to test Combined DVL Acclerometer Kalman filter //modules.push_back(EstimationModulePtr( // new BasicDVLEstimationModule( // config["DVLEstimationModule"], // eventHub, // m_estimatedState))); modules.push_back(EstimationModulePtr( new DVLAccelerometerEstimator( eventHub, m_estimatedState))); modules.push_back(EstimationModulePtr( new BasicIMUEstimationModule( config["IMUEstimationModule"], eventHub, m_estimatedState))); modules.push_back(EstimationModulePtr( new DepthKalmanModule( config["DepthEstimationModule"], eventHub, m_estimatedState))); if(config.exists("GreenBuoy")) { addObstacle(config["GreenBuoy"], Obstacle::GREEN_BUOY); modules.push_back( EstimationModulePtr(new SimpleBuoyEstimationModule( config["GreenBuoyEstimationModule"], eventHub, m_estimatedState, Obstacle::GREEN_BUOY, vision::EventType::BUOY_FOUND))); } if(config.exists("RedBuoy")) { addObstacle(config["RedBuoy"], Obstacle::RED_BUOY); modules.push_back( EstimationModulePtr(new SimpleBuoyEstimationModule( config["RedBuoyEstimationModule"], eventHub, m_estimatedState, Obstacle::RED_BUOY, vision::EventType::BUOY_FOUND))); } if(config.exists("YellowBuoy")) { addObstacle(config["YellowBuoy"], Obstacle::YELLOW_BUOY); modules.push_back( EstimationModulePtr(new SimpleBuoyEstimationModule( config["YellowBuoyEstimationModule"], eventHub, m_estimatedState, Obstacle::YELLOW_BUOY, vision::EventType::BUOY_FOUND))); } }