videotransmiter

User_Code/module/software/powercontrol/RLS.hpp

@@ -0,0 +1,128 @@
+#include "FreeRTOS.h"
+#include "Matrix.hpp"
+#include "task.h"
+
+#pragma once
+
+namespace Core
+{
+namespace Control
+        {
+namespace Math
+                {
+
+                        template < uint32_t dim >
+class RLS
+                        {
+                                public:
+                                /**
+                                 * @brief Delete the default constructor
+                                 */
+                                RLS() = delete;
+
+                                /**
+                                 * @brief The constructor
+                                 * @param delta_ The intialized non-singular value of the transfer matrix
+                                 * @param lambda_ The forgotten index
+                                 */
+    constexpr RLS(float delta_, float lambda_)
+        : dimension(dim), lambda(lambda_), delta(delta_), lastUpdate(0), updateCnt(0),
+                                defaultParamsVector(Matrixf<dim, 1>::zeros())
+                                {
+                                        this->reset();
+                                        this->validate();
+                                }
+
+    constexpr RLS(float delta_, float lambda_, Matrixf<dim, 1> initParam): RLS(delta_, lambda_)
+                                {defaultParamsVector = initParam; }
+
+                                /**
+                                 * @brief Reset the RLS module
+                                 * @retval None
+                                 */
+    void reset()
+                                {
+                                        transMatrix = Matrixf<dim, dim>::eye() * delta;
+                                        gainVector = Matrixf<dim, 1>::zeros();
+                                        paramsVector = Matrixf<dim, 1>::zeros();
+                                }
+
+                                /**
+                                 * @brief Proccess a cycle of RLS update
+                                 * @param sampleVector The new samples input expressed in n x 1 dimensionasl vector form
+                                 * @param actualOutput The actual feedback real output
+                                 * @retval paramsVector
+                                 */
+    const Matrixf<dim, 1> & update(Matrixf<dim, 1> & sampleVector, float actualOutput)
+                                {
+                                        gainVector =
+                                        (transMatrix * sampleVector) / (
+                                            1.0f + (sampleVector.trans() * transMatrix * sampleVector)[0][0] / lambda) /
+                                        lambda;  // Get gain vector
+                                        paramsVector += gainVector * (
+                                            actualOutput - (sampleVector.trans() * paramsVector)[0][0])
+;                                // Get params vector
+                                        transMatrix =
+                                        (transMatrix - gainVector * sampleVector.trans() * transMatrix) / lambda
+;  // Get transferred matrix
+
+                                        updateCnt++;
+                                        lastUpdate = xTaskGetTickCount();
+        return paramsVector;
+                                }
+
+                                /**
+                                 * @brief Set the default regression parameters
+                                 * @param updatedParams
+                                 * @retval None
+                                 */
+    void setParamVector(const Matrixf<dim, 1> & updatedParams)
+                                {
+                                        paramsVector = updatedParams;
+                                        defaultParamsVector = updatedParams;
+                                }
+                                /**
+                                 * @brief The getter function of the params vector
+                                 * @param None
+                                 * @retval paramsVector
+                                 */
+    constexpr Matrixf<dim, 1> & getParamsVector() const{return paramsVector; }
+
+                                /**
+                                 * @brief The getter function of the output vector
+                                 * @param None
+                                 * @retval The estimated / filterd output of the RLS module
+                                 */
+    const float &getOutput() const{return output; }
+
+                                private:
+                                /**
+                                 * @brief Lambda and delta validate check
+                                 * @param None
+                                 * @retval None
+                                 */
+    void validate() const
+                                {
+                                        configASSERT(lambda >= 0.0f || lambda <= 1.0f);
+                                        configASSERT(delta > 0);
+                                }
+
+    uint32_t dimension;  // Dimension of the RLS space
+    float lambda;        // The forget index
+    float delta;         // Intialized value of the transferred matrix
+
+    TickType_t lastUpdate;  // Last update tick
+    uint32_t updateCnt;     // Total update Count
+
+                                /*RLS relvant matrix*/
+    Matrixf<dim, dim > transMatrix;  // Transfer matrix instance
+    Matrixf<dim, 1 > gainVector;     // Gain vector for params update
+    Matrixf<dim, 1 > paramsVector;   // Params vector
+    Matrixf<dim, 1 > defaultParamsVector;
+    float output;  // Estimated / filtered output
+                        }
+;
+
+                } // namespace Math
+        } // namespace Control
+} // namespace Core