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)));
}
}
