CAPL RLS implementation

/* CAPL implementation of RLS algorithm */

/*
    CAPL Types (for reference, with C99 equivalents)
    char = uint8_t  int  = int16_t   long  = int32_t   float  = IEEE 32-bit
    byte = uint8_t  word = uint16_t  dword = uint32_t  double = IEEE 64-bit
*/

/********************* Variables required */
    const byte SET_ON = 1;
    const byte SET_OFF = 0;

    // Vectors, matrices and values for RLS algorithm
    const byte n_theta = 2;     // Assume linear relationship, y = a*x +b;
    const byte n_cov = 4;       // P \in Real^(n_theta*n_theta) [NB: byte type limits n_cov to 0xFF elements]

    double theta_s[n_theta];    // Short-term predictions
    double cov_s[n_cov];        // Short-term covariance matrix
    const double lambda_s = 0.98;   // Forgetting factor for short-term prediction
/* Ljung (1999) says that memory of estimator is approximately M samples
*     Where, M = 1 / (1 - lambda)   (lambda = 0.98 --> M = 50 samples)
*     Therefore, lambda = (M - 1) / M
*     lambda MUST be in interval (0, 1], typically is in [0.95, 0.995]
*/

    double theta_l[n_theta];    // Long-term predictions
    double cov_l[n_cov];        // Long-term covariance matrix
    const double lambda_l = 1.0;    // Forgetting factor for long-term prediction (no forgetting)

    // Indices for covariance matrices
    const byte p11 = 0; const byte p12 = 1; const byte p21 = 2; const byte p22 = 3;

    // Indices for parameter vectors
    const byte th1 = 0;     const byte th2 = 1;

    const double COV_INIT = 1e5;    // Initialisation value for estimator covariances
    int thetaNotInit;       // Tracks if theta has been fully initialased

/********************* Set-up */
    // Initialise covariance matrices to I*1e5:
    cov_s[p11] = COV_INIT;
    cov_s[p22] = COV_INIT;
    cov_l[p11] = COV_INIT;
    cov_l[p22] = COV_INIT;

    // Initialise theta - assume gradient = -1 initially (adjust as required)
    theta_s[th1] = theta_l[th1]= -1.0;

    // Initialise theta - assume offset = 32 initially (adjust as required)
    theta_s[th2] = theta_l[th2]= 32.0;

/********************* RLS functions */
doRLS(double lambda, double theta[], double cov[], double x, double y)
{
/* Perform RLS estimation (with specified forgetting factor), system input is `x', observation is `y'
*      Assumes system being parameterised is y = th1*x+th2
*/

    double cov_next[n_cov];     // Temporary covariance matrix
    double LDivisor, L1, L2, OSAPE; // L1, L2 are gain vector elements, OSAPE is `1-step ahead prediction error'

    LDivisor = 1 + x * (x * cov[p11] + cov[p12] + cov[p21]) + cov[p22];

    L1 = (x * cov[p11] + cov[p12]) / LDivisor;
    L2 = (x * cov[p21] + cov[p22]) / LDivisor;

    OSAPE = y - (x * theta[th1] + theta[th2]);

    theta[th1] = theta[th1] + L1 * OSAPE;
    theta[th2] = theta[th2] + L2 * OSAPE;

    cov_next[p11] = cov[p11] - L1 * (x * cov[p11] + cov[p21]) / lambda;
    cov_next[p12] = cov[p12] - L1 * (x * cov[p12] + cov[p22]) / lambda;
    cov_next[p21] = cov[p21] - L2 * (x * cov[p11] + cov[p21]) / lambda;
    cov_next[p22] = cov[p22] - L2 * (x * cov[p12] + cov[p22]) / lambda;

    _memcpy(cov, cov_next);
}


byte _memcpy (double dest[], double source[])
{
/* Copy one array of doubles to another.
*     Mimics C memcpy() function.

    byte destLen, sourceLen, iter;

    destLen = elcount(dest);
    sourceLen = elcount(source);

    if (destLen != sourceLen) {
        runError(1001,1);   // Can't copy if not equal lengths - throw a runtime error
        return SET_OFF;
    } else {
        for (iter = 0; iter < sourceLen; ++iter){
            dest[iter] = source[iter];
        }
        return SET_ON;
    }
}