Вычисляем скорость наименьшими квадратами / Least square speed calculations

[eddy_em]

Все вожусь с СУ астросибовскими монтировками. Сначала вычислял скорость 1 раз в промежуток времени тупо как отношение разности координат к разности времен. Получалось очень шумно и ступенчато. Сейчас запилил "плавающее" вычисление наименьшими квадратами. Значительно красивше вышло. И по "болтанке" скорости на интервале, где она должна быть постоянна, сразу понял, что я неправильно настроил ПИД-регулятор sidservo (то ли I, то ли P задрал выше, чем нужно).

Код прост до примитивизма: заводим массивы для накопления N данных в соответствии с каждой используемой суммой. При поступлении новых данных в сумму добавляем свежее и вычитаем последнее.

typedef struct{
    double *x, *t, *t2, *xt; // arrays of coord/time and multiply
    double xsum, tsum, t2sum, xtsum; // sums of coord/time and their multiply
    size_t idx; // index of current data in array
    size_t arraysz; // size of arrays
} less_square_t;

less_square_t *LS_init(size_t Ndata){
    if(Ndata < 5){
        DBG("Ndata=%zd - TOO SMALL", Ndata);
        return NULL;
    }
    DBG("Init less squares: %zd", Ndata);
    less_square_t *l = calloc(1, sizeof(less_square_t));
    l->x = calloc(Ndata, sizeof(double));
    l->t2 = calloc(Ndata, sizeof(double));
    l->t = calloc(Ndata, sizeof(double));
    l->xt = calloc(Ndata, sizeof(double));
    l->arraysz = Ndata;
    return l;
}
void LS_delete(less_square_t **l){
    if(!l || !*l) return;
    free((*l)->x); free((*l)->t2); free((*l)->t); free((*l)->xt);
    free(*l);
    *l = NULL;
}
// add next data portion and calculate current slope
double LS_calc_slope(less_square_t *l, double x, double t){
    if(!l) return 0.;
    size_t idx = l->idx;
    double oldx = l->x[idx], oldt = l->t[idx], oldt2 = l->t2[idx], oldxt = l->xt[idx];
    double t2 = t * t, xt = x * t;
    l->x[idx] = x; l->t2[idx] = t2;
    l->t[idx] = t; l->xt[idx] = xt;
    ++idx;
    l->idx = (idx >= l->arraysz) ? 0 : idx;
    l->xsum += x - oldx;
    l->t2sum += t2 - oldt2;
    l->tsum += t - oldt;
    l->xtsum += xt - oldxt;
    double n = (double)l->arraysz;
    double denominator = n * l->t2sum - l->tsum * l->tsum;
    if(fabs(denominator) < 1e-7) return 0.;
    double numerator = n * l->xtsum - l->xsum * l->tsum;
    return (numerator / denominator);
}