Basic example of how to do numerical integration in code-CodePudding

I think most people know how to do numerical derivation in computer programming, (as limit --> 0; read: "as limit approaches zero").

//example code for derivation of position over time to obtain velocity

float currPosition, prevPosition, currTime, lastTime, velocity;

while (true)
{
    prevPosition = currPosition;
    currPosition = getNewPosition();

    lastTime = currTime;
    currTime = getTimestamp();

    // Numerical derivation of position over time to obtain velocity
    velocity = (currPosition - prevPosition)/(currTime - lastTime);
}

// since the while loop runs at the shortest period of time, we've already
// achieved limit --> 0;

This is the basic building block for most derivation programming.

How can I do this with integrals? Do I use a for loop and add or what?

CodePudding user response：

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.

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;
}

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)

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.

Realistically, however, control loops that fast are not needed, and the noise from readings of real sensors over such small time intervals will be too large. Use a control loop anywhere from 10 Hz ~ 100 Hz. Some people 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 10 Hz or so.

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."