SOIdentification.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2010, LABUST, UNIZG-FER
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the LABUST nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 #include <labust/control/SOIdentification.hpp>
00035 #include <labust/math/NumberManipulation.hpp>
00036 
00037 #include <boost/numeric/ublas/matrix_proxy.hpp>
00038 
00039 using namespace labust::control;
00040 
00041 SOIdentification::SOIdentification(
00042                 double C, double X, double R, double E, double identLen):
00043                 finished(false),
00044                 relay(C,X),
00045                 params(numParams,0),
00046                 tSum(0),
00047                 maxOsc(-std::numeric_limits<double>::max()),
00048                 minOsc(std::numeric_limits<double>::max()),
00049                 counterHigh(0),
00050                 counterLow(0),
00051                 eMaxError(E),
00052                 eMinError(E),
00053                 ref(0),
00054                 identLen(identLen),
00055                 xa_high(identLen,0),
00056                 xa_low(identLen,0),
00057                 e_min(identLen,0),
00058                 e_max(identLen,0),
00059                 t_min(identLen,0),
00060                 t_max(identLen,0)
00061 {this->reset();};
00062 
00063 SOIdentification::~SOIdentification(){};
00064 
00065 double SOIdentification::step(double error, double dT)
00066 {
00067         //Calculate output
00068         double retVal = relay.step(error);
00069         //Update timebase.
00070         tSum += dT;
00071 
00072   //Preserve the minimum and maximum oscillation value.
00073   if (error > maxOsc) maxOsc=error;
00074   if (error < minOsc) minOsc=error;
00075 
00076   if (int sw = relay.hasSwitched())
00077   {
00078     //std::cout<<"Relay switched. Error:"<<error<<std::endl;
00079 
00080     //Remember response data.
00081     if (sw == 1)
00082     {
00083       xa_high[counterHigh] = error;
00084       e_min[counterLow] = minOsc;
00085       t_min[counterLow] = tSum;
00086       tSum = 0;
00087       counterLow=(counterLow+1)%identLen;
00088     }
00089     else
00090     {
00091       xa_low[counterLow] = error;
00092       e_max[counterHigh] = maxOsc;
00093       t_max[counterHigh] = tSum;
00094       tSum = 0;
00095       counterHigh=(counterHigh+1)%identLen;
00096     }
00097 
00098     //Reset oscillation min and max.
00099     maxOsc=-std::numeric_limits<double>::max();
00100     minOsc=std::numeric_limits<double>::max();
00101 
00102     //Get the parameters for this cycle.
00103     calculateParameters();
00104   }
00105 
00106   return retVal;
00107 }
00108 
00109 void SOIdentification::calculateParameters()
00110 {
00111         using boost::numeric::ublas::row;
00112 
00113   double meanEMax(labust::math::mean(e_max)),
00114                 stdEMax(labust::math::std2(e_max)),
00115 
00116                 meanEMin(labust::math::mean(e_min)),
00117                 stdEMin(labust::math::std2(e_min)),
00118 
00119                 meanTMax(labust::math::mean(t_max)),
00120                 meanTMin(labust::math::mean(t_min)),
00121 
00122                 meanXaLow(labust::math::mean(xa_low)),
00123                 meanXaHigh(labust::math::mean(xa_high));
00124 
00125   double T(meanTMax+meanTMin),
00126                 Xm(0.5*(meanEMax-meanEMin)),
00127                 X0(0.5*(meanEMax+meanEMin)),
00128                 xa_star(0.5*(meanXaHigh-meanXaLow));
00129 
00130   double omega(2*M_PI/T),
00131                 C(relay.getAmplitude());
00132 
00133   double sq1((xa_star+X0)/Xm);
00134   sq1 = sqrt(1-sq1*sq1);
00135   double sq2((xa_star-X0)/Xm);
00136   sq2 = sqrt(1-sq2*sq2);
00137 
00138   params[alpha] = 2*C*(sq1+sq2)/(M_PI*omega*omega*Xm);
00139   params[kx]=(4.0*C*xa_star)/(omega*M_PI*Xm*Xm);
00140   params[kxx]=(3.0*C*xa_star)/(2*omega*omega*Xm*Xm*Xm);
00141   params[delta] = C*(meanTMax - meanTMin)/(meanTMax + meanTMin);
00142   params[wn] = omega;
00143 
00144   //std::cout<<"Xa_star"<<xa_star<<", X0"<<X0<<std::endl;
00145   //std::cout<<"Alpha = "<<params[alpha]<<", Kx = "<<params[kx]<<", Kxx = "<<params[kxx]<<", Delta:"<<params[delta]<<", w:"<<omega<<std::endl;
00146 
00147   finished = ((std::fabs(stdEMax/meanEMax)<eMaxError) && (std::fabs(stdEMin/meanEMin)<eMinError));
00148   minMaxError = (std::fabs(stdEMax/meanEMax) + std::fabs(stdEMin/meanEMin))/2;
00149 }
00150 
00151 void SOIdentification::reset()
00152 {
00153         finished = false;
00154         xa_high = std::vector<double>(identLen,0);
00155         xa_low = std::vector<double>(identLen,0);
00156         e_min = std::vector<double>(identLen,0);
00157         e_max = std::vector<double>(identLen,0);
00158         t_min = std::vector<double>(identLen,0);
00159         t_max = std::vector<double>(identLen,0);
00160         tSum = 0;
00161         maxOsc = -std::numeric_limits<double>::max();
00162         minOsc = std::numeric_limits<double>::max();
00163         counterHigh = 0;
00164         counterLow = 0;
00165         relay.setAmplitude(relay.getAmplitude());
00166 }


ident_so
Author(s): Gyula Nagy
autogenerated on Fri Aug 28 2015 11:23:43