RLS.hpp

#include <cstdio>
#include <ctime>
 
#include "Matrix.hpp"
#include "core/logger.h"
 
#pragma once
 
namespace roboctrl::utils
{
    template<uint32_t dim>
    class RLS
    {
        public:
        RLS() = delete;
 
        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;
        }
 
        void reset() {
            transMatrix = Matrixf<dim, dim>::eye() * delta;
            gainVector = Matrixf<dim, 1>::zeros();
            paramsVector = Matrixf<dim, 1>::zeros();
        }
 
        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 = clock();
            return paramsVector;
        }
 
        void setParamVector(const Matrixf<dim, 1> &updatedParams) {
            paramsVector = updatedParams;
            defaultParamsVector = updatedParams;
        }
        constexpr Matrixf<dim, 1> &getParamsVector() const {
            return paramsVector;
        }
 
        const float &getOutput() const {
            return output;
        }
 
        private:
        void validate() const {
 
            if(!(lambda >= 0.0f || lambda <= 1.0f))
                LOG_ERROR("!(lambda >= 0.0f || lambda <= 1.0f)");
 
            if(!(delta > 0))
                LOG_ERROR("!(delta > 0)");
        }
 
        uint32_t dimension;  // Dimension of the RLS space
        float lambda;        // The forget index
        float delta;         // Intialized value of the transferred matrix
 
        size_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
    };
}