generalIntegrator.hpp

Go to the documentation of this file.

/** \file generalIntegrator.hpp
 * \author Jared R. Males (jaredmales@gmail.com)
 * \brief Declares and defines a general integrator controller class for AO control.
 * \ingroup mxAO_sim_files
 *
 */
 
#ifndef __generalIntegrator_hpp__
#define __generalIntegrator_hpp__
 
#include <complex>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>
 
#include <Eigen/Dense>
 
#include "../../error/mxErrorOld.hpp"
#include "../../ioutils/stringUtils.hpp"
 
#ifndef BREAD_CRUMB
#define MXLIB_GENERAL_INTEGRATOR_LOCAL_BREAD_CRUMB
#ifdef DEBUG
#define BREAD_CRUMB std::cout << "DEBUG: " << __FILE__ << " " << __LINE__ << "\n";
#else
#define BREAD_CRUMB
#endif
#endif
 
namespace mx
{
namespace AO
{
namespace sim
{
 
template <typename _realT>
struct wfMeasurement
{
    typedef _realT realT;
 
    realT iterNo;
 
    // typedef Eigen::Array< realT, Eigen::Dynamic, Eigen::Dynamic> commandT;
    typedef std::vector<realT> commandT;
 
    commandT measurement;
};
 
/// Implements a general integrator controller.
/** \todo document the math for the G.I.
 * \todo document the filter coefficients, etc.
 *
 * \tparam _realT is the floating point type for all calculations.
 */
template <typename _realT>
class generalIntegrator
{
 
  public:
    /// The real data type
    typedef _realT realT;
 
    /// The real data type
    typedef std::complex<realT> complexT;
 
    /// The wavefront data type
    //   typedef wavefront<realT> wavefrontT;
 
    /// The command type
    typedef wfMeasurement<realT> commandT;
 
    /// The image type, used here as a general storage array
    typedef Eigen::Array<realT, Eigen::Dynamic, Eigen::Dynamic> imageT;
 
    /// Default c'tor.
    generalIntegrator();
 
  protected:
    int m_nModes{ 0 }; ///< The number of modes being filtered.
 
    bool m_openLoop{ false }; ///< If true, then commands are not integrated. Default is false.
 
    int m_openLoopDelay{ 0 }; ///< If > 0, then the loop is open for this time in time steps. Default is 0.
 
    int m_closingRamp{ 0 }; ///< If > 0, then gains are famped linearly up to m_closingGains over this interval in time
                            ///< steps.  Default is 0. This is relative m_openLoopDelay.
 
    int m_closingDelay{ 0 }; ///< If > 0, then the simple integrator,with m_closingGains is used up to this timestep.
                             ///< This is relative m_openLoopDelay. Default = 0.
 
    imageT m_closingGains; ///< Column-vector of gains used for loop closing as simple integrator
 
    int m_lowOrders{ 0 }; ///< If > 0, then this sets the maximum mode number which is filtered. All remaining modes are
                          ///< set to 0.  Default = 0.
 
    imageT m_a;
    int m_currA;
 
    imageT m_b;
    int m_currB;
 
    imageT m_gains; ///< Column-vector of gains
 
    imageT m_commandsIn;
    imageT m_commandsOut;
 
  public:
    /// Allocate and initialize all state.
    /**
     * \returns 0 on success, a negative integer otherwise.
     */
    int initialize( int nModes /**< [in] the number of modes to be filtered */ );
 
    /// Get the number of modes
    /** nModes is only set by calling initialize.
     *
     * \returns the current value of m_nModes
     */
    int nModes();
 
    /// Set the m_openLoop flag.
    /** If m_openLoop is true, then commands are not filtered.
     *
     * \returns 0 on success, a negative integer on error.
     */
    int openLoop( bool ol /**< [in] the new value of m_openLoop */ );
 
    /// Get the value of the m_openLoop flag.
    /**
     * \returns the current value of m_openLoop.
     */
    bool openLoop();
 
    /// Set the open loop delay.
    /** If m_openLoopDelay  > 0, then the loop is open for this period in time-steps
     * Requires m_closingDelay > 0.
     *
     * \returns 0 on success, a negative integer on error.
     */
    int openLoopDelay( int cr /**< [in] The new value of m_openLoopDelay */ );
 
    /// Get the value of the m_openLoopDelay.
    /**
     * \returns the current value of m_openLoopDelay.
     */
    int openLoopDelay();
 
    /// Set the closing ramp.
    /** If m_closingRamp  > 0, then the gains are ramped linearly up to m_closingGains over this timer interval in
     * timesteps. Requires m_closingDelay > 0.
     *
     * \returns 0 on success, a negative integer on error.
     */
    int closingRamp( int cr /**< [in] The new value of m_closingRamp */ );
 
    /// Get the value of the m_closingRamp.
    /**
     * \returns the current value of m_closingRamp.
     */
    int closingRamp();
 
    /// Set the closing delay.
    /** If m_closingDelay  > 0, then the simple integragor is used until this timestep.
     *
     * \returns 0 on success, a negative integer on error.
     */
    int closingDelay( int cd /**< [in] The new value of m_closingDelay */ );
 
    /// Get the value of the m_closingDelay.
    /**
     * \returns the current value of m_closingDelay.
     */
    int closingDelay();
 
    /// Set the simple integrator gains to use during closing.
    /**
     * \returns 0 on success
     * \returns < 0 on error
     */
    int closingGains( const std::vector<realT> &gains /**< [in] vector of gains.*/ );
 
    /// Set the simple integrator gain to use for all modes during closing.
    /**
     * \returns 0 on success
     * \returns < 0 on error
     */
    int closingGains( realT g );
 
    /// Set m_lowOrders.
    /** If m_lowOrders > 0, then this sets the maximum mode number which is filtered. All remaining modes are set to 0.
     *
     * \returns 0 on success, a negative integer on error.
     */
    int lowOrders( int lo /**< [in] The new value of m_lowOrders */ );
 
    /// Get the value of the m_lowOrders.
    /**
     * \returns the current value of m_lowOrders.
     */
    int lowOrders();
 
    /// Set the size of the IIR vector (the a coefficients)
    /** This allocates m_a to be m_nModes X n in size.
     *
     * \returns 0 on success, negative number on error.
     */
    int setASize( int n /**< [in] the number of IIR coefficients */ );
 
    /// Set the IIR coefficients for one mode.
    /**
     * \returns 0 on success, negative number on error.
     */
    int setA( int i,          ///< [in] the mode number
              const imageT &a ///< [in] the IIR coefficients for this mode
    );
 
    /// Set the size of the FIR vector (the b coefficients)
    /** This allocates m_b to be m_nModes X n in size.
     *
     * \returns 0 on success, negative number on error.
     */
    int setBSize( int n /**<  [in] the number of FIR coefficients */ );
 
    /// Set the FIR coefficients for one mode.
    /**
     * \returns 0 on success, negative number on error.
     */
    int setB( int i,          ///< [in] the mode number
              const imageT &b ///< [in] the IIR coefficients for this mode
    );
 
    /// Get the gain for a single mode.
    /**
     * \returns the gain value if mode exists.
     */
    realT gain( int i /**< The mode number*/ );
 
    /// Set the gain for a single mode.
    /**
     * \returns 0 on success, negative number on error.
     */
    int gain( int i,  ///< The mode number
              realT g ///< The new gain value
    );
 
    /// Set the gain for all modes to a single value
    /**
     * \returns 0 on success, negative number on error.
     */
    int gains( realT g /**< The new gain value*/ );
 
    /// Set the gains for all modes, using a vector to specify each gain
    /** The vector must be exactly as long as m_nModes.
     *
     * \returns 0 on success, negative number on error.
     */
    int gains( const std::vector<realT> &gains /**< [in] vector of gains.*/ );
 
    /// Set the gains for all modes, using a file to specify each gain
    /** The file format is a simple ASCII single column, with 1 gain per line.
     * Must be exactly as long as m_nModes.
     *
     * \returns 0 on success, negative number on error.
     */
    int gains( const std::string &ogainf /**<  [in] the name of the file, full path */ );
 
    /// Allocate the provided command structures
    /** Used by the calling system to allocate the commands being passed between components.
     *
     * \returns 0 on success, negative number on error.
     */
    int initMeasurements( commandT &filtAmps, ///< The structure to contain the filtered commands
                          commandT &rawAmps   ///< The structure to contain the raw commands
    );
 
    int filterCommands( commandT &filtAmps, commandT &rawAmps, int iterNo );
};
 
template <typename realT>
generalIntegrator<realT>::generalIntegrator()
{
}
 
template <typename realT>
int generalIntegrator<realT>::initialize( int nModes )
{
    m_nModes = nModes;
 
    // If m_a has been sized, resize it
    if( m_a.cols() > 0 )
    {
        int n = m_a.cols();
        m_a.resize( m_nModes, n );
        m_a.setZero();
        m_currA = 0;
 
        m_commandsOut.resize( m_nModes, n );
        m_commandsOut.setZero();
    }
 
    // If m_b has been sized, resize it
    if( m_b.cols() > 0 )
    {
        int n = m_b.cols();
        m_b.resize( m_nModes, n );
        m_b.setZero();
        m_currB = 0;
 
        m_commandsIn.resize( m_nModes, n );
        m_commandsIn.setZero();
    }
 
    m_closingGains.resize( 1, m_nModes );
 
    m_closingGains.setZero();
 
    m_gains.resize( 1, m_nModes );
 
    m_gains.setZero();
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::nModes()
{
    return m_nModes;
}
 
template <typename realT>
int generalIntegrator<realT>::openLoop( bool ol )
{
    m_openLoop = ol;
    return 0;
}
 
template <typename realT>
bool generalIntegrator<realT>::openLoop()
{
    return m_openLoop;
}
 
template <typename realT>
int generalIntegrator<realT>::openLoopDelay( int old )
{
    m_openLoopDelay = old;
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::openLoopDelay()
{
    return m_openLoopDelay;
}
 
template <typename realT>
int generalIntegrator<realT>::closingRamp( int cr )
{
    m_closingRamp = cr;
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::closingRamp()
{
    return m_closingRamp;
}
 
template <typename realT>
int generalIntegrator<realT>::closingDelay( int cd )
{
    m_closingDelay = cd;
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::closingDelay()
{
    return m_closingDelay;
}
 
template <typename realT>
int generalIntegrator<realT>::closingGains( const std::vector<realT> &gains )
{
 
    if( gains.size() != (size_t)m_closingGains.cols() )
    {
        mxError( "generalIntegrator::closingGains", MXE_SIZEERR, "input gain vector not same size as number of modes" );
        return -1; ///\retval -1 on vector size mismatch
    }
 
    for( int i = 0; i < m_closingGains.cols(); ++i )
    {
        m_closingGains( 0, i ) = gains[i];
    }
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::closingGains( realT g )
{
    for( int i = 0; i < m_closingGains.cols(); ++i )
    {
        m_closingGains( 0, i ) = g;
    }
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::lowOrders( int lo )
{
    m_lowOrders = lo;
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::lowOrders()
{
    return m_lowOrders;
}
 
template <typename realT>
int generalIntegrator<realT>::setASize( int n )
{
    // Resize with m_nModes if set
    if( m_nModes > 0 )
    {
        m_a.resize( m_nModes, n );
        m_commandsOut.resize( m_nModes, n );
    }
    else
    {
        m_a.resize( 1, n );
        m_commandsOut.resize( 1, n );
    }
 
    m_a.setZero();
    m_currA = 0;
    m_commandsOut.setZero();
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::setA( int i, const imageT &a )
{
    m_a.row( i ) = a;
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::setBSize( int n )
{
    // Resize with m_nModes if set
    if( m_nModes > 0 )
    {
        m_b.resize( m_nModes, n );
        m_commandsIn.resize( m_nModes, n );
    }
    else
    {
        m_b.resize( 1, n );
        m_commandsIn.resize( 1, n );
    }
 
    m_b.setZero();
    m_currB = 0;
    m_commandsIn.setZero();
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::setB( int i, const imageT &b )
{
    m_b.row( i ) = b;
 
    return 0;
}
 
template <class realT>
realT generalIntegrator<realT>::gain( int i )
{
    if( i < 0 || i >= m_nModes )
    {
        mxError( "generalIntegrator::gain", MXE_INVALIDARG, "mode index out of range" );
        return 0; ///\retval 0 if the mode doesn't exist.
    }
 
    return m_gains( i );
}
 
template <typename realT>
int generalIntegrator<realT>::gain( int i, realT g )
{
    if( i < 0 || i >= m_nModes )
    {
        mxError( "generalIntegrator::gain", MXE_INVALIDARG, "mode index out of range" );
        return 0; ///\retval 0 if the mode doesn't exist.
    }
 
    m_gains( 0, i ) = g;
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::gains( realT g )
{
    for( int i = 0; i < m_gains.cols(); ++i )
    {
        m_gains( 0, i ) = g;
    }
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::gains( const std::vector<realT> &gains )
{
 
    if( gains.size() != (size_t)m_gains.cols() )
    {
        mxError( "generalIntegrator::gains", MXE_SIZEERR, "input gain vector not same size as number of modes" );
        return -1; ///\retval -1 on vector size mismatch
    }
 
    for( int i = 0; i < m_gains.cols(); ++i )
    {
        m_gains( 0, i ) = gains[i];
    }
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::gains( const std::string &ogainf )
{
    std::ifstream fin;
    fin.open( ogainf );
 
    if( !fin.good() )
    {
        mxError( "generalIntegrator::gains", MXE_FILEOERR, "could not open gan file" );
        return -1; /// \retval -1 if file open fails
    }
 
    std::string tmpstr;
    realT g;
    for( int i = 0; i < m_gains.cols(); ++i )
    {
        fin >> tmpstr;
        g = ioutils::convertFromString<realT>( tmpstr );
        gain( i, g );
    }
 
    return 0;
}
 
template <class realT>
int generalIntegrator<realT>::initMeasurements( commandT &filtAmps, commandT &rawAmps )
{
    filtAmps.measurement.resize( m_nModes );
    for( size_t n = 0; n < m_nModes; ++n )
        filtAmps.measurement[n] = 0;
    // filtAmps.measurement.setZero();
 
    rawAmps.measurement.resize( m_nModes );
    for( size_t n = 0; n < m_nModes; ++n )
        rawAmps.measurement[n] = 0;
    // rawAmps.measurement.resize(1, m_nModes);
    // rawAmps.measurement.setZero();
 
    return 0;
}
 
template <typename realT>
int generalIntegrator<realT>::filterCommands( commandT &filtAmps, commandT &rawAmps, int iterNo )
{
    filtAmps.iterNo = rawAmps.iterNo;
 
    if( m_openLoop || iterNo < m_openLoopDelay )
    {
        for( size_t n = 0; n < m_nModes; ++n )
            filtAmps.measurement[n] = 0;
        // filtAmps.measurement.setZero();
        return 0;
    }
 
    realT aTot, bTot;
 
    if( iterNo - m_openLoopDelay < m_closingDelay )
    {
        BREAD_CRUMB;
 
        for( int i = 0; i < m_nModes; ++i )
        {
            // if( std::isnan( rawAmps.measurement(0,i) ) || !std::isfinite(rawAmps.measurement(0,i)))
            // rawAmps.measurement(0,i) = 0.0;
            if( std::isnan( rawAmps.measurement[i] ) || !std::isfinite( rawAmps.measurement[i] ) )
                rawAmps.measurement[i] = 0.0;
 
            // m_commandsIn(i, m_currB) = rawAmps.measurement(0,i);
            m_commandsIn( i, m_currB ) = rawAmps.measurement[i];
 
            aTot = m_commandsOut( i, m_currA );
            // std::cerr << m_currA << " " << aTot << "\n";
 
            int cA = m_currA + 1;
            if( cA >= m_a.cols() )
                cA = 0;
 
            realT gf = 1.0;
 
            if( iterNo - m_openLoopDelay < m_closingRamp )
                gf = ( (realT)iterNo - m_openLoopDelay ) / m_closingRamp;
 
            // m_commandsOut(i, cA) = aTot + gf*m_closingGains(i) * rawAmps.measurement(0,i);
            m_commandsOut( i, cA ) = aTot + gf * m_closingGains( i ) * rawAmps.measurement[i];
 
            // std::cerr << cA << " " << m_commandsOut(i, cA) << "\n";
 
            if( i <= m_lowOrders || m_lowOrders <= 0 )
            {
                filtAmps.measurement[i] = m_commandsOut( i, cA );
            }
            else
            {
                filtAmps.measurement[i] = 0;
            }
        }
 
        ++m_currB;
        if( m_currB >= m_b.cols() )
            m_currB = 0;
 
        ++m_currA;
        if( m_currA >= m_a.cols() )
            m_currA = 0;
 
        return 0;
    }
 
    for( int i = 0; i < m_nModes; ++i )
    {
        if( m_gains( i ) == 0 )
        {
            // filtAmps.measurement(0,i) = 0;
            filtAmps.measurement[i] = 0;
            continue;
        }
 
        // if( std::isnan( rawAmps.measurement(0,i) ) || !std::isfinite(rawAmps.measurement(0,i)))
        // rawAmps.measurement(0,i) = 0.0;
        if( std::isnan( rawAmps.measurement[i] ) || !std::isfinite( rawAmps.measurement[i] ) )
            rawAmps.measurement[i] = 0.0;
 
        aTot = 0;
        for( int j = 0; j < m_a.cols(); ++j )
        {
            int k = m_currA - j;
            if( k < 0 )
                k += m_a.cols();
            aTot += m_a( i, j ) * m_commandsOut( i, k );
        }
 
        m_commandsIn( i, m_currB ) = rawAmps.measurement[i];
 
        bTot = 0;
        for( int j = 0; j < m_b.cols(); ++j )
        {
            int k = m_currB - j;
            if( k < 0 )
                k += m_b.cols();
 
            bTot += m_b( i, j ) * m_commandsIn( i, k );
        }
 
        int cA = m_currA + 1;
        if( cA >= m_a.cols() )
            cA = 0;
 
        m_commandsOut( i, cA ) = aTot + m_gains( i ) * bTot;
 
        filtAmps.measurement[i] = m_commandsOut( i, cA );
    }
 
    ++m_currB;
    if( m_currB >= m_b.cols() )
        m_currB = 0;
 
    ++m_currA;
    if( m_currA >= m_a.cols() )
        m_currA = 0;
 
    return 0;
}
 
} // namespace sim
} // namespace AO
} // namespace mx
 
#ifdef MXLIB_GENERAL_INTEGRATOR_LOCAL_BREAD_CRUMB
#undef BREAD_CRUMB
#undef MXLIB_GENERAL_INTEGRATOR_LOCAL_BREAD_CRUMB
#endif
 
#endif //__generalIntegrator_hpp__