Akifumi Takahashi
This lib is considered to be used as a sensor's calibration program. Calibration with Spline Interpolation might be useful in the case that you want some model expressing relationship such like between a value of physical quantity and your sensor's voltage, but you cannot estimate a model such as liner, square, cubic polynomial, or sine curve. This makes (Parametric) Cubic Spline Polynomial Model (Coefficients of the polynomial) from some sample plots(e.g. sets of (value, voltage)). The inverse function (x,y)->(y,x) has been implemented so as to get analog data (not stepping or leveled data).
Fork of
TRP105F_Spline
by 
Akifumi Takahashi
Diff: CubicSpline.cpp
- Revision:
- 13:9a51747773af
- Parent:
- 12:b3e07a2220bc
--- a/CubicSpline.cpp Sun May 29 12:15:19 2016 +0000
+++ b/CubicSpline.cpp Mon May 30 09:18:50 2016 +0000
@@ -1,9 +1,3 @@
-#define DEBUG
-#define VERSION_C
-#define DEBUG_MAKE_MODEL
-//#define DEBUG_SOLVE
-#define DEBUG_GETX "DEBUG_GETX\n"
-//#define DEBUG_GETY "DEBUG_GETY\n"
#include "CubicSpline.h"
// To get voltage of TRP105F
@@ -26,7 +20,7 @@
_Sample_Num = 5;
_Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
_Last_Point = (Vxyt) {
- 0,0,0
+ 0.0, 0.0, 0.0
};
for(int i = 0; i < 4; i++) {
_C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -43,7 +37,7 @@
_Sample_Num = arg_num;
_Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
_Last_Point = (Vxyt) {
- 0,0,0
+ 0.0, 0.0, 0.0
};
for(int i = 0; i < 4; i++) {
_C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -61,7 +55,7 @@
_Sample_Num = arg_num;
_Sample_Set = (Vxyt *)malloc(_Sample_Num * sizeof(Vxyt));
_Last_Point = (Vxyt) {
- 0,0,0
+ 0.0, 0.0, 0.0
};
for(int i = 0; i < 4; i++) {
_C_x[i]= (double*)malloc((_Sample_Num - 1)* sizeof(double));;
@@ -98,14 +92,14 @@
// Recieve a Distance datus and store it into member
//
g_Serial_Signal.printf("X:");
- _Sample_Set[i].x = 0;
+ _Sample_Set[i].x = 0.0;
do {
sig = g_Serial_Signal.getc();
if('0' <= sig && sig <= '9') {
if(floatflag == 0) {
- _Sample_Set[i].x = 10 * _Sample_Set[i].x + sig - 48;
+ _Sample_Set[i].x = 10.0 * _Sample_Set[i].x + (double)(sig - 48);
} else {
- _Sample_Set[i].x = _Sample_Set[i].x + (sig - 48) * pow((double)10, (double)- floatflag);
+ _Sample_Set[i].x = _Sample_Set[i].x + (double)(sig - 48) * pow(10.0, (double)- floatflag);
floatflag++;
}
g_Serial_Signal.putc(char(sig));
@@ -115,7 +109,7 @@
g_Serial_Signal.putc(char(sig));
}
} else if(sig == 0x08) {
- _Sample_Set[i].x = 0;
+ _Sample_Set[i].x = 0.0;
g_Serial_Signal.printf("[canseled!]");
g_Serial_Signal.putc('\n');
g_Serial_Signal.putc('>');
@@ -131,13 +125,13 @@
// Get 10 data and store mean as a sample.
// After get one original sample, system waits for 0.1 sec,
// thus it takes 1 sec evry sampling.
- _Sample_Set[i].y = 0;
+ _Sample_Set[i].y = 0.0;
for(int j = 0; j < 10; j++) {
tmp_set.y = g_Sensor_Voltage.read();
#ifdef DEBUG
g_Serial_Signal.printf("%f,",tmp_set.y);
#endif
- _Sample_Set[i].y += (tmp_set.y / 10);
+ _Sample_Set[i].y += (tmp_set.y / 10.0);
wait(0.1);
}
#ifdef DEBUG
@@ -146,8 +140,8 @@
}
// if the input data is over the bound, it is calibrated
- if (_Sample_Set[i].x < 0)
- _Sample_Set[i].x = 0;
+ if (_Sample_Set[i].x < 0.0)
+ _Sample_Set[i].x = 0.0;
}
//
// Sort set data array in x-Ascending order
@@ -166,7 +160,7 @@
}
// if a same dst has been input, calcurate mean.
else if (_Sample_Set[i].x == _Sample_Set[j].x) {
- tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2;
+ tmp_set.y = (_Sample_Set[i].y + _Sample_Set[j].y)/2.0;
_Sample_Set[i].y = tmp_set.y;
for(int k = j; k < _Sample_Num - 1; k++)
_Sample_Set[k] = _Sample_Set[k+1];
@@ -186,12 +180,12 @@
#endif
// generate t which is parameter related to x,y
- _Sample_Set[0].t = 0;
+ _Sample_Set[0].t = 0.0;
for(int i = 1; i < _Sample_Num; i++)
_Sample_Set[i].t =
_Sample_Set[i-1].t
- + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2)
- +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2));
+ + sqrt(pow(_Sample_Set[i].x - _Sample_Set[i-1].x, 2.0)
+ +pow(_Sample_Set[i].y - _Sample_Set[i-1].y, 2.0));
}
//
@@ -241,14 +235,14 @@
double *Upper = (double*)malloc((arg_num - 2) * sizeof(double));
double *Lower = (double*)malloc((arg_num - 2) * sizeof(double));
#ifdef DEBUG_MAKE_MODEL
- _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargment set\n");
+ _printOutDataCouple(arg_sampled_t, arg_sampled_ft, arg_num, "\nargument set\n");
#endif
for(int i = 0; i < arg_num - 1; i++)
h[i] = (double)(arg_sampled_t[i + 1] - arg_sampled_t[i]);
for(int i = 0; i < arg_num - 2; i++)
- v[i] = 6 * (
+ v[i] = 6.0 * (
((double)(arg_sampled_ft[i + 2] - arg_sampled_ft[i + 1])) / h[i + 1]
-
((double)(arg_sampled_ft[i + 1] - arg_sampled_ft[i])) / h[i]
@@ -257,11 +251,11 @@
//
// LU decomposition
//
- Upper[0] = 2 * (h[0] + h[1]);
- Lower[0] = 0;
+ Upper[0] = 2.0 * (h[0] + h[1]);
+ Lower[0] = 0.0;
for (int i = 1; i < arg_num - 2; i++) {
Lower[i] = h[i] / Upper[i - 1];
- Upper[i] = 2 * (h[i] + h[i + 1]) - Lower[i] * h[i];
+ Upper[i] = 2.0 * (h[i] + h[i + 1]) - Lower[i] * h[i];
}
//
// forward substitution
@@ -314,6 +308,30 @@
{
_makeModel(arg_t, arg_ft, arg_C, _Sample_Num);
}
+
+
+void CubicSpline2d::calibrateSensor()
+{
+ double t[_Sample_Num];
+ double ft[_Sample_Num];
+
+ _sampleData();
+ _Last_Point = _Sample_Set[0];
+
+ for(int i = 0; i < _Sample_Num; i++) {
+ t[i] = _Sample_Set[i].t;
+ ft[i]= _Sample_Set[i].x;
+ }
+ _makeModel(t,ft,_C_x);
+
+ for(int i = 0; i < _Sample_Num; i++) {
+ ft[i]= _Sample_Set[i].y;
+ }
+ _makeModel(t,ft,_C_y);
+
+}
+
+
//
// Fuction to return the value of Cubic polynomial f(t)
//
@@ -321,16 +339,18 @@
{
double ft; //the value of Spline f(t).
- ft = arg_C[3] * pow(arg_t, 3) + arg_C[2] * pow(arg_t, 2) + arg_C[1] * arg_t + arg_C[0];
+ ft = arg_C[3] * pow(arg_t, 3.0) + arg_C[2] * pow(arg_t, 2.0) + arg_C[1] * arg_t + arg_C[0];
return ft;
}
+
+
//
// Function to solve a cubic polinomial
// by using Gardano-Tartaglia formula
//
void CubicSpline2d::_solve_cubic_f(
- std::complex<double>* arg_t,
+ std::complex<double> arg_t[3],
const double arg_C[4],
const double arg_ft)
{
@@ -339,49 +359,65 @@
g_Serial_Signal.printf("C%d: %f\n", i, arg_C[i]);
#endif
+ //arg_t: solution that's expected to be solved in this function.
+ //t_sol: the solution that's actually wanted as Sprine's solution.
+ //t0_ival: _Sample_Set[ival].t
+ //arg_t = t_sol - t0_ival
+ //=>
+ //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
+ // = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
double c[3];
- //f(t) = arg_ft/arg_C[3]
- // = t^3 + c[2]*t^2 + c[1]*t + c[0].
+ //f(t) := arg_ft/arg_C[3]
+ // = arg_t^3 + c[2]*arg_t^2 + c[1]*arg_t + c[0].
for(int i = 0; i < 3; i++) {
c[i] = arg_C[i] / arg_C[3];
}
//modify the formula
- //t^3 + c[2]*t^2 + c[1]*t + (c[0] - ft) = 0.
+ //t^3 + c[2]*t^2 + c[1]*t + (c[0] - f(t)) = 0.
c[0] -= arg_ft / arg_C[3];
#ifdef DEBUG_SOLVE
for(int i = 0; i < 3; i++)
g_Serial_Signal.printf("c%d: %f\n", i, c[i]);
#endif
+ std::complex<double> d;
+ //d := c[2] / 3
+ //T := t + d (== arg_t - t_iavl + c[2]/3)
+ d = std::complex<double>(c[2] / 3.0, 0.0);
+ //=> T^3 + 3pT + 2q = 0.
+ double p,q;
//The values defined from coefficients of the formula
//that identify solutions
- double p,q,d;
- p = ( -pow(c[2], 2) + 3 * c[1]) / 9;
- q = (2 * pow(c[2], 3) - 9 * c[2] * c[1] + 27 * c[0]) / 54;
- d = - c[2] / 3;
+ p = ( -pow(c[2], 2.0) + 3.0 * c[1]) / 9.0;
+ q = (2.0 * pow(c[2], 3.0) - 9.0 * c[2] * c[1] + 27.0 * c[0]) / 54.0;
//Discriminant section
double D;
- D = pow(p, 3) + pow(q, 2);
+ D = pow(p, 3.0) + pow(q, 2.0);
#ifdef DEBUG_SOLVE
g_Serial_Signal.printf("p: %f\n", p);
g_Serial_Signal.printf("q: %f\n", q);
- g_Serial_Signal.printf("d: %f\n", d);
g_Serial_Signal.printf("D: %f\n", D);
#endif
+ //For all T, u, there exsists v: T = u + v; increment degree of freedom.
+ //Futhermore,
+ // u^3 + v^3 - 2q = 0
+ // uv + p = 0 <=> u^3v^3 = -p^3
+ //those because: T = u + v, T^3 + 3pT + 2q = 0,
+ //=> (u + v)^3 + 3p(u + v) + 2q = 0
+ //=> u^3 + v^3 + 3(uv + p)(u+v) + 2q = 0
//The values defined from p and q
//that idetify solutions
std::complex<double> u,v;
-
//Real root only
- if(D <= 0) {
+ if(D <= 0.0) {
u = std::complex<double>(-q, sqrt(-D));
v = std::complex<double>(-q,-sqrt(-D));
//u = pow(u, 1/3);
//v = pow(v, 1/3);
u = std::exp(std::log(u)/std::complex<double>(3.0,0.0));
- v = std::exp(std::log(u)/std::complex<double>(3.0,0.0));
+ v = std::exp(std::log(v)/std::complex<double>(3.0,0.0));
}
//One real root and two complex root
else {
@@ -393,19 +429,20 @@
#ifdef DEBUG_SOLVE
g_Serial_Signal.printf("u: %f + (%f)i\n", u.real(), u.imag());
g_Serial_Signal.printf("v: %f + (%f)i\n", v.real(), v.imag());
+ g_Serial_Signal.printf("d: %f + (%f)i\n", d.real(), d.imag());
#endif
//Cubic root of 1
std::complex<double> omega[3]= {
std::complex<double>( 1.0, 0.0),
- std::complex<double>(-1/2, sqrt(3.0)/2),
- std::complex<double>(-1/2,-sqrt(3.0)/2)
+ std::complex<double>(-1.0/2.0, sqrt(3.0)/2.0),
+ std::complex<double>(-1.0/2.0,-sqrt(3.0)/2.0)
};
//Solution of the formula
- arg_t[0] = omega[0] * u + omega[0] * v + d;
- arg_t[1] = omega[1] * u + omega[2] * v + d;
- arg_t[2] = omega[2] * u + omega[1] * v + d;
+ arg_t[0] = omega[0] * u + omega[0] * v - d;
+ arg_t[1] = omega[1] * u + omega[2] * v - d;
+ arg_t[2] = omega[2] * u + omega[1] * v - d;
#ifdef DEBUG_SOLVE
for(int i = 0; i < 3; i++)
@@ -435,20 +472,27 @@
for(int ival = 0; ival < _Sample_Num - 1; ival++) {
for(int i = 0; i < 4; i++) C[i] = _C_y[i][ival];
_solve_cubic_f(t_sol, C, arg_y);
+ for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
#ifdef DEBUG_GETX
- g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival+1].t);
+ g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
#endif
for(int i = 0; i < 3; i++) {
// regarding only real solution
// acuracy (error range) is supposed +-10E-3 here(groundless)
if(std::abs(t_sol[i].imag()) < 0.000001) {
- /* if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
- t_real.push_back(_Sample_Set[ival].t);
+ /* */ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
+ t_real.push_back(t_sol[i].real());
t_ival.push_back(ival);
+#ifdef DEBUG_GETX
+ g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
} else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
- t_real.push_back(_Sample_Set[ival + 1].t);
+ t_real.push_back(t_sol[i].real());
t_ival.push_back(ival);
- } else*/ if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
+#ifdef DEBUG_GETX
+ g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
+ } else if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival + 1].t) {
t_real.push_back(t_sol[i].real());
t_ival.push_back(ival);
#ifdef DEBUG_GETX
@@ -459,7 +503,7 @@
}
}
- if(t_real.size() > 0) {
+ if(!t_real.empty()) {
the_t = t_real[0];
the_i = t_ival[0];
//if t's size is bigger than 1
@@ -478,6 +522,9 @@
if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
the_i = i;
}
+ /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
+ else if (_Sample_Set[_Sample_Num - 1].t <= the_t) the_t = _Sample_Set[_Sample_Num - 1].t;
+
for(int i = 0; i < 4; i++) C[i] = _C_x[i][the_i];
x = _cubic_f(the_t - _Sample_Set[the_i].t, C);
#ifdef DEBUG_GETX
@@ -502,6 +549,7 @@
#ifdef DEBUG_GETY
g_Serial_Signal.printf(DEBUG_GETY);
+ g_Serial_Signal.printf("arg_x: %f\n", arg_x);
#endif
// For the every Intervals of Spline,
//it solves the polynomial defined by C[i] of the interval,
@@ -512,20 +560,36 @@
for(int ival = 0; ival < _Sample_Num - 1; ival++) {
for(int i = 0; i < 4; i++) C[i] = _C_x[i][ival];
_solve_cubic_f(t_sol, C, arg_x);
+ for(int i = 0; i < 3; i++) t_sol[i] += _Sample_Set[ival].t;
+ //arg_ft = C[3](t_sol - t0_ival)^3 + C[2](t_sol - t0_ival)^2 + C[1](t_sol - t0_ival) + C[0]
+ // = C[3]arg_t^3 + C[2]arg_t^2 + C[1]arg_t + C[0]
#ifdef DEBUG_GETY
- g_Serial_Signal.printf("interval:%d \t %f < t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival+1].t);
+ g_Serial_Signal.printf("interval:%d \t %f <= t < %f\n", ival, _Sample_Set[ival].t, _Sample_Set[ival + 1].t);
+ for(int i = 0; i < 3; i++)
+ g_Serial_Signal.printf("t%d \t %f + (%f)i\n", i, t_sol[i].real(), t_sol[i].imag());
#endif
for(int i = 0; i < 3; i++) {
// regarding only real solution
- // acuracy (error range) is supposed +-10E-3 here(groundless)
+ // acuracy (error range) is supposed +-10E-6 here(groundless)
if(std::abs(t_sol[i].imag()) < 0.000001) {
- /* if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
- t_real.push_back(_Sample_Set[ival].t);
+ /**/ if (ival == 0 && t_sol[i].real() < _Sample_Set[ival].t) {
+ t_real.push_back(t_sol[i].real());
t_ival.push_back(ival);
- } else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
- t_real.push_back(_Sample_Set[ival + 1].t);
+#ifdef DEBUG_GETY
+ g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
+ }//
+ else if (ival == _Sample_Num - 2 && _Sample_Set[ival + 1].t <= t_sol[i].real()) {
+ t_real.push_back(t_sol[i].real());
t_ival.push_back(ival);
- } else*/ if (_Sample_Set[ival].t <= t_sol[i].real() && t_sol[i].real() < _Sample_Set[ival+1].t) {
+#ifdef DEBUG_GETY
+ g_Serial_Signal.printf("(t, i) = (%f, %d)\n", t_real[t_real.size() - 1], ival);
+#endif
+ }
+ // There sometimes be a so fucking small error in solutions,
+ // so acuracy is set at 10E-6(groundless)
+ else if (static_cast<int>(_Sample_Set[ival].t * std::pow(10., 6.)) <= static_cast<int>(t_sol[i].real() * std::pow(10., 6.))
+ && static_cast<int>(t_sol[i].real() * std::pow(10., 6.)) < static_cast<int>(_Sample_Set[ival + 1].t * std::pow(10., 6.))) {
t_real.push_back(t_sol[i].real());
t_ival.push_back(ival);
#ifdef DEBUG_GETY
@@ -536,7 +600,7 @@
}
}
- if(t_real.size() > 0) {
+ if(!t_real.empty()) {
the_t = t_real[0];
the_i = t_ival[0];
//if t's size is bigger than 1
@@ -546,7 +610,9 @@
the_i = t_ival[i];
}
}
- } else {
+ }
+ //if no solution muched due to any errors
+ else {
#ifdef DEBUG_GETY
g_Serial_Signal.printf("LastPoint\n");
#endif
@@ -555,6 +621,12 @@
if(_Sample_Set[i].t <= the_t && the_t <= _Sample_Set[i+1].t)
the_i = i;
}
+
+ //
+ /* */if (the_t < _Sample_Set[0].t) the_t = _Sample_Set[0].t;
+ else if (_Sample_Set[_Sample_Num - 1].t < the_t) the_t = _Sample_Set[_Sample_Num - 1].t;
+
+ //
for(int i = 0; i < 4; i++) C[i] = _C_y[i][the_i];
y = _cubic_f(the_t - _Sample_Set[the_i].t, C);
#ifdef DEBUG_GETY
@@ -567,26 +639,6 @@
}
-void CubicSpline2d::calibrateSensor()
-{
- double t[_Sample_Num];
- double ft[_Sample_Num];
-
- _sampleData();
- _Last_Point = _Sample_Set[0];
-
- for(int i = 0; i < _Sample_Num; i++) {
- t[i] = _Sample_Set[i].t;
- ft[i]= _Sample_Set[i].x;
- }
- _makeModel(t,ft,_C_x);
-
- for(int i = 0; i < _Sample_Num; i++) {
- ft[i]= _Sample_Set[i].y;
- }
- _makeModel(t,ft,_C_y);
-
-}
void CubicSpline2d::saveSetting()
{
@@ -762,19 +814,23 @@
void CubicSpline2d::printOutData()
{
FILE *fp;
- double x,y;
- double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100;
+ double x,y,t;
+ double d = (_Sample_Set[_Sample_Num - 1].x - _Sample_Set[0].x) / 100.0;
fp = fopen("/local/log.txt", "w"); // open file in writing mode
fprintf(fp, "x, y, (t)\n");
for(int ival = 0; ival < _Sample_Num - 1; ival++) {
fprintf(fp, "ival: %d \n", ival);
- for(x = _Sample_Set[ival].x; x < _Sample_Set[ival + 1].x; x += d) {
+ for(x = _Sample_Set[ival].x; x <= _Sample_Set[ival + 1].x; x += d) {
y = getY(x);
- fprintf(fp, "%f,%f,(%f)\n", x, y, sqrt(x*x + y*y));
+ if(ival == 0)
+ t = sqrt((x - _Sample_Set[ival].x)*(x - _Sample_Set[ival].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
+ else
+ t = _Sample_Set[ival-1].t + sqrt((x - _Sample_Set[ival-1].x)*(x - _Sample_Set[ival-1].x) + (x - _Sample_Set[ival].y)*(x - _Sample_Set[ival].y));
+ fprintf(fp, "%f,%f,(%f)\n", x, y, t);
}
- fprintf(fp, "ival: %d \n", ival);
+ fprintf(fp, "-----------------------------------------\n");
}
fprintf(fp, "\nSample:dst, vol\n");