diff --git a/doc/qpid.dox b/doc/qpid.dox
index 5afba09..12cff9d 100644
--- a/doc/qpid.dox
+++ b/doc/qpid.dox
@@ -268,40 +268,63 @@
*
* @section qpid_autotune Autotuning
*
-* Autotuning can eliminate much of the trial and error of a manual tuning
-* approach, especially if you do not have a lot of loop tuning experience.
-* Performing the autotuning procedure will get the tuning parameters close to
-* their optimal values, but additional manual tuning may be required to get
-* the tuning parameters to their optimal values.
+* Autotuning eliminates much of the trial-and-error required by manual PID tuning,
+* especially when loop dynamics are complex or the user lacks tuning experience.
+* The procedure provides controller gains that are close to optimal, but a fine
+* adjustment step may still be necessary to achieve the best performance.
*
-* The Autotune feature for the controller will only run for a limited amount of time
-* after it gets enabled. In other words, autotuning does not run continuously
-* during operation. Whenever there is a substantial change in the process
-* dynamics, the tuning process will need to be repeated in order to derive new
-* gains required for optimal control.
+* The Autotune feature is executed only for a limited time after activation;
+* it is not a continuous background process. If the plant dynamics change
+* significantly (e.g., due to load variation or environmental conditions),
+* the autotuning sequence must be repeated to update the control gains.
*
-* Autotuning is performed by using the following recursive algorithm:
+* The underlying method is based on a Recursive Least Squares (RLS) identification
+* of the process, followed by an analytical mapping to PID parameters. The RLS
+* algorithm estimates a first-order process model, from which the controller
+* computes suitable gains. The recursive update makes the method efficient for
+* real-time embedded systems, as it avoids repeated excitation tests or large
+* datasets.
+*
+* @note
+* The estimation assumes a first-order stable process model. Therefore,
+* this autotuning method is only reliable when applied to inherently
+* stable processes. For unstable or integrating processes, the RLS
+* identification may diverge and the computed PID gains may not be valid.
+*
+* The algorithm proceeds as follows:
+*
+* Compute the correction gain \f$ L(t) \f$
*
-*
* \f$ L(t) = \frac{ P(t-1)\phi }{ \lambda + \phi^{T}P(t-1)\phi } \f$
*
+* Perform correction
+*
* \f$\theta(t) = \theta(t-1) + L[ y(t) - \phi^{T}\theta(t) ] \f$
*
-* \f$ P(t) = \lambda^{-1}[ I - L(t)+\phi^{T}]P(t-1) \f$
+* where,
*
* \f$ \theta(t) = \begin{bmatrix} \theta_{1}(t) & \theta_{2}(t) \end{bmatrix}^T \f$
* and \f$ \phi(t) = \begin{bmatrix} -y(t-1) & u(t-1) \end{bmatrix}^T \f$
*
+* Update covariance
+*
+* \f$ P(t) = \lambda^{-1}[ I - L(t)+\phi^{T}]P(t-1) \f$
+*
+* Compute the system gain \f$ g(t) \f$
+*
* \f$ g(t) = \frac{ (1 - \mu)\theta_{2}(t) }{ 1 + \theta_{1} } + \mu g(t-1) \f$
*
+* Compute the system time-constant \f$ \tau(t) \f$
+*
* \f$ \tau(t) = \frac{ ( \mu - 1 )dt }{ ln( |\theta_{1}| ) } + \mu \tau(t-1) \f$
*
+* Compute the PID gains
+*
* \f$ K_{c}(t) = \alpha \frac{ 1.35 }{ g(t) } \left ( \frac{ \tau(t) }{ T_d } + 0.185 \right ) \f$
*
* \f$ K_{i}(t) = K_{c}(t) \frac {\tau(t) + 0.611 T_d}{ 2.5 T_d( \tau(t) + 0.185 T_d)} \f$
*
* \f$ K_{d}(t) = \frac{ 0.37 K_{c}(t) T_d \tau(t) }{ \tau(t) + 0.185 T_d} \f$
-*
*
*
* and the remaining parameters \f$\mu\f$, \f$\alpha\f$, \f$\lambda\f$ are internally
diff --git a/doc/qrms.dox b/doc/qrms.dox
index e323e49..cebc0e0 100644
--- a/doc/qrms.dox
+++ b/doc/qrms.dox
@@ -17,9 +17,12 @@
* specifications, either by configuring a timer interrupt or by delegating
* responsibility to a real-time task.
*
-* @section qrms_ex1 Example : By using a timer interrupt
+* @section qrms_ex1 Example: Using a Timer Interrupt
*
-* rms_compute.c
+* The following example demonstrates how to configure a timer to periodically update the RMS
+* estimator at a 1 ms interval.
+*
+* @b File: @c rms_compute.c
* @code{.c}
* #include "bsp.h"
* #include "hal_timer.h"
@@ -53,7 +56,8 @@
*
*
* @endcode
-* rms_compute.h
+*
+* @b File: @c rms_compute.h
* @code{.c}
*
* #ifndef RMS_COMPUTE_H
diff --git a/doc/qssmoother.dox b/doc/qssmoother.dox
index eedb422..0d34b41 100644
--- a/doc/qssmoother.dox
+++ b/doc/qssmoother.dox
@@ -44,12 +44,10 @@
*
* @section qssmoother_lpf2 Second Order Low Pass Filter
*
-* \ref qlibs::smootherLPF2 is a filter with similar properties to the 1st order low pass filter. The main
-* difference is that the stop band roll-off will be twice the 1st order filters
-* at 40dB/decade (12dB/octave) as the operating frequency increases above the
-* cut-off frequency \f$w\f$.
+* \ref qlibs::smootherLPF2 extends the first-order filter with steeper attenuation.
+* Its stop-band roll-off is twice as sharp (≈40 dB/decade or 12 dB/octave).
*
-* The difference-equation of the output of this filter is given by:
+* The difference equation is:
*
* \f$y(t)=kx(t)+b_{1}x(t-1)+kx(t-2)-a_{1}y(t-1)-a_{2}y(t-2)\f$
*
diff --git a/doc/qtdl.dox b/doc/qtdl.dox
index d864a71..8a313ff 100644
--- a/doc/qtdl.dox
+++ b/doc/qtdl.dox
@@ -1,43 +1,45 @@
/*! @page qtdl_desc Tapped Delay Line in O(1)
-* The class \ref qtdl is an implementation of the Tapped Delay Line (TDL) structure.
-* A TDL is a discrete element in digital filter theory, that allows a signal to
-* be delayed by a number of samples and provides an output signal for each delay.
-* Here, a TDL is implemented as a circular buffer. This means
-* that the complexity of the enqueue and dequeue operations is constant @c O(1), so
-* integer delays can be computed very efficiently.
-*
-* The delay by one sample is notated \f$z^{-1}\f$ and delays of \f$N\f$ samples
-* is notated as \f$z^{-N}\f$ motivated by the role the z-transform plays in
-* describing digital filter structures.
-*
-* To create a TDL, you just need to define an instance of type \ref qlibs::tdl and
-* then, configure it by using the constructor or \ref qlibs::tdl::setup(),
-* where you can define the number of lines to be delayed and the initial values
-* for all of them. Then, you can start operating over this structure by inserting
-* samples of the input signal by using \ref qlibs::tdl::insertSample().
-* You can also get any specific delay from it by using:
-*
-* - \ref qlibs::tdl::getOldest(), to get the oldest delay at tap \f$z^{-N}\f$
-* - \ref qlibs::tdl::getAtIndex(), to get the delay at tap \f$z^{-i}\f$.
-* You can also use the index using an unsigned value as follows: @c "delay[ i ]"
-* - Index operator
-*
-* Given the applications of this structure, the \ref qlibs::tdl class is used as the
-* base component of some aspects of \ref qlibs::smoother and \ref qlibs::ltisys.
-*
-* @section qtdl_ex1 Example : Code snippet to instantiate a TDL to hold up to 256 delays.
+* The class \ref qtdl implements a Tapped Delay Line (TDL), a fundamental structure
+* in digital filter theory. A TDL delays an input signal by a number of samples and
+* exposes each delayed version as an output
+*
+* In this implementation, the TDL is backed by a circular buffer. As a result,
+* both enqueue and dequeue operations run in constant time @c O(1), making integer
+* delays highly efficient.
+*
+* A delay of one sample is denoted \f$z^{-1}\f$ , while a delay of \f$N\f$ samples
+* is denoted \f$z^{-N}\f$ consistent with z-transform notation in digital signal
+* processing.
+*
+* To use a TDL, create an instance of \ref qlibs::tdl and configure it using either
+* the constructor or \ref qlibs::tdl::setup(). During setup you specify both the
+* number of delay taps and their initial values. Once configured, samples of the
+* input signal can be inserted using \ref qlibs::tdl::insertSample() (or the
+* function-call operator).
+*
+* You can access specific delayed samples via:
+* - \ref qlibs::tdl::getOldest() - retrieves the oldest sample (tap \f$z^{-N}\f$)
+* - \ref qlibs::tdl::getAtIndex() — retrieves the sample at tap \f$z^{-i}\f$
+* - The index operator @c delay[i] — equivalent to @c getAtIndex(i)
+*
+* Because of its versatility, the \ref qlibs::tdl class is a core component in
+* higher-level modules such as \ref qlibs::smoother and \ref qlibs::ltisys.
+*
+* @section qtdl_ex1 Example : Instantiating a TDL with 256 delay taps
*
* @code{.c}
* real_t storage[ 256 ] = { 0.0f };
* tdl delay( storage );
* @endcode
*
+* Insert new samples:
* @code{.c}
* delay.insertSample( 2.5 );
* delay.insertSample( -2.3 );
* delay( 4.8 ); // same as delay.insertSample( 4.8 )
* @endcode
*
+* Retrieve delayed samples:
* @code{.c}
* auto d3 = delay[ 3 ]; // get delay at t-3, same as delay.getAtIndex( 3 )
* auto dOldest = delay.getOldest(); // get the oldest sample at t-N
diff --git a/src/include/pid.hpp b/src/include/pid.hpp
index fedfb92..789f349 100644
--- a/src/include/pid.hpp
+++ b/src/include/pid.hpp
@@ -87,12 +87,33 @@ namespace qlibs {
}
/*! @endcond */
+
/**
* @brief A PID Auto-tuning object
* @details The instance should be bound to a configured PID controller by
* using the pidController::bindAutoTuning() method
*/
class pidAutoTuning {
+ /*! @cond */
+ struct systemParams {
+ real_t gain;
+ real_t tau1;
+ real_t tau2;
+ bool stable;
+ };
+
+ struct estimationParams {
+ real_t theta[4]; /*parameter estimations*/
+ real_t P[4][4]; /*covariance matrix*/
+ real_t lambda; /*forgetting factor [ 0.9 < l < 1 ]*/
+ real_t y1, y2; /*past values of y(t)*/
+ real_t u1, u2; /*past values of u(t)*/
+ };
+
+ using EstimationStepFn = void(*)(estimationParams&, real_t);
+ using ComputeParamsFn = void(*)(systemParams&,const estimationParams&, const real_t);
+
+ /*! @endcond */
friend class pidController;
public:
/**
@@ -101,20 +122,33 @@ namespace qlibs {
static const uint32_t UNDEFINED;
protected:
/*! @cond */
- real_t p00{ 1.0_re }; /*covariance value*/
- real_t p01{ 0.0_re }; /*covariance value*/
- real_t p10{ 0.0_re }; /*covariance value*/
- real_t p11{ 1.0_re }; /*covariance value*/
- real_t b1{ 0.1_re }; /*estimation value*/
- real_t a1{ 0.9_re }; /*estimation value*/
- real_t uk{ 0.0_re }; /*process input*/
- real_t yk{ 0.0_re }; /*process output*/
- real_t l{ 0.9898_re }; /*memory factor [ 0.9 < l < 1 ]*/
- real_t k{ 1.0_re }; /*process static gain*/
- real_t tao{ 1.0_re }; /*process time constant*/
+ EstimationStepFn estimationStep{ &pidAutoTuning::estimationStepN1 };
+ ComputeParamsFn computeParameters{ &pidAutoTuning::computeParamsN1 };
+ estimationParams estParams;
+ systemParams sysParams;
+
real_t mu{ 0.95_re }; /*variation attenuation*/
real_t speed{ 0.25_re }; /*final controller speed*/
uint32_t it{ UNDEFINED }; /*enable time*/
+
+
+ static bool resetEstimation( estimationParams &p,
+ const real_t r );
+ static bool getTimeConstant( real_t& tau,
+ const real_t abs_z,
+ const real_t dt );
+ static void computeParamsN1( systemParams& p,
+ const estimationParams& e,
+ const real_t dt );
+ static void computeParamsN2( systemParams& p,
+ const estimationParams& e,
+ const real_t dt );
+
+ static void estimationStepN1( estimationParams &p,
+ const real_t y );
+ static void estimationStepN2( estimationParams &p,
+ const real_t y );
+
static bool isValidValue( const real_t x ) noexcept;
pidType type{ pidType::PID_TYPE_PI };
void initialize( const pidGains current,
@@ -129,7 +163,7 @@ namespace qlibs {
}
inline void setMemoryFactor( const real_t lambda ) noexcept
{
- l = lambda;
+ estParams.lambda = lambda;
}
inline void setMomentum( const real_t Mu ) noexcept
{
@@ -154,6 +188,7 @@ namespace qlibs {
public:
pidAutoTuning() = default;
pidGains getEstimates( void ) const noexcept;
+ bool enableOrder2Estimates( const bool en ) noexcept;
};
/**
diff --git a/src/pid.cpp b/src/pid.cpp
index 3d92295..316d15e 100644
--- a/src/pid.cpp
+++ b/src/pid.cpp
@@ -315,77 +315,196 @@ real_t pidController::control( const real_t w,
return u;
}
/*============================================================================*/
+bool pidAutoTuning::getTimeConstant( real_t& tau,
+ const real_t abs_z,
+ const real_t dt )
+{
+ const bool stable = ( abs_z < 1.0_re );
+
+ if ( stable && ( abs_z > 1e-12 ) ) {
+ tau = -dt/ffmath::log( abs_z );
+ }
+
+ return stable;
+}
+/*============================================================================*/
+void pidAutoTuning::computeParamsN1( systemParams& p,
+ const estimationParams& e,
+ const real_t dt )
+{
+ const real_t b1 = e.theta[ 0 ];
+ const real_t a1 = e.theta[ 1 ];
+ const real_t abs_z1 = ffmath::absf( a1 );
+
+ p.gain = b1/( 1.0_re + a1 );
+ p.tau2 = 0.0_re;
+ p.stable = getTimeConstant( p.tau1, abs_z1, dt );
+}
+/*============================================================================*/
+void pidAutoTuning::computeParamsN2( systemParams& p,
+ const estimationParams& e,
+ const real_t dt )
+{
+ const real_t b1 = e.theta[ 0 ];
+ const real_t b2 = e.theta[ 1 ];
+ const real_t a1 = e.theta[ 2 ];
+ const real_t a2 = e.theta[ 3 ];
+
+ const real_t a1a1 = a1*a1;
+ const real_t a2_4 = 4.0_re*a2;
+ const real_t d = a1a1 - a2_4;
+
+ p.gain = ( b1 + b2 )/( 1.0_re + a1 + a2 );
+ p.stable = true;
+
+ if ( d >= 0.0_re ) {
+ const real_t sqrt_d = ffmath::sqrt( d );
+ const real_t z1 = (-a1 + sqrt_d)*0.5_re;
+ const real_t z2 = (-a1 - sqrt_d)*0.5_re;
+ const real_t abs_z1 = ffmath::absf( z1 );
+ const real_t abs_z2 = ffmath::absf( z2 );
+
+ p.stable = getTimeConstant( p.tau1, abs_z1, dt );
+ p.stable &= getTimeConstant( p.tau2, abs_z2, dt );
+ }
+ else {
+ const real_t rPart = -0.5_re*a1;
+ const real_t iPart = -0.5_re*ffmath::sqrt( a2_4 - a1a1 );
+ const real_t r = ffmath::hypot( rPart, iPart );
+ p.tau2 = 0.0_re;
+ p.stable = getTimeConstant( p.tau1, r, dt );
+ }
+}
+/*============================================================================*/
+bool pidAutoTuning::resetEstimation( estimationParams &p,
+ const real_t r )
+{
+ bool rst = ( r < 0.0_re ) || ( r > 10000.0_re );
+ if ( rst ) {
+ constexpr real_t r_alfa = 0.1_re;
+ p.P[0][0] = r_alfa; p.P[0][1] = 0.0_re; p.P[0][2] = 0.0_re; p.P[0][3] = 0.0_re;
+ p.P[1][0] = 0.0_re; p.P[1][1] = r_alfa; p.P[1][2] = 0.0_re; p.P[1][3] = 0.0_re;
+ p.P[2][0] = 0.0_re; p.P[2][1] = 0.0_re; p.P[2][2] = r_alfa; p.P[2][3] = 0.0_re;
+ p.P[3][0] = 0.0_re; p.P[3][1] = 0.0_re; p.P[3][2] = 0.0_re; p.P[3][3] = r_alfa;
+ }
+ return rst;
+}
+/*============================================================================*/
+void pidAutoTuning::estimationStepN1( estimationParams &p,
+ const real_t y )
+{
+ constexpr real_t reg = 0.0001_re;
+ const real_t f = 1.0_re/p.lambda;
+ const real_t phi[2] = { p.u1, p.y2 };
+ const real_t P_phi[2] = {
+ ( p.P[0][0]*phi[0] ) + ( p.P[0][1]*phi[1] ),
+ ( p.P[1][0]*phi[0] ) + ( p.P[1][1]*phi[1] ),
+ };
+ const real_t r = p.lambda + ( ( phi[0]*P_phi[0] ) + ( phi[1]*P_phi[1] ) );
+ if ( resetEstimation( p, r ) ) {
+ return;
+ }
+ const real_t inv_r = 1.0_re/r;
+ const real_t L[4] ={ P_phi[0]*inv_r, P_phi[1]*inv_r };
+ const real_t e = y - ( ( phi[0]*p.theta[0] ) + ( phi[1]*p.theta[1] ) );
+ p.theta[0] += L[0]*e;
+ p.theta[1] += L[1]*e;
+ p.P[0][0] = f*( p.P[0][0] - ( L[0]*P_phi[0] ) ) + reg;
+ p.P[0][1] = f*( p.P[0][1] - ( L[0]*P_phi[1] ) );
+ p.P[1][0] = f*( p.P[1][0] - ( L[1]*P_phi[0] ) );
+ p.P[1][1] = f*( p.P[1][1] - ( L[1]*P_phi[1] ) ) + reg;
+}
+/*============================================================================*/
+void pidAutoTuning::estimationStepN2( estimationParams &p,
+ const real_t y )
+{
+ constexpr real_t reg = 0.0001_re;
+ const real_t f = 1.0_re/p.lambda;
+ const real_t phi[4] = { p.u1, p.u2, p.y1, p.y2 };
+ const real_t P_phi[4] = {
+ ( p.P[0][0]*phi[0] ) + ( p.P[0][1]*phi[1] ) + ( p.P[0][2]*phi[2] ) + ( p.P[0][3]*phi[3] ),
+ ( p.P[0][1]*phi[0] ) + ( p.P[1][1]*phi[1] ) + ( p.P[1][2]*phi[2] ) + ( p.P[1][3]*phi[3] ),
+ ( p.P[0][2]*phi[0] ) + ( p.P[1][2]*phi[1] ) + ( p.P[2][2]*phi[2] ) + ( p.P[2][3]*phi[3] ),
+ ( p.P[0][3]*phi[0] ) + ( p.P[1][3]*phi[1] ) + ( p.P[2][3]*phi[2] ) + ( p.P[3][3]*phi[3] ),
+ };
+ const real_t r = p.lambda + ( ( phi[0]*P_phi[0] ) + ( phi[1]*P_phi[1] ) + ( phi[2]*P_phi[2] ) + ( phi[3]*P_phi[3]) );
+ if ( resetEstimation( p, r ) ) {
+ return;
+ }
+ const real_t inv_r = 1.0_re/r;
+ const real_t L[4] ={ P_phi[0]*inv_r, P_phi[1]*inv_r, P_phi[2]*inv_r, P_phi[3]*inv_r };
+ const real_t e = y - ( ( phi[0]*p.theta[0] ) + ( phi[1]*p.theta[1] ) + ( phi[2]*p.theta[2] ) + ( phi[3]*p.theta[3] ) );
+ p.theta[0] += L[0]*e;
+ p.theta[1] += L[1]*e;
+ p.theta[2] += L[2]*e;
+ p.theta[3] += L[3]*e;
+ p.P[0][0] = f*( p.P[0][0] - ( L[0]*P_phi[0] ) ) + reg;
+ p.P[0][1] = f*( p.P[0][1] - ( L[0]*P_phi[1] ) );
+ p.P[0][2] = f*( p.P[0][2] - ( L[0]*P_phi[2] ) );
+ p.P[0][3] = f*( p.P[0][3] - ( L[0]*P_phi[3] ) );
+ p.P[1][1] = f*( p.P[1][1] - ( L[1]*P_phi[1] ) ) + reg;
+ p.P[1][2] = f*( p.P[1][2] - ( L[1]*P_phi[2] ) );
+ p.P[1][3] = f*( p.P[1][3] - ( L[1]*P_phi[3] ) );
+ p.P[2][2] = f*( p.P[2][2] - ( L[2]*P_phi[2] ) ) + reg;
+ p.P[2][3] = f*( p.P[2][3] - ( L[2]*P_phi[3] ) );
+ p.P[3][3] = f*( p.P[3][3] - ( L[3]*P_phi[3] ) ) + reg;
+}
+/*============================================================================*/
bool pidAutoTuning::step( const real_t u,
const real_t y,
const real_t dt ) noexcept
{
- real_t error , r, l0, l1;
- real_t lp00, lp01, lp10, lp11;
- real_t tmp1, tmp2;
- real_t gain, timeConstant;
- const real_t il = 1.0_re/l;
bool ready = false;
- tmp1 = p00*uk;
- tmp2 = p11*yk;
- r = l + ( uk*( tmp1 - ( p10*yk ) ) ) - ( yk*( ( p01*uk ) - tmp2 ) );
- /*compute corrections*/
- l0 = ( tmp1 - ( p01*yk ) )/r;
- l1 = ( ( p10*uk ) - tmp2 )/r;
- error = y - ( ( b1*uk ) - ( a1*yk ) );
- /*fix estimations*/
- b1 += l0*error;
- a1 += l1*error;
- /*update covariances*/
- lp00 = il*p00;
- lp01 = il*p01;
- lp10 = il*p10;
- lp11 = il*p11;
- tmp1 = ( l0*uk ) - 1.0_re;
- tmp2 = ( l1*yk ) + 1.0_re;
- p00 = ( l0*lp10*yk ) - ( lp00*tmp1 ) + 1e-10_re;
- p01 = ( l0*lp11*yk ) - ( lp01*tmp1 );
- p10 = ( lp10*tmp2 ) - ( l1*lp00*uk );
- p11 = ( lp11*tmp2 ) - ( l1*lp01*uk ) + 1e-10_re;
- /*update I/O measurements*/
- yk = y;
- uk = u;
- gain = b1/( 1.0_re + a1 );
- timeConstant = -dt/ffmath::log( ffmath::absf( a1 ) );
- /*cstat -MISRAC++2008-5-14-1*/
- if ( isValidValue( timeConstant ) && isValidValue( gain ) && ( it > 0UL ) ) { /*no side effects here*/
- /*cstat +MISRAC++2008-5-14-1*/
- k = gain + ( mu*( k - gain ) );
- tao = timeConstant + ( mu*( tao - timeConstant ) );
+ estimationStep( estParams, y );
+ estParams.y2 = estParams.y1;
+ estParams.y1 = -y;
+ estParams.u2 = estParams.u1;
+ estParams.u1 = u;
+ computeParameters( sysParams, estParams, dt );
+ if ( sysParams.stable && ( it > 0UL ) ) {
if ( ( 0UL == --it ) && ( pidAutoTuning::UNDEFINED != it ) ) {
ready = true;
}
}
-
return ready;
}
/*============================================================================*/
+bool pidAutoTuning::enable2ndOrderEstimates( const bool en ) noexcept
+{
+ if ( en ) {
+ estimationStep = estimationStepN2;
+ computeParameters = computeParamsN2;
+ }
+ else {
+ estimationStep = estimationStepN1;
+ computeParameters = computeParamsN1;
+ }
+}
+/*============================================================================*/
pidGains pidAutoTuning::getEstimates( void ) const noexcept
{
pidGains gains = { 0.0_re, 0.0_re, 0.0_re };
- const real_t td = tao*0.1_re;
+ const real_t tau = sysParams.tau1 + sysParams.tau2;
+ const real_t k = sysParams.gain;
+ const real_t td = tau*0.1_re;
switch ( type ) {
case pidType::PID_TYPE_P:
- gains.Kc = speed*( 1.03_re/k )*( ( tao/td ) + 0.34_re );
+ gains.Kc = speed*( 1.03_re/k )*( ( tau/td ) + 0.34_re );
break;
case pidType::PID_TYPE_PD:
- gains.Kc = speed*( 1.24_re/k )*( ( tao/td ) + 0.129_re );
- gains.Kd = gains.Kc*( 0.27_re*td )*( tao - 0.324_re*td )/( tao + 0.129_re*td );
+ gains.Kc = speed*( 1.24_re/k )*( ( tau/td ) + 0.129_re );
+ gains.Kd = gains.Kc*( 0.27_re*td )*( tau - 0.324_re*td )/( tau + 0.129_re*td );
break;
case pidType::PID_TYPE_PI:
- gains.Kc = speed*( 0.9_re/k )*( ( tao/td ) + 0.092_re );
- gains.Ki = gains.Kc*( tao + 2.22_re*td )/( 3.33_re*td*( tao + 0.092_re*td ) );
+ gains.Kc = speed*( 0.9_re/k )*( ( tau/td ) + 0.092_re );
+ gains.Ki = gains.Kc*( tau + 2.22_re*td )/( 3.33_re*td*( tau + 0.092_re*td ) );
break;
case pidType::PID_TYPE_PID:
- gains.Kc = speed*( 1.35_re/k )*( ( tao/td ) + 0.185_re );
- gains.Ki = gains.Kc*( tao + 0.611_re*td )/( 2.5_re*td*( tao + 0.185_re*td ) );
- gains.Kd = ( 0.37_re*gains.Kc*td*tao )/( tao + 0.185_re*td );
+ gains.Kc = speed*( 1.35_re/k )*( ( tau/td ) + 0.185_re );
+ gains.Ki = gains.Kc*( tau + 0.611_re*td )/( 2.5_re*td*( tau + 0.185_re*td ) );
+ gains.Kd = ( 0.37_re*gains.Kc*td*tau )/( tau + 0.185_re*td );
break;
default:
break;
@@ -397,25 +516,41 @@ pidGains pidAutoTuning::getEstimates( void ) const noexcept
void pidAutoTuning::initialize( const pidGains current,
const real_t dt ) noexcept
{
- real_t k_i, T_i;
-
- l = 0.9898_re;
- p00 = 1000.0_re;
- p11 = 1000.0_re;
- p01 = 0.0_re;
- p10 = 0.0_re;
- uk = 0.0_re;
- yk = 0.0_re;
- k = 0.0_re;
- tao = 0.0_re;
+ real_t th0 = 0.25_re; // b0
+ real_t th1 = -0.077_re; // b1
+ real_t th2 = -1.1_re; // a0
+ real_t th3 = 0.28_re; // b1
+ if ( &pidAutoTuning::computeParamsN1 == computeParameters ) {
+ real_t k_i, T_i;
+ k_i = current.Kc/0.9_re;
+ T_i = ( 0.27_re*k_i )/current.Ki;
+ /*cstat -CERT-FLP32-C_b*/
+ th1 = -ffmath::exp( -dt/T_i ); //a1
+ /*cstat +CERT-FLP32-C_b*/
+ th0 = k_i*( 1.0_re + th1 ); //b1
+ }
+
+ estParams = {
+ { th0, th1, th2, th3 },
+ {
+ { 1000.0_re, 0.0_re, 0.0_re, 0.0_re },
+ { 0.0_re, 1000.0_re, 0.0_re, 0.0_re },
+ { 0.0_re, 0.0_re, 1000.0_re, 0.0_re },
+ { 0.0_re, 0.0_re, 0.0_re, 1000.0_re },
+ },
+ 0.9898_re,
+ 0.0_re, 0.0_re, 0.0_re, 0.0_re
+ };
+ sysParams = {
+ 0.0_re,
+ 0.0_re,
+ 0.0_re,
+ false,
+ };
it = 100UL;
mu = 0.95_re;
- k_i = current.Kc/0.9_re;
- T_i = ( 0.27_re*k_i )/current.Ki;
- /*cstat -CERT-FLP32-C_b*/
- a1 = -ffmath::exp( -dt/T_i );
- /*cstat +CERT-FLP32-C_b*/
- b1 = k_i*( 1.0_re + a1 );
+
+
speed = 0.25_re;
it = pidAutoTuning::UNDEFINED;
}