54 lines
1.1 KiB
C
54 lines
1.1 KiB
C
#include "liblocky.h"
|
|
|
|
#include <assert.h>
|
|
#include <math.h>
|
|
#include <string.h>
|
|
|
|
|
|
void blky_sensor_feed(blky_sensor_t* b, const float* v, uint64_t n) {
|
|
assert(b->time+n <= BLKY_SENSOR_MAX_DUR);
|
|
if (n == 0) return;
|
|
|
|
const uint64_t t = b->time;
|
|
memcpy(b->values+t, v, n*sizeof(float));
|
|
b->time += n;
|
|
|
|
for (uint64_t i = t; i < b->time; ++i) {
|
|
b->sum += b->values[i];
|
|
}
|
|
b->avg = b->sum / (double) b->time;
|
|
|
|
b->var = 0;
|
|
for (uint64_t i = 0; i < b->time; ++i) {
|
|
const double diff = b->values[i] - b->avg;
|
|
b->var += diff*diff;
|
|
}
|
|
b->var /= (double) b->time;
|
|
|
|
b->cov = 0;
|
|
for (uint64_t i = 0; i < b->time; ++i) {
|
|
const double diff_v = b->values[i] - b->avg;
|
|
const double diff_t = (double) i - (double) b->time/2.;
|
|
b->cov += diff_v * diff_t;
|
|
}
|
|
b->cov /= (double) b->time;
|
|
|
|
const double tf = (double) b->time;
|
|
const double tvar =
|
|
(tf/6*(tf+1)*(2*tf+2)-tf*tf/2-tf*tf*tf/4)/tf;
|
|
|
|
b->prev_correl = b->correl;
|
|
|
|
if (b->var == 0) {
|
|
b->correl = 1;
|
|
} else {
|
|
b->correl = b->cov / sqrt(b->var*tvar);
|
|
}
|
|
}
|
|
|
|
void blky_sensor_drop(blky_sensor_t* b, uint64_t until) {
|
|
(void) b;
|
|
(void) until;
|
|
// TODO
|
|
}
|