[Solved] Basic example of how to do numerical integration in C++

Question

Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls

Pay attention to where I use the words “estimate” vs “measurement” below. The difference is important.

Measurements are direct readings from a sensor.
1. Ex: a GPS measures position (meters) directly, and a speedometer measures speed (m/s) directly.
Estimates are calculated projections you can obtain through integrating and derivating (deriving) measured values.
Ex: you can derive position measurements (m) with respect to time to obtain speed or velocity (m/s) estimates, and you can integrate speed or velocity measurements (m/s) with respect to time to obtain position or displacement (m) estimates.
Wait, aren’t all “measurements” actually just “estimates” at some fundamental level?
1. Yeah–pretty much! But, they are not necessarily produced through derivations or integrations with respect to time, so that is a bit different.
2. Also note that technically, virtually nothing can truly be measured directly. All sensors get reduced down to a voltage or a current, and guess how you measure a current?–a voltage!–either as a voltage drop across a tiny resistance, or as a voltage induced through an inductive coil due to current flow. So, everything boils down to a voltage. Even devices which “measure speed directly” may be using pressure (pitot-static tube on airplane), doppler/phase shift (radar or sonar), or looking at distance over time and then outputting speed. Fluid speed, or speed with respect to fluid such as air or water, can even be measured via a hot wire anemometer by measuring the current required to keep a hot wire at a fixed temperature, or by measuring the temperature change of the hot wire at a fixed current. And how is that temperature measured? Temperature is just a thermo-electrically-generated voltage, or a voltage drop across a diode or other resistance.
3. As you can see, all of these “measurements” and “estimates”, at the low level, are intertwined. However, if a given device has been produced, tested, and calibrated to output a given “measurement”, then you can accept it as a “source of truth” for all practical purposes and call it a “measurement”. Then, anything you derive from that measurement, with respect to time or some other variable, you can consider an “estimate”. The irony of this is that if you calibrate your device and output derived or integrated estimates, someone else could then consider your output “estimates” as their input “measurements” in their system, in a sort of never-ending chain down the line. That’s being pedantic, however. Let’s just go with the simplified definitions I have above for the time being.

The following table is true, for example. Read the 2nd line, for instance, as: “If you take the derivative of a velocity measurement with respect to time, you get an acceleration estimate, and if you take its integral, you get a position estimate.”

Derivatives and integrals of position

Measurement, y              Derivative                  Integral
                            Estimate (dy/dt)            Estimate (dy*dt)
-----------------------     -----------------------     -----------------------
position        [m]         velocity        [m/s]       -               [m*s]
velocity        [m/s]       acceleration    [m/s^2]     position        [m]      
acceleration    [m/s^2]     jerk            [m/s^3]     velocity        [m/s]
jerk            [m/s^3]     snap            [m/s^4]     acceleration    [m/s^2]
snap            [m/s^4]     crackle         [m/s^5]     jerk            [m/s^3]
crackle         [m/s^5]     pop             [m/s^6]     snap            [m/s^4]
pop             [m/s^6]     -               [m/s^7]     crackle         [m/s^5]

For jerk, snap or jounce, crackle, and pop, see: https://en.wikipedia.org/wiki/Fourth,_fifth,_and_sixth_derivatives_of_position.

1. numerical derivation

Remember, derivation obtains the slope of the line, dy/dx, on an x-y plot. The general form is (y_new - y_old)/(x_new - x_old).

In order to obtain a velocity estimate from a system where you are obtaining repeated position measurements (ex: you are taking GPS readings periodically), you must numerically derivate your position measurements over time. Your y-axis is position, and your x-axis is time, so dy/dx is simply (position_new - position_old)/(time_new - time_old). A units check shows this might be meters/sec, which is indeed a unit for velocity.

In code, that would look like this, for a system where you’re only measuring position in 1-dimension:

double position_new_m = getPosition(); // m = meters
double position_old_m;
// `getNanoseconds()` should return a `uint64_t timestamp in nanoseconds, for
// instance
double time_new_sec = NS_TO_SEC((double)getNanoseconds());
double time_old_sec;

while (true)
{
    position_old_m = position_new_m;
    position_new_m = getPosition();

    time_old_sec = time_new_sec;
    time_new_sec = NS_TO_SEC((double)getNanoseconds());

    // Numerical derivation of position measurements over time to obtain
    // velocity in meters per second (mps)
    double velocity_mps = 
        (position_new_m - position_old_m)/(time_new_sec - time_old_sec);
}

2. numerical integration

Numerical integration obtains the area under the curve, dy*dx, on an x-y plot. One of the best ways to do this is called trapezoidal integration, where you take the average dy reading and multiply by dx. This would look like this: (y_old + y_new)/2 * (x_new - x_old).

In order to obtain a position estimate from a system where you are obtaining repeated velocity measurements (ex: you are trying to estimate distance traveled while only reading the speedometer on your car), you must numerically integrate your velocity measurements over time. Your y-axis is velocity, and your x-axis is time, so (y_old + y_new)/2 * (x_new - x_old) is simply velocity_old + velocity_new)/2 * (time_new - time_old). A units check shows this might be meters/sec * sec = meters, which is indeed a unit for distance.

In code, that would look like this. Notice that the numerical integration obtains the distance traveled over that one tiny time interval. To obtain an estimate of the total distance traveled, you must sum all of the individual estimates of distance traveled.

double velocity_new_mps = getVelocity(); // mps = meters per second
double velocity_old_mps;
// `getNanoseconds()` should return a `uint64_t timestamp in nanoseconds, for
// instance
double time_new_sec = NS_TO_SEC((double)getNanoseconds());
double time_old_sec;

// Total meters traveled
double distance_traveled_m_total = 0;

while (true)
{
    velocity_old_mps = velocity_new_mps;
    velocity_new_mps = getVelocity();

    time_old_sec = time_new_sec;
    time_new_sec = NS_TO_SEC((double)getNanoseconds());

    // Numerical integration of velocity measurements over time to obtain 
    // a distance estimate (in meters) over this time interval
    double distance_traveled_m = 
        (velocity_old_mps + velocity_new_mps)/2 * (time_new_sec - time_old_sec);
    distance_traveled_m_total += distance_traveled_m;
}

See also: https://en.wikipedia.org/wiki/Numerical_integration.

Going further:

high-resolution timestamps

To do the above, you’ll need a good way to obtain timestamps. Here are various techniques I use:

In C++, use my uint64_t nanos() function here.

If using Linux in C or C++, use my uint64_t nanos() function which uses clock_gettime() here. Even better, I have wrapped it up into a nice timinglib library for Linux, in my eRCaGuy_hello_world repo here:

Here is the NS_TO_SEC() macro from timing.h:

#define NS_PER_SEC (1000000000L)
/// Convert nanoseconds to seconds
#define NS_TO_SEC(ns)   ((ns)/NS_PER_SEC)

If using a microcontroller, you’ll need to read an incrementing periodic counter from a timer or counter register which you have configured to increment at a steady, fixed rate. Ex: on Arduino: use micros() to obtain a microsecond timestamp with 4-us resolution (by default, it can be changed). On STM32 or others, you’ll need to configure your own timer/counter.

use high data sample rates

Taking data samples as fast as possible in a sample loop is a good idea, because then you can average many samples to achieve:

Reduced noise: averaging many raw samples reduces noise from the sensor.
Higher-resolution: averaging many raw samples actually adds bits of resolution in your measurement system. This is known as oversampling.
1. I write about it on my personal website here: ElectricRCAircraftGuy.com: Using the Arduino Uno’s built-in 10-bit to 16+-bit ADC (Analog to Digital Converter).
2. And Atmel/Microchip wrote about it in their white-paper here: Application Note AN8003: AVR121: Enhancing ADC resolution by oversampling.
3. Taking 4^n samples increases your sample resolution by n bits of resolution. For example:
```
4^0 = 1    sample  at 10-bits resolution --> 1 10-bit sample
4^1 = 4    samples at 10-bits resolution --> 1 11-bit sample
4^2 = 16   samples at 10-bits resolution --> 1 12-bit sample
4^3 = 64   samples at 10-bits resolution --> 1 13-bit sample
4^4 = 256  samples at 10-bits resolution --> 1 14-bit sample
4^5 = 1024 samples at 10-bits resolution --> 1 15-bit sample
4^6 = 4096 samples at 10-bits resolution --> 1 16-bit sample
```
  See:

So, sampling at high sample rates is good. You can do basic filtering on these samples.

If you process raw samples at a high rate, doing numerical derivation on high-sample-rate raw samples will end up derivating a lot of noise, which produces noisy derivative estimates. This isn’t great. It’s better to do the derivation on filtered samples: ex: the average of 100 or 1000 rapid samples. Doing numerical integration on high-sample-rate raw samples, however, is fine, because as Edgar Bonet says, “when integrating, the more samples you get, the better the noise averages out.” This goes along with my notes above.

Just using the filtered samples for both numerical integration and numerical derivation, however, is just fine.

use reasonable control loop rates

Control loop rates should not be too fast. The higher the sample rates, the better, because you can filter them to reduce noise. The higher the control loop rate, however, not necessarily the better, because there is a sweet spot in control loop rates. If your control loop rate is too slow, the system will have a slow frequency response and won’t respond to the environment fast enough, and if the control loop rate is too fast, it ends up just responding to sample noise instead of to real changes in the measured data.

Therefore, even if you have a sample rate of 1 kHz, for instance, to oversample and filter the data, control loops that fast are not needed, as the noise from readings of real sensors over very small time intervals will be too large. Use a control loop anywhere from 10 Hz ~ 100 Hz, perhaps up to 400+ Hz for simple systems with clean data. In some scenarios you can go faster, but 50 Hz is very common in control systems. The more-complicated the system and/or the more-noisy the sensor measurements, generally, the slower the control loop must be, down to about 1~10 Hz or so. Self-driving cars, for instance, which are very complicated, frequently operate at control loops of only 10 Hz.

loop timing and multi-tasking

In order to accomplish the above, independent measurement and filtering loops, and control loops, you’ll need a means of performing precise and efficient loop timing and multi-tasking.

If needing to do precise, repetitive loops in Linux in C or C++, use the sleep_until_ns() function from my timinglib above. I have a demo of my sleep_until_us() function in-use in Linux to obtain repetitive loops as fast as 1 KHz to 100 kHz here.

If using bare-metal (no operating system) on a microcontroller as your compute platform, use timestamp-based cooperative multitasking to perform your control loop and other loops such as measurements loops, as required. See my detailed answer here: How to do high-resolution, timestamp-based, non-blocking, single-threaded cooperative multi-tasking.

full, numerical integration and multi-tasking example

I have an in-depth example of both numerical integration and cooperative multitasking on a bare-metal system using my CREATE_TASK_TIMER() macro in my Full coulomb counter example in code. That’s a great demo to study, in my opinion.

Kalman filters

For robust measurements, you’ll probably need a Kalman filter, perhaps an “unscented Kalman Filter,” or UKF, because apparently they are “unscented” because they “don’t stink.”

Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls

Pay attention to where I use the words “estimate” vs “measurement” below. The difference is important.

Measurements are direct readings from a sensor.
1. Ex: a GPS measures position (meters) directly, and a speedometer measures speed (m/s) directly.
Estimates are calculated projections you can obtain through integrating and derivating (deriving) measured values.
Ex: you can derive position measurements (m) with respect to time to obtain speed or velocity (m/s) estimates, and you can integrate speed or velocity measurements (m/s) with respect to time to obtain position or displacement (m) estimates.
Wait, aren’t all “measurements” actually just “estimates” at some fundamental level?
1. Yeah–pretty much! But, they are not necessarily produced through derivations or integrations with respect to time, so that is a bit different.
2. Also note that technically, virtually nothing can truly be measured directly. All sensors get reduced down to a voltage or a current, and guess how you measure a current?–a voltage!–either as a voltage drop across a tiny resistance, or as a voltage induced through an inductive coil due to current flow. So, everything boils down to a voltage. Even devices which “measure speed directly” may be using pressure (pitot-static tube on airplane), doppler/phase shift (radar or sonar), or looking at distance over time and then outputting speed. Fluid speed, or speed with respect to fluid such as air or water, can even be measured via a hot wire anemometer by measuring the current required to keep a hot wire at a fixed temperature, or by measuring the temperature change of the hot wire at a fixed current. And how is that temperature measured? Temperature is just a thermo-electrically-generated voltage, or a voltage drop across a diode or other resistance.
3. As you can see, all of these “measurements” and “estimates”, at the low level, are intertwined. However, if a given device has been produced, tested, and calibrated to output a given “measurement”, then you can accept it as a “source of truth” for all practical purposes and call it a “measurement”. Then, anything you derive from that measurement, with respect to time or some other variable, you can consider an “estimate”. The irony of this is that if you calibrate your device and output derived or integrated estimates, someone else could then consider your output “estimates” as their input “measurements” in their system, in a sort of never-ending chain down the line. That’s being pedantic, however. Let’s just go with the simplified definitions I have above for the time being.

The following table is true, for example. Read the 2nd line, for instance, as: “If you take the derivative of a velocity measurement with respect to time, you get an acceleration estimate, and if you take its integral, you get a position estimate.”

Derivatives and integrals of position

Measurement, y              Derivative                  Integral
                            Estimate (dy/dt)            Estimate (dy*dt)
-----------------------     -----------------------     -----------------------
position        [m]         velocity        [m/s]       -               [m*s]
velocity        [m/s]       acceleration    [m/s^2]     position        [m]      
acceleration    [m/s^2]     jerk            [m/s^3]     velocity        [m/s]
jerk            [m/s^3]     snap            [m/s^4]     acceleration    [m/s^2]
snap            [m/s^4]     crackle         [m/s^5]     jerk            [m/s^3]
crackle         [m/s^5]     pop             [m/s^6]     snap            [m/s^4]
pop             [m/s^6]     -               [m/s^7]     crackle         [m/s^5]

For jerk, snap or jounce, crackle, and pop, see: https://en.wikipedia.org/wiki/Fourth,_fifth,_and_sixth_derivatives_of_position.

1. numerical derivation

Remember, derivation obtains the slope of the line, dy/dx, on an x-y plot. The general form is (y_new - y_old)/(x_new - x_old).

In order to obtain a velocity estimate from a system where you are obtaining repeated position measurements (ex: you are taking GPS readings periodically), you must numerically derivate your position measurements over time. Your y-axis is position, and your x-axis is time, so dy/dx is simply (position_new - position_old)/(time_new - time_old). A units check shows this might be meters/sec, which is indeed a unit for velocity.

In code, that would look like this, for a system where you’re only measuring position in 1-dimension:

double position_new_m = getPosition(); // m = meters
double position_old_m;
// `getNanoseconds()` should return a `uint64_t timestamp in nanoseconds, for
// instance
double time_new_sec = NS_TO_SEC((double)getNanoseconds());
double time_old_sec;

while (true)
{
    position_old_m = position_new_m;
    position_new_m = getPosition();

    time_old_sec = time_new_sec;
    time_new_sec = NS_TO_SEC((double)getNanoseconds());

    // Numerical derivation of position measurements over time to obtain
    // velocity in meters per second (mps)
    double velocity_mps = 
        (position_new_m - position_old_m)/(time_new_sec - time_old_sec);
}

2. numerical integration

Numerical integration obtains the area under the curve, dy*dx, on an x-y plot. One of the best ways to do this is called trapezoidal integration, where you take the average dy reading and multiply by dx. This would look like this: (y_old + y_new)/2 * (x_new - x_old).

In order to obtain a position estimate from a system where you are obtaining repeated velocity measurements (ex: you are trying to estimate distance traveled while only reading the speedometer on your car), you must numerically integrate your velocity measurements over time. Your y-axis is velocity, and your x-axis is time, so (y_old + y_new)/2 * (x_new - x_old) is simply velocity_old + velocity_new)/2 * (time_new - time_old). A units check shows this might be meters/sec * sec = meters, which is indeed a unit for distance.

In code, that would look like this. Notice that the numerical integration obtains the distance traveled over that one tiny time interval. To obtain an estimate of the total distance traveled, you must sum all of the individual estimates of distance traveled.

double velocity_new_mps = getVelocity(); // mps = meters per second
double velocity_old_mps;
// `getNanoseconds()` should return a `uint64_t timestamp in nanoseconds, for
// instance
double time_new_sec = NS_TO_SEC((double)getNanoseconds());
double time_old_sec;

// Total meters traveled
double distance_traveled_m_total = 0;

while (true)
{
    velocity_old_mps = velocity_new_mps;
    velocity_new_mps = getVelocity();

    time_old_sec = time_new_sec;
    time_new_sec = NS_TO_SEC((double)getNanoseconds());

    // Numerical integration of velocity measurements over time to obtain 
    // a distance estimate (in meters) over this time interval
    double distance_traveled_m = 
        (velocity_old_mps + velocity_new_mps)/2 * (time_new_sec - time_old_sec);
    distance_traveled_m_total += distance_traveled_m;
}

See also: https://en.wikipedia.org/wiki/Numerical_integration.

Going further:

high-resolution timestamps

To do the above, you’ll need a good way to obtain timestamps. Here are various techniques I use:

In C++, use my uint64_t nanos() function here.

If using Linux in C or C++, use my uint64_t nanos() function which uses clock_gettime() here. Even better, I have wrapped it up into a nice timinglib library for Linux, in my eRCaGuy_hello_world repo here:

Here is the NS_TO_SEC() macro from timing.h:

#define NS_PER_SEC (1000000000L)
/// Convert nanoseconds to seconds
#define NS_TO_SEC(ns)   ((ns)/NS_PER_SEC)

If using a microcontroller, you’ll need to read an incrementing periodic counter from a timer or counter register which you have configured to increment at a steady, fixed rate. Ex: on Arduino: use micros() to obtain a microsecond timestamp with 4-us resolution (by default, it can be changed). On STM32 or others, you’ll need to configure your own timer/counter.

use high data sample rates

Taking data samples as fast as possible in a sample loop is a good idea, because then you can average many samples to achieve:

Reduced noise: averaging many raw samples reduces noise from the sensor.
Higher-resolution: averaging many raw samples actually adds bits of resolution in your measurement system. This is known as oversampling.
1. I write about it on my personal website here: ElectricRCAircraftGuy.com: Using the Arduino Uno’s built-in 10-bit to 16+-bit ADC (Analog to Digital Converter).
2. And Atmel/Microchip wrote about it in their white-paper here: Application Note AN8003: AVR121: Enhancing ADC resolution by oversampling.
3. Taking 4^n samples increases your sample resolution by n bits of resolution. For example:
```
4^0 = 1    sample  at 10-bits resolution --> 1 10-bit sample
4^1 = 4    samples at 10-bits resolution --> 1 11-bit sample
4^2 = 16   samples at 10-bits resolution --> 1 12-bit sample
4^3 = 64   samples at 10-bits resolution --> 1 13-bit sample
4^4 = 256  samples at 10-bits resolution --> 1 14-bit sample
4^5 = 1024 samples at 10-bits resolution --> 1 15-bit sample
4^6 = 4096 samples at 10-bits resolution --> 1 16-bit sample
```
  See:

So, sampling at high sample rates is good. You can do basic filtering on these samples.

If you process raw samples at a high rate, doing numerical derivation on high-sample-rate raw samples will end up derivating a lot of noise, which produces noisy derivative estimates. This isn’t great. It’s better to do the derivation on filtered samples: ex: the average of 100 or 1000 rapid samples. Doing numerical integration on high-sample-rate raw samples, however, is fine, because as Edgar Bonet says, “when integrating, the more samples you get, the better the noise averages out.” This goes along with my notes above.

Just using the filtered samples for both numerical integration and numerical derivation, however, is just fine.

use reasonable control loop rates

Control loop rates should not be too fast. The higher the sample rates, the better, because you can filter them to reduce noise. The higher the control loop rate, however, not necessarily the better, because there is a sweet spot in control loop rates. If your control loop rate is too slow, the system will have a slow frequency response and won’t respond to the environment fast enough, and if the control loop rate is too fast, it ends up just responding to sample noise instead of to real changes in the measured data.

Therefore, even if you have a sample rate of 1 kHz, for instance, to oversample and filter the data, control loops that fast are not needed, as the noise from readings of real sensors over very small time intervals will be too large. Use a control loop anywhere from 10 Hz ~ 100 Hz, perhaps up to 400+ Hz for simple systems with clean data. In some scenarios you can go faster, but 50 Hz is very common in control systems. The more-complicated the system and/or the more-noisy the sensor measurements, generally, the slower the control loop must be, down to about 1~10 Hz or so. Self-driving cars, for instance, which are very complicated, frequently operate at control loops of only 10 Hz.

loop timing and multi-tasking

In order to accomplish the above, independent measurement and filtering loops, and control loops, you’ll need a means of performing precise and efficient loop timing and multi-tasking.

If needing to do precise, repetitive loops in Linux in C or C++, use the sleep_until_ns() function from my timinglib above. I have a demo of my sleep_until_us() function in-use in Linux to obtain repetitive loops as fast as 1 KHz to 100 kHz here.

If using bare-metal (no operating system) on a microcontroller as your compute platform, use timestamp-based cooperative multitasking to perform your control loop and other loops such as measurements loops, as required. See my detailed answer here: How to do high-resolution, timestamp-based, non-blocking, single-threaded cooperative multi-tasking.

full, numerical integration and multi-tasking example

I have an in-depth example of both numerical integration and cooperative multitasking on a bare-metal system using my CREATE_TASK_TIMER() macro in my Full coulomb counter example in code. That’s a great demo to study, in my opinion.

Kalman filters

For robust measurements, you’ll probably need a Kalman filter, perhaps an “unscented Kalman Filter,” or UKF, because apparently they are “unscented” because they “don’t stink.”