|
@@ -3,7 +3,7 @@
|
|
|
|
|
|
|
|
//y = Kax + Cb
|
|
//y = Kax + Cb
|
|
|
void least_square_init(least_square_t *ls, int samples){
|
|
void least_square_init(least_square_t *ls, int samples){
|
|
|
- ls->max_samples = samples > MAX_SAMPLES?MAX_SAMPLES:samples;
|
|
|
|
|
|
|
+ ls->max_samples = samples;
|
|
|
ls->num_samples = 0;
|
|
ls->num_samples = 0;
|
|
|
ls->coeff.Ka = 1.0f;
|
|
ls->coeff.Ka = 1.0f;
|
|
|
ls->coeff.Cb = 0.0f;
|
|
ls->coeff.Cb = 0.0f;
|
|
@@ -15,41 +15,26 @@ void least_square_init(least_square_t *ls, int samples){
|
|
|
}
|
|
}
|
|
|
int least_square_put(least_square_t *ls, double x, double y){
|
|
int least_square_put(least_square_t *ls, double x, double y){
|
|
|
if (!ls->finished){
|
|
if (!ls->finished){
|
|
|
- ls->x[ls->num_samples] = x;
|
|
|
|
|
- ls->y[ls->num_samples] = y;
|
|
|
|
|
- ls->num_samples = (ls->num_samples + 1) % ls->max_samples;
|
|
|
|
|
|
|
+ ls->num_samples ++;
|
|
|
ls->mul_xx += x * x;
|
|
ls->mul_xx += x * x;
|
|
|
ls->mul_xy += x * y;
|
|
ls->mul_xy += x * y;
|
|
|
ls->sum_x += x;
|
|
ls->sum_x += x;
|
|
|
ls->sum_y += y;
|
|
ls->sum_y += y;
|
|
|
- if (ls->num_samples == 0){
|
|
|
|
|
- ls->coeff.Ka = (ls->max_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->max_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
|
|
|
|
|
- ls->coeff.Cb = ls->sum_y / ls->max_samples - ls->coeff.Ka * ls->sum_x / ls->max_samples;
|
|
|
|
|
|
|
+ if (ls->num_samples == ls->max_samples){
|
|
|
|
|
+ ls->coeff.Ka = (ls->num_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->num_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
|
|
|
|
|
+ ls->coeff.Cb = ls->sum_y / ls->num_samples - ls->coeff.Ka * ls->sum_x / ls->num_samples;
|
|
|
ls->finished = 1;
|
|
ls->finished = 1;
|
|
|
}
|
|
}
|
|
|
}else {
|
|
}else {
|
|
|
- double pre_x, pre_y;
|
|
|
|
|
- pre_x = ls->x[ls->num_samples];
|
|
|
|
|
- pre_y = ls->y[ls->num_samples];
|
|
|
|
|
-
|
|
|
|
|
- ls->x[ls->num_samples] = x;
|
|
|
|
|
- ls->y[ls->num_samples] = y;
|
|
|
|
|
- ls->num_samples = (ls->num_samples + 1) % ls->max_samples;
|
|
|
|
|
|
|
+ ls->num_samples ++;
|
|
|
|
|
|
|
|
ls->mul_xx += x * x;
|
|
ls->mul_xx += x * x;
|
|
|
ls->mul_xy += x * y;
|
|
ls->mul_xy += x * y;
|
|
|
ls->sum_x += x;
|
|
ls->sum_x += x;
|
|
|
ls->sum_y += y;
|
|
ls->sum_y += y;
|
|
|
-
|
|
|
|
|
- ls->mul_xx -= pre_x * pre_x;
|
|
|
|
|
- ls->mul_xy -= pre_x * pre_y;
|
|
|
|
|
- ls->sum_x -= pre_x;
|
|
|
|
|
- ls->sum_y -= pre_y;
|
|
|
|
|
-
|
|
|
|
|
- ls->coeff.Ka = (ls->max_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->max_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
|
|
|
|
|
- ls->coeff.Cb = ls->sum_y / ls->max_samples - ls->coeff.Ka * ls->sum_x / ls->max_samples;
|
|
|
|
|
-
|
|
|
|
|
- ls->finished = 1;
|
|
|
|
|
|
|
+
|
|
|
|
|
+ ls->coeff.Ka = (ls->num_samples * ls->mul_xy - ls->sum_x * ls->sum_y) / (ls->num_samples * ls->mul_xx - ls->sum_x * ls->sum_x);
|
|
|
|
|
+ ls->coeff.Cb = ls->sum_y / ls->num_samples - ls->coeff.Ka * ls->sum_x / ls->num_samples;
|
|
|
}
|
|
}
|
|
|
return ls->finished;
|
|
return ls->finished;
|
|
|
}
|
|
}
|