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

I think most people know how to do numerical derivation in computer programming, (as limit --> 0; read: "as the 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.

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.

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:

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