mirror of
https://gitee.com/dlmu-cone/tronone-h7-scaffold
synced 2026-07-23 19:25:09 +08:00
129 lines
5.7 KiB
C++
129 lines
5.7 KiB
C++
#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
|