bms/Application/app/sox/Least_Square.c

#include <stdio.h>
#include "libs/logger.h"

//y = ax + b
typedef struct {
	double x;
	double y;
}smaple_t;

static smaple_t samples[5];
static int sample_count;
static float AA, BB;


void Least_square_method(void)
{
	int i=0;
	double K = 0, A = 0, B = 0, C = 0, D = 0;	
	for(i=0;i<sample_count;i++) {
		A += samples[i].x * samples[i].y;
		B += samples[i].x;
		C += samples[i].y;
		D += samples[i].x * samples[i].x;
	}
	AA = (sample_count * A - B * C) / ( sample_count * D - B * B);
	BB = C / sample_count - K * B / sample_count;
	sys_debug("Gain = %f, zero Off = %f\n", AA, BB);
}

void add_smaple(float x, float y){
	if (sample_count < 5){
		samples[sample_count].x = x;
		samples[sample_count].y = y;
		sample_count ++;
		sys_debug("add x = %f, y = %f\n", x, y);
	}
	if (sample_count == 5){
		Least_square_method();
	}
}