First Derivative in C++?

[CroCo]

I have data represents positions(x, y, z) as a double array. I want to calculate x', y', z' which represent the velocities. I'm dealing with real time project, so I need efficient way to calculate the derivative of stream data from a file or coming from another computer over the Internet. I've read a chapter about numerical derivatives. It seems that there are a lot of things should be concerned for implementing the derivatives. I need a library for this purpose. Any suggestions?

[naraku9333]

You take a derivative of a function not a value.

[CroCo]

@naraku9333,
Yes you are right but I don't differentiate a value (it gonna be zero) however, I have a set of data which represents a function. I have done it with Matlab and it gave me the velocities with perfect results however, I need to do the exact process in C++. Reading data which represents position from a file and calculate the first derivative.

[a k n]

Is that data dx,dy,dy (an infinitesimal change) , if so then you can just divide it by a very small number say epsilon = 0.1e-10 or even 0.1e-18.
Does that helps?
Please be more specific about your needs.
This might be of your interest .
https://projects.coin-or.org/CppAD

@naruku
you can differentiate value,they are called differentials.

[CroCo]

@amhndu,
Thank you,
Actually, I don't know the analytical form of the function. I have a device that gives me position of x, y, z with a given time ( from starting the application until stopping it). So, I need numerical estimation for the derivative of this function which represents the velocities dx, dy, dz ( with assuming the time is given).

what do you mean by saying divide them by a very small number? With Matlab, it is trivial. I need only to invoke diff() and this function returns the first derivative of the function without knowing the analytical form of it.

[Alrededor]

Have you tried something as simple as:

dx/dt = lim ( f(x) - f(x-t) ) / t as t→0

if your sequence of x values is x_n then

f'(x_n) is approximately ( f(x_n) - f(x_n-1) ) / t where t is the time difference between your samples.

[a k n]

so, the 'cppad' doesnt prove useful

As alrededor said you could use the first principle method


what i meant by divide was :

//pseudo code:
epsilon = 0.1e-10; //say

dx = posx(t + epsilon) - posx(t);
dx/dt = dx / epsilon;

[mik2718]

Assuming the positions represent equal spaced time points separated by delta_t

Then the velocity derivative will be approximated by

( (x_n+1 - x_n)/delta_t, (y_n+1-y_n)/delta_t, (z_n+1-z_n)/delta_t )

You can check if matlab is in fact doing this simple calculation

[CroCo]

@Alrededor,

My device is running at rate 1kHz.
Every 1ms I'm getting new data. I have f(xn) and t, but what about f(x_n-1)? Does it represent the last data? My understanding for n-1 is the last data.
Let's say f(xn) = 0.212 and after 1ms I've got f(xn) = 1.234. Now
f(x_n-1) = 0.212
f(xn) = 1.234
t = 1m
f'(xn) = ( 1.234 - 0.212 ) / 1m ??? I'm getting huge data.


@amhndu,
I'm still confused with cppad and I need a little time to use it properly. But if the case as you mentioned, so why I need to use it?

@mik2718,
So in my case the delta_t = 1/1000 ??

[Thumper]

CroCo wrote:

f'(xn) = ( 1.234 - 0.212 ) / 1m ??? I'm getting huge data.

What are you dividing by here? If you're getting new data every 1ms, you should be doing
f'(xn) = (1.234-0.212)/.0001 = 10,220.
f'(xn) = (1.234-0.212)/.001 = 1022 m/s. (Unit correction by Alreddor.)

If, like you said, you are receiving data every 1ms, and an object moves from a point 0.212m from the origin to 1.234m from the origin, it's velocity is very large (1ms is a very small amount of time.) So you should expect a large number if those two data points are truly 1ms apart.

[CroCo]

@Thumper,
Sorry my bad. Yes you are right since the difference is very very small in time from changing an object from position to another. About 1ms, this is what the manual of the device is saying about the servo Loop of the device

[Thumper]

Ah. Well it depends on what you're doing with that servo and what you're measuring as to how you calculate the time difference between two position points. Can you elaborate more on your project?

[CroCo]

@Thumper,
Basically, I have a device (manipulator) running with very high speed 1kHz. This device is supported with API that allows the user to get its info (positions of end-effector, velocities, joint angles, ...). I have stored positions in a file (x, y, z) and by Matlab I've got the first derivative which represents the velocities. Why do I need to calculate the velocities? I have a mathematical model and control algorithms to control another device(slave). Now, I need to send the positions of the Master and in the slave side I need to calculate the velocities which are required to the control algorithm. But I'm stuck with that in C++. I'm using C++ because I'm dealing with real time process.

[Alrededor]

Thumper has a decimal place wrong for the time. It should be t = .001

Even then the velocity would still be 1022 meters/sec. You would be breaking the sound barrier. Is it possible you have the wrong units on your positions? Centimeters or millimeters instead of meters?

The formula is velocity = ( currentPosition - previousPosition ) / timeInterval

[Thumper]

@Alrededor
Wooops. I'm terrible at unit conversions. :P
Thanks for catching that.

[Daleth]

Let's say f(xn) = 0.212 and after 1ms I've got f(xn) = 1.234. Now
f(x_n-1) = 0.212
f(xn) = 1.234
t = 1m[s]
f'(xn) = ( 1.234 - 0.212 ) / 1m[s] ??? I'm getting huge data.

What are the units for f(xn) ? You never mentioned that. Are the positions measured in meters? Nanometers? Megameters?

[andywestken]

(For a sequence of values x_n separated by time t, as before.)

Rather than using (Alg #1)

f'(x) = ( f(x_n) - f(x_n-1) ) / t

or the n/n+1 variant, you might want to consider using (Alg #2)

f'(x) = ( f(x_n+1) - f(x_n-1) ) / 2t

This gives better accuracy in the most part (it's the solution of the quadratric polynominal interpolation, which amounts in this case to taking the mean of the values calculated using n-1/n and n/n+1)

Or even better (Alg #3)

f'(x) = 4/3 (f(x_n+1) - f(x_n-1)) / 2t - 1/3 (f(x_n+2) - f(x_n-2)) / 4t

This comes from:

Computer Physics Communications 177 (2007) 764–774
Numerical differentiation of experimental data: local versus global methods
http://www.stat.physik.uni-potsdam.de/~kahnert/publications/COMPHY3395.pdf

Andy

PS A couple of illustrations: you can see that Alg #2 does better than Alg #1. And that Alg #3 does rather better.

Where
- Alg#1 = ( f(x_n) - f(x_n-1) ) / t
- Alg#2 = ( f(x_n+1) - f(x_n-1) ) / 2t
- Alg#3 = 4/3 (f(x_n+1) - f(x_n-1)) / 2t - 1/3 (f(x_n+2) - f(x_n-2)) / 4t

For
- f(x) = 4 + 3x² + 2x³ + x⁴ (input)
- f'(x) = 6x + 6x² + 4x³ (expect)
- t = 0.1

----------------------------------------------------------------------------------------------------------------------------
                              [Alg #1]                        [Alg #2]                        [Alg #3]                      
index       input     expect     output      delta   delta%      output      delta   delta%      output      delta   delta%   
----------------------------------------------------------------------------------------------------------------------------
    0      4.0000     0.0000   --------   --------    ------   --------   --------    ------   --------   --------    ------  
    1      4.0321     0.6640     0.3210  -3.4e-001  -51.657%     0.6880   2.4e-002    3.614%   --------   --------    ------  
    2      4.1376     1.4720     1.0550  -4.2e-001  -28.329%     1.5000   2.8e-002    1.902%     1.4720   4.0e-015    0.000%  
    3      4.3321     2.4480     1.9450  -5.0e-001  -20.547%     2.4800   3.2e-002    1.307%     2.4480   2.2e-015    0.000%  
    4      4.6336     3.6160     3.0150  -6.0e-001  -16.621%     3.6520   3.6e-002    0.996%     3.6160  -4.0e-015   -0.000%  
    5      5.0625     5.0000     4.2890  -7.1e-001  -14.220%     5.0400   4.0e-002    0.800%     5.0000  -8.9e-016   -0.000%  
    6      5.6416     6.6240     5.7910  -8.3e-001  -12.575%     6.6680   4.4e-002    0.664%     6.6240   1.8e-015    0.000%  
    7      6.3961     8.5120     7.5450  -9.7e-001  -11.360%     8.5600   4.8e-002    0.564%     8.5120  -3.6e-015   -0.000%  
    8      7.3536    10.6880     9.5750  -1.1e+000  -10.414%    10.7400   5.2e-002    0.487%    10.6880  -3.6e-015   -0.000%  
    9      8.5441    13.1760    11.9050  -1.3e+000   -9.646%    13.2320   5.6e-002    0.425%    13.1760  -7.1e-015   -0.000%  
   10     10.0000    16.0000    14.5590  -1.4e+000   -9.006%    16.0600   6.0e-002    0.375%    16.0000   7.1e-015    0.000%  
   11     11.7561    19.1840    17.5610  -1.6e+000   -8.460%    19.2480   6.4e-002    0.334%    19.1840   7.1e-015    0.000%  
   12     13.8496    22.7520    20.9350  -1.8e+000   -7.986%    22.8200   6.8e-002    0.299%    22.7520   3.6e-015    0.000%  
   13     16.3201    26.7280    24.7050  -2.0e+000   -7.569%    26.8000   7.2e-002    0.269%    26.7280  -7.1e-015   -0.000%  
   14     19.2096    31.1360    28.8950  -2.2e+000   -7.197%    31.2120   7.6e-002    0.244%    31.1360  -3.9e-014   -0.000%  
   15     22.5625    36.0000    33.5290  -2.5e+000   -6.864%    36.0800   8.0e-002    0.222%    36.0000   2.8e-014    0.000%  
   16     26.4256    41.3440    38.6310  -2.7e+000   -6.562%    41.4280   8.4e-002    0.203%    41.3440   2.8e-014    0.000%  
   17     30.8481    47.1920    44.2250  -3.0e+000   -6.287%    47.2800   8.8e-002    0.186%    47.1920  -1.4e-014   -0.000%  
   18     35.8816    53.5680    50.3350  -3.2e+000   -6.035%    53.6600   9.2e-002    0.172%    53.5680  -2.1e-014   -0.000%  
   19     41.5801    60.4960    56.9850  -3.5e+000   -5.804%    60.5920   9.6e-002    0.159%    60.4960  -6.4e-014   -0.000%  
   20     48.0000    68.0000    64.1990  -3.8e+000   -5.590%    68.1000   1.0e-001    0.147%    68.0000   0.0e+000    0.000%  
   21     55.2001    76.1040    72.0010  -4.1e+000   -5.391%    76.2080   1.0e-001    0.137%    76.1040   8.5e-014    0.000%  
   22     63.2416    84.8320    80.4150  -4.4e+000   -5.207%    84.9400   1.1e-001    0.127%    84.8320   2.8e-014    0.000%  
   23     72.1881    94.2080    89.4650  -4.7e+000   -5.035%    94.3200   1.1e-001    0.119%    94.2080   8.5e-014    0.000%  
   24     82.1056   104.2560    99.1750  -5.1e+000   -4.874%   104.3720   1.2e-001    0.111%   104.2560  -1.7e-013   -0.000%  
   25     93.0625   115.0000   109.5690  -5.4e+000   -4.723%   115.1200   1.2e-001    0.104%   115.0000  -2.0e-013   -0.000%  
   26    105.1296   126.4640   120.6710  -5.8e+000   -4.581%   126.5880   1.2e-001    0.098%   126.4640   1.4e-013    0.000%  
   27    118.3801   138.6720   132.5050  -6.2e+000   -4.447%   138.8000   1.3e-001    0.092%   138.6720   5.7e-014    0.000%  
   28    132.8896   151.6480   145.0950  -6.6e+000   -4.321%   151.7800   1.3e-001    0.087%   151.6480   1.1e-013    0.000%  
   29    148.7361   165.4160   158.4650  -7.0e+000   -4.202%   165.5520   1.4e-001    0.082%   165.4160  -2.8e-013   -0.000%  
   30    166.0000   180.0000   172.6390  -7.4e+000   -4.089%   180.1400   1.4e-001    0.078%   180.0000  -8.5e-014   -0.000% 

(Continued in next post...)

[andywestken]

(continued from previous post...)

And for
- f(x) = 5cos(x) + 3sin(2x) + cos(3x) (input)
- f'(x) = -5sin(x) + 6cos(x) - 3sin(3x) (expect)
- t = 0.01745... (ie 1 degree in radians)

-----------------------------------------------------------------------------------------------------------------
                         [Alg #1]                      [Alg #2]                      [Alg #3]                    
index    input   expect   output      delta   delta%    output      delta   delta%    output      delta   delta%   
-----------------------------------------------------------------------------------------------------------------
   22   7.1266  -0.2976  -0.1520   1.5e-001  -48.920%  -0.2972   4.7e-004   -0.158%  -0.2976   4.8e-007   -0.000%  
   23   7.1189  -0.5864  -0.4423   1.4e-001  -24.581%  -0.5859   5.3e-004   -0.091%  -0.5864   5.0e-007   -0.000%  
   24   7.1062  -0.8721  -0.7295   1.4e-001  -16.344%  -0.8715   5.9e-004   -0.068%  -0.8721   5.2e-007   -0.000%  
   25   7.0885  -1.1541  -1.0134   1.4e-001  -12.193%  -1.1535   6.5e-004   -0.056%  -1.1541   5.4e-007   -0.000%  
   26   7.0659  -1.4323  -1.2936   1.4e-001   -9.687%  -1.4316   7.0e-004   -0.049%  -1.4323   5.6e-007   -0.000%  
   27   7.0385  -1.7063  -1.5697   1.4e-001   -8.007%  -1.7056   7.5e-004   -0.044%  -1.7063   5.7e-007   -0.000%  
   28   7.0064  -1.9758  -1.8414   1.3e-001   -6.799%  -1.9750   8.0e-004   -0.041%  -1.9758   5.9e-007   -0.000%  
   29   6.9696  -2.2404  -2.1085   1.3e-001   -5.888%  -2.2396   8.5e-004   -0.038%  -2.2404   6.0e-007   -0.000%  
   30   6.9282  -2.5000  -2.3706   1.3e-001   -5.174%  -2.4991   8.9e-004   -0.036%  -2.5000   6.1e-007   -0.000%  
   31   6.8823  -2.7542  -2.6276   1.3e-001   -4.599%  -2.7533   9.3e-004   -0.034%  -2.7542   6.2e-007   -0.000%  
   32   6.8321  -3.0029  -2.8791   1.2e-001   -4.125%  -3.0020   9.6e-004   -0.032%  -3.0029   6.3e-007   -0.000%  
   33   6.7776  -3.2458  -3.1249   1.2e-001   -3.727%  -3.2448   1.0e-003   -0.031%  -3.2458   6.3e-007   -0.000%  
   34   6.7188  -3.4828  -3.3648   1.2e-001   -3.387%  -3.4817   1.0e-003   -0.029%  -3.4828   6.3e-007   -0.000%  
   35   6.6560  -3.7135  -3.5987   1.1e-001   -3.093%  -3.7125   1.1e-003   -0.028%  -3.7135   6.3e-007   -0.000%  
   36   6.5892  -3.9380  -3.8263   1.1e-001   -2.836%  -3.9369   1.1e-003   -0.027%  -3.9380   6.3e-007   -0.000%  
   37   6.5186  -4.1560  -4.0475   1.1e-001   -2.610%  -4.1549   1.1e-003   -0.026%  -4.1560   6.3e-007   -0.000%  
   38   6.4442  -4.3674  -4.2623   1.1e-001   -2.408%  -4.3663   1.1e-003   -0.025%  -4.3674   6.2e-007   -0.000%  
   39   6.3662  -4.5722  -4.4703   1.0e-001   -2.227%  -4.5710   1.1e-003   -0.025%  -4.5722   6.2e-007   -0.000%  
   40   6.2846  -4.7701  -4.6717   9.8e-002   -2.063%  -4.7690   1.1e-003   -0.024%  -4.7701   6.1e-007   -0.000%  
   41   6.1997  -4.9613  -4.8663   9.5e-002   -1.915%  -4.9601   1.1e-003   -0.023%  -4.9613   6.0e-007   -0.000%  
   42   6.1115  -5.1455  -5.0540   9.2e-002   -1.779%  -5.1444   1.2e-003   -0.022%  -5.1455   5.9e-007   -0.000%  
   43   6.0201  -5.3229  -5.2348   8.8e-002   -1.655%  -5.3217   1.2e-003   -0.022%  -5.3229   5.7e-007   -0.000%  
   44   5.9257  -5.4933  -5.4087   8.5e-002   -1.541%  -5.4922   1.2e-003   -0.021%  -5.4933   5.6e-007   -0.000%  
   45   5.8284  -5.6569  -5.5757   8.1e-002   -1.435%  -5.6557   1.1e-003   -0.020%  -5.6569   5.4e-007   -0.000%  
   46   5.7283  -5.8135  -5.7357   7.8e-002   -1.337%  -5.8123   1.1e-003   -0.020%  -5.8135   5.2e-007   -0.000%  
   47   5.6255  -5.9633  -5.8889   7.4e-002   -1.246%  -5.9621   1.1e-003   -0.019%  -5.9633   5.0e-007   -0.000%  
   48   5.5202  -6.1063  -6.0353   7.1e-002   -1.162%  -6.1051   1.1e-003   -0.018%  -6.1063   4.8e-007   -0.000%  
   49   5.4124  -6.2425  -6.1749   6.8e-002   -1.082%  -6.2414   1.1e-003   -0.018%  -6.2425   4.6e-007   -0.000%  
   50   5.3023  -6.3721  -6.3079   6.4e-002   -1.008%  -6.3710   1.1e-003   -0.017%  -6.3721   4.4e-007   -0.000%  
   51   5.1900  -6.4952  -6.4342   6.1e-002   -0.939%  -6.4941   1.1e-003   -0.017%  -6.4952   4.1e-007   -0.000%  
   52   5.0756  -6.6118  -6.5540   5.8e-002   -0.874%  -6.6107   1.1e-003   -0.016%  -6.6118   3.9e-007   -0.000%  
   53   4.9593  -6.7221  -6.6675   5.5e-002   -0.813%  -6.7211   1.0e-003   -0.015%  -6.7221   3.6e-007   -0.000%  
   54   4.8410  -6.8262  -6.7747   5.2e-002   -0.755%  -6.8252   1.0e-003   -0.015%  -6.8262   3.4e-007   -0.000%  
   55   4.7210  -6.9243  -6.8758   4.9e-002   -0.701%  -6.9234   9.8e-004   -0.014%  -6.9243   3.1e-007   -0.000%  
   56   4.5994  -7.0166  -6.9709   4.6e-002   -0.650%  -7.0156   9.5e-004   -0.014%  -7.0166   2.8e-007   -0.000%  
   57   4.4761  -7.1031  -7.0603   4.3e-002   -0.602%  -7.1022   9.2e-004   -0.013%  -7.1031   2.5e-007   -0.000% 

[CroCo]

The unit of f(x_n) is millimetre.

@andywestken
I would like to implement this
- Alg#3 = 4/3 (f(xn+1) - f(xn-1)) / 2t - 1/3 (f(xn+2) - f(xn-2)) / 4t

In C++ programming, where should I start? Also, about t, I'm sending data over the Internet using UDP, so is it ok to set t as 0.001. I'm assuming there is no delay and there is no dropped packet. I will be happy at least to start the project then enhance it by considering the delay time and dropped packet.
This is my code

double FirstDerivative(double a[5])
{
    double firstDer(0);
    double t(0.001);
    for (int i = 0; i < 5; ++i)
    {
       firstDer =  ((double)(4/3*2*t))*(a[i+1] - a[i-1]) -
                   ((double)(1/3*4*t))*(a[i+2] - a[i-2]);
    }
}

[andywestken]

In C++

f'(x_n) = 4/3 (f(x_n+1) - f(x_n-1)) / 2t - 1/3 (f(x_n+2) - f(x_n-2)) / 4t

ends up as

double FirstDerivative(double currentData[5])
{
    double firstDer(0.0);
    double t(0.001);

    // no loop here as 5 points is just enough to calculate the derivative
    // of one elem (the middle one). If you do use a loop, it has to start
    // at 2 and end 2 elems before the end (so i - 2 and i + 2 are always
    // valid indexes for the array.)

    size_t i(2);

    firstDer = (   4.0 / 3.0 * (a[i + 1] - a[i - 1]) / (2.0 * t)
                 - 1.0 / 3.0 * (a[i + 2] - a[i - 2]) / (4.0 * t) );
    // Rather than use (double)4 you can just use 4.0. The .0 tells the
    // compiler you mean a double (if you need a float instead, append
    // an f, like 3.0f )

    return firstDer;
}

Andy