From fca339c891cd38d4d27691d2576fee98eb75545f Mon Sep 17 00:00:00 2001 From: camilo Date: Fri, 29 Aug 2025 20:35:11 -0500 Subject: [PATCH 1/3] initial changes on pid to handle 2nd rls estimation --- doc/qpid.dox | 51 +++++++--- doc/qrms.dox | 10 +- doc/qssmoother.dox | 8 +- doc/qtdl.dox | 56 +++++------ src/include/pid.hpp | 55 ++++++++--- src/pid.cpp | 226 ++++++++++++++++++++++++++++++++------------ 6 files changed, 282 insertions(+), 124 deletions(-) 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..c5859a8 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,30 @@ 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 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 +160,7 @@ namespace qlibs { } inline void setMemoryFactor( const real_t lambda ) noexcept { - l = lambda; + estParams.lambda = lambda; } inline void setMomentum( const real_t Mu ) noexcept { diff --git a/src/pid.cpp b/src/pid.cpp index 3d92295..4f2790b 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -315,77 +315,161 @@ 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.stable = getTimeConstant( p.tau1, r, dt ); + } +} +/*============================================================================*/ +void pidAutoTuning::estimationStepN1( estimationParams &p, + const real_t y ) +{ + 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] ) ); + 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] ) ); + 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] ) ); +} +/*============================================================================*/ +void pidAutoTuning::estimationStepN2( estimationParams &p, + const real_t y ) +{ + 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]) ); + 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] ) ); + 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] ) ); + 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] ) ); + 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] ) ); +} +/*============================================================================*/ 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; } /*============================================================================*/ 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; + 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 +481,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, 10000.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; } From 3f07791110e8ea9ee700733433deb748b92d94e2 Mon Sep 17 00:00:00 2001 From: camilo Date: Mon, 1 Sep 2025 11:30:49 -0500 Subject: [PATCH 2/3] some fixes --- src/include/pid.hpp | 3 +++ src/pid.cpp | 45 ++++++++++++++++++++++++++++++++++----------- 2 files changed, 37 insertions(+), 11 deletions(-) diff --git a/src/include/pid.hpp b/src/include/pid.hpp index c5859a8..550805a 100644 --- a/src/include/pid.hpp +++ b/src/include/pid.hpp @@ -131,6 +131,9 @@ namespace qlibs { 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 ); diff --git a/src/pid.cpp b/src/pid.cpp index 4f2790b..768cb81 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -371,13 +371,29 @@ void pidAutoTuning::computeParamsN2( systemParams& p, 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] = { @@ -385,20 +401,24 @@ void pidAutoTuning::estimationStepN1( estimationParams &p, ( 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] ) ); + 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] ) ); + 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] = { @@ -408,6 +428,9 @@ void pidAutoTuning::estimationStepN2( estimationParams &p, ( 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] ) ); @@ -415,16 +438,16 @@ void pidAutoTuning::estimationStepN2( estimationParams &p, 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] ) ); + 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] ) ); + 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] ) ); + 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] ) ); + p.P[3][3] = f*( p.P[3][3] - ( L[3]*P_phi[3] ) ) + reg; } /*============================================================================*/ bool pidAutoTuning::step( const real_t u, @@ -435,7 +458,7 @@ bool pidAutoTuning::step( const real_t u, estimationStep( estParams, y ); estParams.y2 = estParams.y1; - estParams.y1 = y; + estParams.y1 = -y; estParams.u2 = estParams.u1; estParams.u1 = u; computeParameters( sysParams, estParams, dt ); @@ -498,10 +521,10 @@ void pidAutoTuning::initialize( const pidGains current, 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, 10000.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.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 From 95002866d5622d8bebef2c8c4801f21e21231d55 Mon Sep 17 00:00:00 2001 From: camilo Date: Wed, 3 Sep 2025 21:00:18 -0500 Subject: [PATCH 3/3] some needed changes --- src/include/pid.hpp | 1 + src/pid.cpp | 14 +++++++++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/include/pid.hpp b/src/include/pid.hpp index 550805a..789f349 100644 --- a/src/include/pid.hpp +++ b/src/include/pid.hpp @@ -188,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 768cb81..316d15e 100644 --- a/src/pid.cpp +++ b/src/pid.cpp @@ -470,10 +470,22 @@ bool pidAutoTuning::step( const real_t u, 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 tau = sysParams.tau1; + const real_t tau = sysParams.tau1 + sysParams.tau2; const real_t k = sysParams.gain; const real_t td = tau*0.1_re;