64 lines
3.3 KiB
C
64 lines
3.3 KiB
C
|
#include "calc.h"
|
||
|
|
|
||
|
|
static NUM_TYPE quadratic_fit_sumx1_arr[CALC_POINTS];
|
||
|
|
static NUM_TYPE quadratic_fit_sumx2_arr[CALC_POINTS];
|
||
|
|
static NUM_TYPE quadratic_fit_sumx1;
|
||
|
|
static NUM_TYPE quadratic_fit_sumx2;
|
||
|
|
static NUM_TYPE quadratic_fit_sumx3;
|
||
|
|
static NUM_TYPE quadratic_fit_sumx4;
|
||
|
|
static NUM_TYPE quadratic_fit_d;
|
||
|
|
static NUM_TYPE quadratic_fit_matrix[3][3];
|
||
|
|
|
||
|
|
void calc_quadratic_fit_init(int length)
|
||
|
|
{
|
||
|
|
for (int i = 0; i < length; i++) {
|
||
|
|
quadratic_fit_sumx1_arr[i] = (NUM_TYPE)i;
|
||
|
|
quadratic_fit_sumx2_arr[i] = quadratic_fit_sumx1_arr[i] * quadratic_fit_sumx1_arr[i];
|
||
|
|
}
|
||
|
|
quadratic_fit_sumx1 = (NUM_TYPE)0;
|
||
|
|
quadratic_fit_sumx2 = (NUM_TYPE)0;
|
||
|
|
quadratic_fit_sumx3 = (NUM_TYPE)0;
|
||
|
|
quadratic_fit_sumx4 = (NUM_TYPE)0;
|
||
|
|
for (int i = 0; i < length; i++) {
|
||
|
|
quadratic_fit_sumx1 += quadratic_fit_sumx1_arr[i];
|
||
|
|
quadratic_fit_sumx2 += quadratic_fit_sumx2_arr[i];
|
||
|
|
quadratic_fit_sumx3 += quadratic_fit_sumx1_arr[i] * quadratic_fit_sumx2_arr[i];
|
||
|
|
quadratic_fit_sumx4 += quadratic_fit_sumx2_arr[i] * quadratic_fit_sumx2_arr[i];
|
||
|
|
}
|
||
|
|
quadratic_fit_d = \
|
||
|
|
quadratic_fit_sumx2 * quadratic_fit_sumx2 * quadratic_fit_sumx2 + \
|
||
|
|
quadratic_fit_sumx1 * quadratic_fit_sumx1 * quadratic_fit_sumx4 + \
|
||
|
|
(NUM_TYPE)length * quadratic_fit_sumx3 * quadratic_fit_sumx3 - \
|
||
|
|
(NUM_TYPE)length * quadratic_fit_sumx2 * quadratic_fit_sumx4 - \
|
||
|
|
(NUM_TYPE)2 * quadratic_fit_sumx1 * quadratic_fit_sumx2 * quadratic_fit_sumx3;
|
||
|
|
quadratic_fit_matrix[0][0] = quadratic_fit_sumx2 * quadratic_fit_sumx2 - quadratic_fit_sumx1 * quadratic_fit_sumx3;
|
||
|
|
quadratic_fit_matrix[0][1] = (NUM_TYPE)length * quadratic_fit_sumx3 - quadratic_fit_sumx1 * quadratic_fit_sumx2;
|
||
|
|
quadratic_fit_matrix[0][2] = quadratic_fit_sumx1 * quadratic_fit_sumx1 - (NUM_TYPE)length * quadratic_fit_sumx2;
|
||
|
|
quadratic_fit_matrix[1][0] = quadratic_fit_sumx1 * quadratic_fit_sumx4 - quadratic_fit_sumx2 * quadratic_fit_sumx3;
|
||
|
|
quadratic_fit_matrix[1][1] = quadratic_fit_sumx2 * quadratic_fit_sumx2 - (NUM_TYPE)length * quadratic_fit_sumx4;
|
||
|
|
quadratic_fit_matrix[1][2] = quadratic_fit_matrix[0][1];
|
||
|
|
quadratic_fit_matrix[2][0] = quadratic_fit_sumx3 * quadratic_fit_sumx3 - quadratic_fit_sumx2 * quadratic_fit_sumx4;
|
||
|
|
quadratic_fit_matrix[2][1] = quadratic_fit_matrix[1][0];
|
||
|
|
quadratic_fit_matrix[2][2] = quadratic_fit_matrix[0][0];
|
||
|
|
}
|
||
|
|
|
||
|
|
void calc_quadratic_fit(NUM_TYPE *buff, struct quadratic_coefficient_s *coeff, int length)
|
||
|
|
{
|
||
|
|
NUM_TYPE sum_y1, sum_xy, sum_x2y;
|
||
|
|
|
||
|
|
if ((buff == (void *)0) || (coeff == (void *)0) || (length == 0)) {
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
sum_y1 = (NUM_TYPE)0;
|
||
|
|
sum_xy = (NUM_TYPE)0;
|
||
|
|
sum_x2y = (NUM_TYPE)0;
|
||
|
|
for (int i = 0; i < length; i++) {
|
||
|
|
sum_y1 += buff[i];
|
||
|
|
sum_xy += quadratic_fit_sumx1_arr[i] * buff[i];
|
||
|
|
sum_x2y += quadratic_fit_sumx2_arr[i] * buff[i];
|
||
|
|
}
|
||
|
|
coeff->a = (sum_y1 * quadratic_fit_matrix[0][0] + sum_xy * quadratic_fit_matrix[0][1] + sum_x2y * quadratic_fit_matrix[0][2]) / quadratic_fit_d;
|
||
|
|
coeff->b = (sum_y1 * quadratic_fit_matrix[1][0] + sum_xy * quadratic_fit_matrix[1][1] + sum_x2y * quadratic_fit_matrix[1][2]) / quadratic_fit_d;
|
||
|
|
coeff->c = (sum_y1 * quadratic_fit_matrix[2][0] + sum_xy * quadratic_fit_matrix[2][1] + sum_x2y * quadratic_fit_matrix[2][2]) / quadratic_fit_d;
|
||
|
|
}
|