0

I have a data point array. Which is recorded at 20Hz(0.05 second. It can be 30Hz, 40Hz, 50Hz. 20Hz is an example value)

little t

I want to interpolate this data to bigger frequency for example 1kHz(0.001 second) with cubic interpolation to get smooth data set. y(t) = at^3 + bt^2 + ct + d

little t

But I can't figure out how can I derive the function and implement with C.

sphinxlike
  • 101
  • 1

2 Answers2

1

Maybe you are looking for spline interpolation such as Cubic Hermite Splines, etc. However, it really depends on how you want you path looks.

  • If it is something like enter image description here

and you want something of third order (specifically, a velocity profile of third order), you can solve for a function $P_{01}(t) = at^3 + bt^2 + ct + d$ which satisfies, for example, $$ \begin{align} P_{01}(t_0) &= x_0\\ P_{01}(t_0) &= x_1\\ P_{01}'(t_0) &= 0\\ P_{01}'(t_1) &= 0\\ P_{01}''(t_0) &= 0\\ P_{01}''(t_1) &= 0, \end{align} $$ where $P'$ indicates the first time derivative of $P$, etc. You can substitute all the boundary conditions above to the function and solve for all the coefficients $a$, $b$, $c$, and $d$. You may want to have a look at polynomial interpolation and Vandermonde matrix. Note that if you change boundary conditions, you can also make the path looks smooth (i.e., not so zigzaggy).

  • If you just want a straight line between two consecutive points, you can even do $$ P_{01}(t) = x_0 + \left(\frac{x_1 - x_0}{t_1 - t_0}\right)(t - t_0). $$

You don't need to use higher-order polynomials.

  • If you, instead, want something like enter image description here

Spline interpolation might be more suitable.

Petch Puttichai
  • 1,839
  • 1
  • 10
  • 23
0

As I understand it, you want to fit a degree three polynomial to course data to be able to use the polynomial to interpolate between the course data points.

To be able to fit the coefficients ($a$, $b$, $c$, and $d$) of the degree three polynomial so that it goes through the original data points, you need exactly four data points, $(t_1,y_1), \cdots, (t_4,y_4)$. If you have more than four data points, the polynomial values will not necessarily go through the original data points.

Edit: Calculation of Polynomial Coefficients

For four sequential data points ($i,i+1,i+2,i+3$), the following equations must hold: \begin{eqnarray} at_i^3 +bt_i^2 +ct_i + d &=& y_i \\ at_{i+1}^3 +bt_{i+1}^2 +ct_{i+1} + d &=& y_{i+1} \\ at_{i+2}^3 +bt_{i+2}^2 +ct_{i+2} + d &=& y_{i+2} \\ at_{i+3}^3 +bt_{i+3}^2 +ct_{i+3} + d &=& y_{i+3} \end{eqnarray} This can be written as a matrix-vector equation $$ \left[\begin{array}{llll} t_i^3 & t_i^2 & t_i & 1 \\ t_{i+1}^3 & t_{i+1}^2 & t_{i+1} & 1 \\ t_{i+2}^3 & t_{i+2}^2 & t_{i+2} & 1 \\ t_{i+3}^3 & t_{i+3}^2 & t_{i+3} & 1 \end{array}\right] \left[\begin{array}{c} a \\ b \\ c \\ d \end{array}\right] = \left[\begin{array}{l} y_i \\ y_{i+1} \\ y_{i+2} \\ y_{i+3} \end{array}\right] $$ which can be solved for the vector of unknown coefficients ($a$, $b$, $c$, and $d$) using $$ \left[\begin{array}{c} a \\ b \\ c \\ d \end{array}\right] = \left[\begin{array}{llll} t_i^3 & t_i^2 & t_i & 1 \\ t_{i+1}^3 & t_{i+1}^2 & t_{i+1} & 1 \\ t_{i+2}^3 & t_{i+2}^2 & t_{i+2} & 1 \\ t_{i+3}^3 & t_{i+3}^2 & t_{i+3} & 1 \end{array}\right]^{-1} \left[\begin{array}{l} y_i \\ y_{i+1} \\ y_{i+2} \\ y_{i+3} \end{array}\right] $$ These coefficients can then be used to calculate the value of $y$ for a given $t$ where $t_i<t<t_{i+3}$: $$ y = at_3 + bt^2 +ct +d $$

I would recommend using a cubic spline interpolation method so that you will get a smooth interpolating curve that goes through all the original data points and that can handle more that four data points. See Numerical Recipes 3rd Edition: The Art of Scientific Computing by Press, et al. for very good C or C++ code to perform cubic spline interpolation.

Christo
  • 385
  • 1
  • 6