RLS.hpp

#include <cstdio>
#include <ctime>
#include <stdexcept>
 
#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;
            paramsVector = initParam;
        }
 
        void reset() {
            transMatrix = Matrixf<dim, dim>::eye() * delta;
            gainVector = Matrixf<dim, 1>::zeros();
            paramsVector = defaultParamsVector;
            output = 0.0f;
            lastUpdate = 0;
            updateCnt = 0;
        }
 
        const Matrixf<dim, 1> &update(const Matrixf<dim, 1> &sampleVector, float actualOutput) {
            const float previousOutput = (sampleVector.trans() * paramsVector)[0][0];
            gainVector = (transMatrix * sampleVector) /
                            (1.0f + (sampleVector.trans() * transMatrix * sampleVector)[0][0] / lambda) /
                            lambda;  // Get gain vector
            paramsVector +=
                gainVector * (actualOutput - previousOutput);  // Get params vector
            transMatrix =
                (transMatrix - gainVector * sampleVector.trans() * transMatrix) / lambda;  // Get transferred matrix
            output = (sampleVector.trans() * paramsVector)[0][0];
 
            updateCnt++;
            lastUpdate = std::clock();
            return paramsVector;
        }
 
        void setParamVector(const Matrixf<dim, 1> &updatedParams) {
            paramsVector = updatedParams;
            defaultParamsVector = updatedParams;
        }
        constexpr Matrixf<dim, 1>& getParamsVector() {
            return paramsVector;
        }
 
        constexpr const Matrixf<dim, 1>& getParamsVector() const {
            return paramsVector;
        }
 
        const float &getOutput() const {
            return output;
        }
 
        private:
        void validate() const {
 
            if (!(lambda > 0.0f && lambda <= 1.0f))
                throw std::invalid_argument("RLS lambda must be in (0, 1]");
 
            if(!(delta > 0))
                throw std::invalid_argument("RLS delta must be positive");
        }
 
        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 {0.0f};  // Estimated / filtered output
    };
}