From e9f740f7575a37b7a2c5fa115de92208e163d8af Mon Sep 17 00:00:00 2001 From: jzh Date: Sun, 21 Jul 2024 16:07:07 +0800 Subject: [PATCH] [feat] add quadratic_fit --- math/quadratic_fit/calc.c | 63 +++++++++++++++++++++++++++++++++++++++ math/quadratic_fit/calc.h | 25 ++++++++++++++++ math/quadratic_fit/main.c | 41 +++++++++++++++++++++++++ 3 files changed, 129 insertions(+) create mode 100644 math/quadratic_fit/calc.c create mode 100644 math/quadratic_fit/calc.h create mode 100644 math/quadratic_fit/main.c diff --git a/math/quadratic_fit/calc.c b/math/quadratic_fit/calc.c new file mode 100644 index 0000000..12deb88 --- /dev/null +++ b/math/quadratic_fit/calc.c @@ -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; +} diff --git a/math/quadratic_fit/calc.h b/math/quadratic_fit/calc.h new file mode 100644 index 0000000..56feb01 --- /dev/null +++ b/math/quadratic_fit/calc.h @@ -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__ */ diff --git a/math/quadratic_fit/main.c b/math/quadratic_fit/main.c new file mode 100644 index 0000000..db9abcb --- /dev/null +++ b/math/quadratic_fit/main.c @@ -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; +} \ No newline at end of file