jzh/demo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

[feat] add quadratic_fit

Browse Source
This commit is contained in:
jzh 2024-07-21 16:07:07 +08:00
parent e378061865
commit e9f740f757
3 changed files with 129 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

63
math/quadratic_fit/calc.c Normal file
View File

@ -0,0 +1,63 @@
#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;
}

25
math/quadratic_fit/calc.h Normal file
View File

@ -0,0 +1,25 @@
#ifndef __CALC_H__
#define __CALC_H__
#define NUM_TYPE float
#define CALC_POINTS (128)
/* y = a * x^2 + b * x + c */
struct quadratic_coefficient_s {
NUM_TYPE a;
NUM_TYPE b;
NUM_TYPE c;
};
#ifdef __cplusplus
extern "C" {
#endif
void calc_quadratic_fit_init(int length);
void calc_quadratic_fit(NUM_TYPE *buff, struct quadratic_coefficient_s *coeff, int length);
#ifdef __cplusplus
}
#endif
#endif /* __CALC_H__ */

41
math/quadratic_fit/main.c Normal file
View File

@ -0,0 +1,41 @@
#include "stdint.h"
#include "stdio.h"
#include "calc.h"
uint16_t raw[CALC_POINTS] = {
11555, 11501, 11432, 11370, 11307, 11250, 11186, 11131,
11083, 11008, 10968, 10901, 10858, 10794, 10754, 10695,
10651, 10603, 10554, 10505, 10470, 10418, 10374, 10336,
10287, 10249, 10215, 10169, 10140, 10097, 10056, 10024,
9992, 9953, 9934, 9891, 9867, 9837, 9806, 9782,
9764, 9742, 9704, 9685, 9667, 9644, 9623, 9612,
9591, 9576, 9559, 9545, 9537, 9526, 9516, 9502,
9489, 9482, 9474, 9469, 9460, 9460, 9456, 9452,
9452, 9463, 9453, 9457, 9456, 9468, 9474, 9476,
9486, 9490, 9504, 9513, 9527, 9542, 9545, 9568,
9579, 9598, 9610, 9632, 9656, 9671, 9696, 9722,
9744, 9763, 9799, 9812, 9850, 9881, 9904, 9927,
9980, 9995, 10034, 10063, 10112, 10143, 10176, 10224,
10258, 10296, 10344, 10383, 10417, 10465, 10515, 10559,
10608, 10651, 10702, 10752, 10804, 10853, 10905, 10952,
11010, 11070, 11129, 11179, 11233, 11289, 11357, 11418};
NUM_TYPE buff[CALC_POINTS];
struct quadratic_coefficient_s coeff;
int main(void)
{
uint32_t c, r;
for (int i = 0; i < CALC_POINTS; i++) {
buff[i] = (NUM_TYPE)raw[i];
}
calc_quadratic_fit_init(CALC_POINTS);
calc_quadratic_fit(buff, &coeff, CALC_POINTS);
printf("y = %f * x^2 + %f * x + %f\r\n", coeff.a, coeff.b, coeff.c);
for (int i = 0; i < CALC_POINTS; i++) {
c = (uint32_t)(coeff.a * i * i + coeff.b * i + coeff.c);
r = (uint32_t)raw[i];
printf("fit[%d] = %d, raw = %d, err = %d\r\n", i, c, r, c - r);
}
return 0;
}