Numerical derivation and integration in code for physics, mapping, robotics, gaming, dead-reckoning, and controls<\/h2>\n
Pay attention to where I use the words “estimate” vs “measurement” below. The difference is important.<\/em><\/p>\n The following table is true, for example. Read the 2nd line, for instance, as: “If you take the derivative of a velocity measurement<\/em> with respect to time, you get an acceleration estimate<\/em>, and if you take its integral, you get a position estimate<\/em>.”<\/p>\n For jerk, snap or jounce, crackle, and pop, see: https:\/\/en.wikipedia.org\/wiki\/Fourth,_fifth,_and_sixth_derivatives_of_position<\/a>.<\/p>\n Remember, derivation obtains the slope<\/em> of the line, In order to obtain a velocity estimate<\/em> from a system where you are obtaining repeated position measurements<\/em> (ex: you are taking GPS readings periodically), you must numerically derivate your position measurements over time. Your y-axis is position<\/strong>, and your x-axis is time<\/strong>, so In code, that would look like this, for a system where you’re only measuring position in 1-dimension:<\/p>\n Numerical integration obtains the area under the curve<\/em>, In order to obtain a position estimate<\/em> from a system where you are obtaining repeated velocity measurements<\/em> (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<\/strong>, and your x-axis is time<\/strong>, so 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<\/em> distance traveled, you must sum all of the individual estimates of distance traveled.<\/p>\n See also: https:\/\/en.wikipedia.org\/wiki\/Numerical_integration<\/a>.<\/p>\n To do the above, you’ll need a good way to obtain timestamps. Here are various techniques I use:<\/p>\n In C++, use my If using Linux in C or<\/em> C++, use my Here is the 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 Taking data samples as fast as possible in a sample loop is a good idea, because then you can average many samples to achieve:<\/p>\n See: So, sampling at high sample rates<\/em> is good. You can do basic filtering on these samples.<\/p>\n If you process raw samples at a high rate, doing numerical derivation<\/em> on high-sample-rate raw samples will end up derivating a lot of noise<\/em>, 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<\/em> 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.<\/p>\n Just using the filtered samples for both numerical integration and numerical derivation, however, is just fine.<\/p>\n Control loop rates should not be too fast. The higher the sample rates<\/em>, the better, because you can filter them to reduce noise. The higher the control loop rate<\/em>, however, not<\/em> 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<\/em> instead of to real changes in the measured data.<\/p>\n Therefore, even if you have a sample rate<\/em> of 1 kHz, for instance, to oversample and filter the data, control loops<\/em> 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<\/strong>, perhaps up to 400+ Hz<\/strong> for simple systems with clean data. In some scenarios you can go faster, but 50 Hz<\/strong> is very common in control systems. The more-complicated the system and\/or the more-noisy the sensor measurements, generally, the slower<\/em> the control loop must be, down to about 1~10 Hz<\/strong> or so. Self-driving cars, for instance, which are very complicated<\/em>, frequently operate at control loops of only 10 Hz<\/a>.<\/p>\n In order to accomplish the above, independent measurement and filtering loops<\/em>, and control loops<\/em>, you’ll need a means of performing precise and efficient loop timing and multi-tasking.<\/p>\n If needing to do precise, repetitive loops in Linux in C or C++<\/strong>, use the If using bare-metal (no operating system) on a microcontroller<\/strong> as your compute platform, use timestamp-based cooperative multitasking<\/em> 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.<\/p>\n I have an in-depth example of both numerical integration<\/em> and cooperative multitasking on a bare-metal system using my 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.”<\/p>\n\n
\n
\nEx: you can derive<\/em> position measurements (m) with respect to time to obtain speed or velocity (m\/s) estimates<\/em>, and you can integrate<\/em> speed or velocity measurements (m\/s) with respect to time to obtain position or displacement (m) estimates<\/em>.<\/li>\n\n
Derivatives and integrals of position\n\nMeasurement, y Derivative Integral\n Estimate (dy\/dt) Estimate (dy*dt)\n----------------------- ----------------------- -----------------------\nposition [m] velocity [m\/s] - [m*s]\nvelocity [m\/s] acceleration [m\/s^2] position [m] \nacceleration [m\/s^2] jerk [m\/s^3] velocity [m\/s]\njerk [m\/s^3] snap [m\/s^4] acceleration [m\/s^2]\nsnap [m\/s^4] crackle [m\/s^5] jerk [m\/s^3]\ncrackle [m\/s^5] pop [m\/s^6] snap [m\/s^4]\npop [m\/s^6] - [m\/s^7] crackle [m\/s^5]\n<\/code><\/pre>\n1. numerical derivation<\/h2>\n
dy\/dx<\/code>, on an x-y plot. The general form is (y_new - y_old)\/(x_new - x_old)<\/code>.<\/p>\ndy\/dx<\/code> is simply (position_new - position_old)\/(time_new - time_old)<\/code>. A units check shows this might be meters\/sec<\/code>, which is indeed a unit for velocity.<\/p>\ndouble position_new_m = getPosition(); \/\/ m = meters\ndouble position_old_m;\n\/\/ `getNanoseconds()` should return a `uint64_t timestamp in nanoseconds, for\n\/\/ instance\ndouble time_new_sec = NS_TO_SEC((double)getNanoseconds());\ndouble time_old_sec;\n\nwhile (true)\n{\n position_old_m = position_new_m;\n position_new_m = getPosition();\n\n time_old_sec = time_new_sec;\n time_new_sec = NS_TO_SEC((double)getNanoseconds());\n\n \/\/ Numerical derivation of position measurements over time to obtain\n \/\/ velocity in meters per second (mps)\n double velocity_mps = \n (position_new_m - position_old_m)\/(time_new_sec - time_old_sec);\n}\n<\/code><\/pre>\n2. numerical integration<\/h2>\n
dy*dx<\/code>, on an x-y plot. One of the best ways to do this is called trapezoidal integration<\/em>, where you take the average dy<\/code> reading and multiply by dx<\/code>. This would look like this: (y_old + y_new)\/2 * (x_new - x_old)<\/code>.<\/p>\n(y_old + y_new)\/2 * (x_new - x_old)<\/code> is simply velocity_old + velocity_new)\/2 * (time_new - time_old)<\/code>. A units check shows this might be meters\/sec * sec = meters<\/code>, which is indeed a unit for distance.<\/p>\ndouble velocity_new_mps = getVelocity(); \/\/ mps = meters per second\ndouble velocity_old_mps;\n\/\/ `getNanoseconds()` should return a `uint64_t timestamp in nanoseconds, for\n\/\/ instance\ndouble time_new_sec = NS_TO_SEC((double)getNanoseconds());\ndouble time_old_sec;\n\n\/\/ Total meters traveled\ndouble distance_traveled_m_total = 0;\n\nwhile (true)\n{\n velocity_old_mps = velocity_new_mps;\n velocity_new_mps = getVelocity();\n\n time_old_sec = time_new_sec;\n time_new_sec = NS_TO_SEC((double)getNanoseconds());\n\n \/\/ Numerical integration of velocity measurements over time to obtain \n \/\/ a distance estimate (in meters) over this time interval\n double distance_traveled_m = \n (velocity_old_mps + velocity_new_mps)\/2 * (time_new_sec - time_old_sec);\n distance_traveled_m_total += distance_traveled_m;\n}\n<\/code><\/pre>\nGoing further:<\/h2>\n
high-resolution timestamps<\/h3>\n
uint64_t nanos()<\/code> function here.<\/p>\nuint64_t nanos()<\/code> function which uses clock_gettime()<\/code> here. Even better, I have wrapped it up into a nice timinglib<\/code> library for Linux, in my eRCaGuy_hello_world<\/a> repo here:<\/p>\n\n
NS_TO_SEC()<\/code> macro from timing.h:<\/p>\n#define NS_PER_SEC (1000000000L)\n\/\/\/ Convert nanoseconds to seconds\n#define NS_TO_SEC(ns) ((ns)\/NS_PER_SEC)\n<\/code><\/pre>\nmicros()<\/code><\/a> 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.<\/p>\nuse high<\/em> data sample rates<\/h3>\n
\n
\n
4^n<\/code> samples increases your sample resolution by n<\/code> bits of resolution. For example:\n4^0 = 1 sample at 10-bits resolution --> 1 10-bit sample\n4^1 = 4 samples at 10-bits resolution --> 1 11-bit sample\n4^2 = 16 samples at 10-bits resolution --> 1 12-bit sample\n4^3 = 64 samples at 10-bits resolution --> 1 13-bit sample\n4^4 = 256 samples at 10-bits resolution --> 1 14-bit sample\n4^5 = 1024 samples at 10-bits resolution --> 1 15-bit sample\n4^6 = 4096 samples at 10-bits resolution --> 1 16-bit sample\n<\/code><\/pre>\n![\"\"](\"https:\/\/jassweb.com\/solved\/wp-content\/uploads\/2022\/11\/Solved-Basic-example-of-how-to-do-numerical-integration-in.PNG\")
<\/a><\/li>\n<\/ol>\n<\/li>\n<\/ol>\nuse reasonable<\/em> control loop rates<\/h3>\n
loop timing and multi-tasking<\/h3>\n
sleep_until_ns()<\/code> function from my timinglib<\/code> above. I have a demo of my sleep_until_us()<\/code> function in-use in Linux to obtain repetitive loops as fast as 1 KHz to 100 kHz<\/code> here.<\/p>\nfull, numerical integration and multi-tasking example<\/h3>\n
CREATE_TASK_TIMER()<\/code> macro in my Full coulomb counter example in code<\/a>.<\/strong> That’s a great demo to study, in my opinion.<\/p>\nKalman filters<\/h3>\n
See also<\/h2>\n
\n