initial changes on pid to handle 2nd rls estimation

Author: camilo

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$
 *
-* <center>
 * \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$
-* </center>
 *
 *
 * and the remaining parameters \f$\mu\f$, \f$\alpha\f$, \f$\lambda\f$ are internally

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

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:
 *
 *     <center> \f$y(t)=kx(t)+b_{1}x(t-1)+kx(t-2)-a_{1}y(t-1)-a_{2}y(t-2)\f$ </center>
 *

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

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
             {