/** \file * * Contains the ElevatorOffsetCalculator class implementation. * * Copyright (c) 2007,2008,2009 MBARI * MBARI Proprietary Information. All Rights Reserved */ #include #include #include "data/ConfigReader.h" #include "data/Slate.h" #include "data/UniversalDataReader.h" #include "data/DataWriter.h" #include "ElevatorOffsetCalculator.h" #include "ElevatorOffsetCalculatorIF.h" #include "controlModule/SpeedControlIF.h" #include "controlModule/VerticalControlIF.h" // TODO: 1. check syslog entry priority // 2. modify syslog entry contant /// Coeff value for exponential smoothing const float ElevatorOffsetCalculator::EXP_SMOOTHING_COEFF( -0.25 ); // determined based on exp model fit to historical data ElevatorOffsetCalculator::ElevatorOffsetCalculator( const Module* module ) : SyncDerivationComponent( ElevatorOffsetCalculatorIF::NAME, module ), targetErrorBound_( 0.0 ), initialized_( false ), depth_( nanf( "" ) ), pitch_( nanf( "" ) ), eleAngle_( nanf( "" ) ), cmdSpeed_( nanf( "" ) ), cmdPitch_( nanf( "" ) ), cmdMassPosition_( nanf( "" ) ), report_( false ), maxActiveEstimators_( 10 ), minEstimationTime_( 24.0 ), // minutes estimationTimeout_( 240.0 ) // minutes { // Universal inputs depthReader_ = newUniversalReader( UniversalURI::DEPTH ); elevatorAngleReader_ = newUniversalReader( UniversalURI::PLATFORM_ELEVATOR_ANGLE ); pitchReader_ = newUniversalReader( UniversalURI::PLATFORM_PITCH_ANGLE ); // Control Slate inputs speedCmdReader_ = newDataReader( SpeedControlIF::SPEED_CMD ); cmdPitchReader_ = newDataReader( VerticalControlIF::PITCH_CMD ); massPositionCmdReader_ = newDataReader( VerticalControlIF::MASS_POSITION_CMD ); // Config input targetErrorBoundCfgReader_ = newConfigReader( ElevatorOffsetCalculatorIF::ELEV_OFFSET_TARGET_ERROR_BOUND_CFG ); targetConfidenceLevelCfgReader_ = newConfigReader( ElevatorOffsetCalculatorIF::ELEV_OFFSET_TARGET_CONFIDENCE_CFG ); verbosityCfgReader_ = newConfigReader( ElevatorOffsetCalculatorIF::ELEV_OFFSET_VERBOSITY_CFG ); surfaceThresholdCfgReader_ = newConfigReader( VerticalControlIF::SURFACE_THRESHOLD_CFG ); // Slate output elevatorAngleAverageWriter_ = newDataWriter( ElevatorOffsetCalculatorIF::ELEVATOR_ANGLE_AVERAGE ); elevatorAngleVarianceWriter_ = newDataWriter( ElevatorOffsetCalculatorIF::ELEVATOR_ANGLE_VARIANCE ); elevatorAngleErrorBoundWriter_ = newDataWriter( ElevatorOffsetCalculatorIF::ELEVATOR_ANGLE_ERROR_BOUND ); elevatorAngleCmdSpeedIDWriter_ = newDataWriter( ElevatorOffsetCalculatorIF::ELEVATOR_ANGLE_CMD_SPEED_IDENTIFIER ); elevatorAngleCmdPitchIDWriter_ = newDataWriter( ElevatorOffsetCalculatorIF::ELEVATOR_ANGLE_CMD_PITCH_IDENTIFIER ); elevatorAngleCmdMassPositionIDWriter_ = newDataWriter( ElevatorOffsetCalculatorIF::ELEVATOR_ANGLE_CMD_MASS_POSITION_IDENTIFIER ); } ElevatorOffsetCalculator::~ElevatorOffsetCalculator() {} void ElevatorOffsetCalculator::initialize( void ) { logger_.syslog( "Initializing ElevatorOffsetCalculator.", Syslog::DEBUG ); report_ = false; initialized_ = readConfig(); } void ElevatorOffsetCalculator::uninitialize() { logger_.syslog( "Uninitializing ElevatorOffsetCalculator.", Syslog::DEBUG ); activeEstimators_.clear(); initialized_ = false; } void ElevatorOffsetCalculator::run() // TODO: restructure run as state machine { if( !initialized_ ) { initialize(); } depth_ = nanf( "" ); pitch_ = nanf( "" ); eleAngle_ = nanf( "" ); cmdPitch_ = nanf( "" ); cmdSpeed_ = nanf( "" ); cmdMassPosition_ = nanf( "" ); // Compute estimates of elev angle mean and variance for observed commanded speed, pitch and mass-position. if( readData() ) { if( ( cmdSpeed_ > 0 ) && ( depth_ > surfaceThreshold_ ) ) { // Get index of active estimator for observed commanded speed, pitch and mass-position int active_id = activateEstimator(); if( active_id >= 0 ) { // Estimate elevator angle running mean and variance double weight = computeSampleWeight(); estimateWeightedAverageAndVariance( activeEstimators_[active_id], eleAngle_, weight ); // Enable reporting after observing a valid sample report_ = true; } } else if( report_ ) { // Iterate over active estimators, if satisfied write/report then remove std::vector::iterator it; for( it = activeEstimators_.begin() ; it != activeEstimators_.end(); ) { if( isSatisfied( *it ) ) activeEstimators_.erase( it ); else ++it; } // We're on the surface or thruster is off (or both), disable reporting until next valid observation comes along report_ = false; } } return; } double ElevatorOffsetCalculator::computeSampleWeight() { return exp( EXP_SMOOTHING_COEFF * abs( pitch_ - cmdPitch_ ) ); } /// Weighted Welford algorithm for online mean and varinace (see Knuth TAOCP vol 2, 3rd edition, page 232) void ElevatorOffsetCalculator::estimateWeightedAverageAndVariance( elevatorAngleEstimator &elevAngEst, float &sample, double &weight ) { double delta, delta2; elevAngEst.sampleSize_++; elevAngEst.weightAccum_ += weight; delta = sample - elevAngEst.mean_; elevAngEst.mean_ += weight * ( delta / elevAngEst.weightAccum_ ); delta2 = sample - elevAngEst.mean_; elevAngEst.varAccum_ += weight * ( delta * delta2 ); elevAngEst.variance_ = ( elevAngEst.weightAccum_ > 0 ) ? elevAngEst.varAccum_ / elevAngEst.weightAccum_ : 0.0; } /// Computes the error bound for weighted avarge and variance estimates void ElevatorOffsetCalculator::estimateWeightedErrorBound( elevatorAngleEstimator &elevAngEst ) { // Weighted Chebyshev's inequality double epsilon = ( elevAngEst.weightAccum_ > 0 ) ? elevAngEst.variance_ / ( elevAngEst.weightAccum_ * ( 1 - targetConfidenceLevel_ / 100 ) ) : 0.0; elevAngEst.errorBound_ = ( epsilon > 0 ) ? sqrt( epsilon ) : nanf( "" ); } /// Find active estimator for observed commanded speed, pitch and mass-position int ElevatorOffsetCalculator::activateEstimator() { for( size_t i = 0; i < activeEstimators_.size(); i++ ) { if( ( abs( cmdSpeed_ - activeEstimators_[i].cmdSpeedID_ ) < 0.1 ) && // observed commanded speed and estimator speed are less than 0.1 m/s apart ( abs( cmdPitch_ - activeEstimators_[i].cmdPitchID_ ) < 1 ) && // observed commanded pitch and estimator pitch are less than 1 degree apart ( abs( cmdMassPosition_ - activeEstimators_[i].cmdMassPosID_ ) < 5 ) ) // observed cmd mass-position and estimator mass-position are less than 5 mm apart { return static_cast( i ); } } // None of the active etimators match the observed commanded variables, create a new estimator return addEstimator(); } int ElevatorOffsetCalculator::addEstimator() { if( activeEstimators_.size() <= maxActiveEstimators_ ) { // Add new estimator for commanded speed, pitch and mass-position activeEstimators_.push_back( elevatorAngleEstimator( cmdSpeed_, cmdPitch_, cmdMassPosition_ ) ); if( verbosity_ > 0 ) logger_.syslog( "New estimator for commanded vars: speed " + Str( cmdSpeed_, 2 ) + " m/s, pitch " + Str( cmdPitch_, 2 ) + " deg, mass-position " + Str( cmdMassPosition_, 2 ) + " mm (" + Str( activeEstimators_.size() ) + " active estimators).", Syslog::INFO ); return ( static_cast( activeEstimators_.size() ) - 1 ); } else { if( verbosity_ > 1 ) logger_.syslog( "Number of estimators exceeded allowed limit. Did NOT create estimator for commanded vars: speed " + Str( cmdSpeed_, 2 ) + " m/s, pitch " + Str( cmdPitch_, 2 ) + " deg, mass-position " + Str( cmdMassPosition_, 2 ) + " mm (" + Str( activeEstimators_.size() ) + " active estimators).", Syslog::INFO ); } return -1; } bool ElevatorOffsetCalculator::isSatisfied( elevatorAngleEstimator &elevAngEst ) { float runTime = elevAngEst.startTime_.elapsed().asFloat() / 60; // Report estimated variables to syslog and write to slate if( runTime >= minEstimationTime_ ) { estimateWeightedErrorBound( elevAngEst ); if( elevAngEst.errorBound_ <= targetErrorBound_ ) { // Announce estimator results to syslog if( verbosity_ > 0 ) logger_.syslog( "Completed estimation for commanded vars: speed " + Str( elevAngEst.cmdSpeedID_, 2 ) + " m/s, pitch " + Str( elevAngEst.cmdPitchID_, 2 ) + " deg, mass-position " + Str( elevAngEst.cmdMassPosID_, 2 ) + " mm. Average elevator angle=" + Str( elevAngEst.mean_ ) + " +/- " + Str( elevAngEst.errorBound_ ) + " deg (conf. level " + Str( targetConfidenceLevel_, 2 ) + "%, sigma: " + Str( sqrt( elevAngEst.variance_ ) ) + " deg).", Syslog::IMPORTANT ); // Write estimator values to slate writeData( elevAngEst ); // We're done with this elevator angle estimator, mark it for removal return true; } else if( runTime >= estimationTimeout_ ) { // This elevator angle estimator has timed out, mark it for removal if( verbosity_ > 0 ) logger_.syslog( "Removing expired estimator for commanded vars: speed " + Str( elevAngEst.cmdSpeedID_, 2 ) + " m/s, pitch " + Str( elevAngEst.cmdPitchID_, 2 ) + " deg, mass-position " + Str( elevAngEst.cmdMassPosID_, 2 ) + " mm.", Syslog::INFO ); return true; } } return false; } bool ElevatorOffsetCalculator::readConfig() { // read and check configuration data bool ok( true ); ok &= ( targetErrorBoundCfgReader_->read( Units::DEGREE, targetErrorBound_ ) && !isnan( targetErrorBound_ ) ); ok &= ( targetConfidenceLevelCfgReader_->read( Units::PERCENT, targetConfidenceLevel_ ) && !isnan( targetConfidenceLevel_ ) ); ok &= ( surfaceThresholdCfgReader_->read( Units::METER, surfaceThreshold_ ) && !isnan( surfaceThreshold_ ) ); ok &= verbosityCfgReader_->read( Units::COUNT, verbosity_ ); return ok; } bool ElevatorOffsetCalculator::readData() { // read and check slate data bool ok( true ); ok &= readConfig(); // Universal Slate inputs ok &= ( depthReader_->read( Units::METER, depth_ ) && !isnan( depth_ ) ); ok &= ( elevatorAngleReader_->read( Units::DEGREE, eleAngle_ ) && !isnan( eleAngle_ ) ); ok &= ( pitchReader_->read( Units::DEGREE, pitch_ ) && !isnan( pitch_ ) ); // Control Slate inputs ok &= ( speedCmdReader_->read( Units::METER_PER_SECOND, cmdSpeed_ ) && !isnan( cmdSpeed_ ) ); ok &= ( cmdPitchReader_->read( Units::DEGREE, cmdPitch_ ) && !isnan( cmdPitch_ ) ); ok &= ( massPositionCmdReader_ ->read( Units::MILLIMETER, cmdMassPosition_ ) && !isnan( cmdMassPosition_ ) ); return ok; } void ElevatorOffsetCalculator::writeData( elevatorAngleEstimator &elevAngEst ) { // Grab data timestamp Timestamp dataTimestamp = Timestamp::Now(); // Write results to slate // TODO: dynamically write to appropriate identifier elevatorAngleAverageWriter_->write( Units::RADIAN, elevAngEst.mean_ * ( M_PI / 180 ), dataTimestamp ); elevatorAngleVarianceWriter_->write( Units::RADIAN, elevAngEst.variance_ * ( M_PI / 180 ), dataTimestamp ); elevatorAngleErrorBoundWriter_->write( Units::RADIAN, elevAngEst.errorBound_ * ( M_PI / 180 ), dataTimestamp ); elevatorAngleCmdSpeedIDWriter_->write( Units::METER_PER_SECOND, elevAngEst.cmdSpeedID_, dataTimestamp ); elevatorAngleCmdPitchIDWriter_->write( Units::RADIAN, elevAngEst.cmdPitchID_ * ( M_PI / 180 ), dataTimestamp ); elevatorAngleCmdMassPositionIDWriter_->write( Units::METER, elevAngEst.cmdMassPosID_ / 1000, dataTimestamp ); } ElevatorOffsetCalculator::elevatorAngleEstimator::elevatorAngleEstimator( const float cmdSpeedID, const float cmdPitchID, const float cmdMassPosID ): cmdSpeedID_( cmdSpeedID ), cmdPitchID_( cmdPitchID ), cmdMassPosID_( cmdMassPosID ), startTime_( Timestamp::Now() ), sampleSize_( 0 ), weightAccum_( 0.0 ), mean_( 0.0 ), varAccum_( 0.0 ), variance_( 0.0 ), errorBound_( nanf( "" ) ) {} ElevatorOffsetCalculator::elevatorAngleEstimator::~elevatorAngleEstimator() {}