如何在 C++ 中进行数值积分的基本示例
Basic example of how to do numerical integration in C++
我想大多数人都知道如何在计算机编程中进行数值推导，（作为极限 --> 0；阅读：“作为极限接近零”）。
//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;
这是大多数推导程序的基本构建块。
我如何使用积分来做到这一点？我是使用 for 循环并添加还是什么？
物理、制图、机器人、游戏、dead-reckoning 和控制的代码中的数值推导和集成
请注意我在下面使用“估计”和“测量”这两个词的地方。区别很重要。
- 测量值 是来自传感器的直接读数。
- 例如：GPS 直接测量 position（米），速度计直接测量 speed（m/s）。
- Estimates 是 计算出的预测 您可以通过积分和推导（导出）测量值来获得。
例如：您可以 推导 相对于时间的位置测量值 (m) 以获得速度或速度 (m/s) 估计值，并且您可以相对于时间积分速度或速度测量值（m/s）以获得位置或位移（m）估计.
例如下面的table是正确的。例如，阅读第二行：“如果你对速度测量值求导数，你会得到一个加速度估计值，如果你求其积分，你会得到一个位置估计值。”
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]
对于急动、啪嗒声或颠簸、噼啪声和砰砰声，请参见：https://en.wikipedia.org/wiki/Fourth,_fifth,_and_sixth_derivatives_of_position。
1。数值推导
请记住，推导会在 x-y 图上获得直线 dy/dx 的 斜率 。一般形式为(y_new - y_old)/(x_new - x_old).
为了从您获得重复位置测量的系统中获得速度估计（例如：您正在获取 GPS 读数周期性地），您必须随着时间的推移以数字方式推导您的位置测量值。你的y-axis是位置，你的x-axis是时间，所以dy/dx就是(position_new - position_old)/(time_new - time_old).单位检查显示这可能是 meters/sec，这确实是速度单位。
在代码中，对于仅在一维中测量位置的系统，它看起来像这样：
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。数值积分
数值积分在 x-y 图上获得 曲线下的面积 、dy*dx。执行此操作的最佳方法之一称为 梯形积分 ，其中您取平均 dy 读数并乘以 dx。这看起来像这样：(y_old + y_new)/2 * (x_new - x_old).
为了从您正在获得重复速度测量值的系统中获得位置估计（例如：您正在尝试估计仅读取汽车上的速度表时行驶的距离），您必须随时间对速度测量值进行数值积分。你的y-axis是速度，你的x-axis是时间，所以(y_old + y_new)/2 * (x_new - x_old)就是velocity_old + velocity_new)/2 * (time_new - time_old).单位检查显示这可能是 meters/sec * sec = meters，这确实是距离单位。
在代码中，它看起来像这样。请注意，数值积分获得了在那个微小的时间间隔内行进的距离。要获得总 行进距离的估计值，您必须将所有单独的行进距离估计值相加。
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;
}
另请参阅：https://en.wikipedia.org/wiki/Numerical_integration。
更进一步：
high-resolution 时间戳
要执行上述操作，您需要一种获取时间戳的好方法。以下是我使用的各种技术：
在 C++ 中，使用 my uint64_t nanos() function here。
如果在 C 或 C++ 中使用 Linux，请在此处使用 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 存储库：
这是来自 timing.h 的 NS_TO_SEC() 宏：
#define NS_PER_SEC (1000000000L)
/// Convert nanoseconds to seconds
#define NS_TO_SEC(ns) ((ns)/NS_PER_SEC)
如果使用微控制器，您需要从定时器或计数器寄存器中读取递增周期计数器，您已将其配置为以稳定、固定的速率递增。例如：在 Arduino 上：使用 micros() 获取具有 4 微秒分辨率的微秒时间戳（默认情况下，它可以更改）。在 STM32 或其他平台上，您需要配置自己的 timer/counter.
使用高数据采样率
在样本循环中尽可能快地获取数据样本是个好主意，因为这样您可以对许多样本进行平均以实现：
- 降低噪声：对许多原始样本进行平均可降低传感器的噪声。
- Higher-resolution：平均许多原始样本实际上会增加测量系统的分辨率。这就是所谓的 过采样。
- 我把它写在我的个人网站上：ElectricRCAircraftGuy.com: Using the Arduino Uno’s built-in 10-bit to 16+-bit ADC (Analog to Digital Converter)。
- 并且 Atmel/Microchip 在他们的 white-paper 中写道：Application Note AN8003: AVR121: Enhancing ADC resolution by oversampling。
- 采用
4^n 样本可将样本分辨率提高 n 位分辨率。例如：
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
参见：
因此，以 高采样率 进行采样是好的。您可以对这些示例进行基本过滤。
如果您以高速率处理原始样本，对 high-sample-rate 个原始样本进行 数值推导 最终会推导很多噪声，它会产生噪声导数估计。这不太好。最好对过滤后的样本进行推导：例如：100 或 1000 个快速样本的平均值。然而，对 high-sample-rate 原始样本进行 数值积分 很好，因为 ，“当积分时，你得到的样本越多，噪声平均效果越好”这与我上面的笔记一致。
然而，仅使用过滤后的样本进行数值积分和数值推导就可以了。
使用合理控制循环速率
控制循环速率不应太快。 采样率越高越好，因为您可以过滤它们以减少噪音。然而，控制循环率越高，不一定越好，因为控制循环率有一个最佳点。如果你的控制回路速率太慢，系统的频率响应就会很慢，并且对环境的响应速度不够快，如果控制回路速率太快，它最终只会响应样本 噪音而不是测量数据的真实变化。
因此，例如，即使您的 采样率 为 1 kHz，要过采样和过滤数据， 控制循环 不需要这么快，因为在非常短的时间间隔内，真实传感器读数的噪声会太大。在 10 Hz ~ 100 Hz 之间的任何位置使用控制回路，对于具有干净数据的简单系统，可能高达 400+ Hz。在某些情况下，您可以走得更快，但 50 Hz 在控制系统中非常常见。 more-complicated 系统 and/or more-noisy 传感器测量，通常， 较慢 控制回路必须下降到大约 1~10 Hz左右。 Self-driving 汽车，例如，非常复杂，经常在 control loops of only 10 Hz.
运行
循环计时和multi-tasking
为了完成上述任务，独立的测量和过滤循环，以及控制循环，您需要一种执行方法精确高效的循环计时和multi-tasking.
如果需要在 C 或 C++Linux 中执行精确的重复循环 Linux，请使用我上面 timinglib 中的 sleep_until_ns() 函数.我在 Linux 中有一个 sleep_until_us() 函数 in-use 的演示，以获得 repetitive loops as fast as 1 KHz to 100 kHz here.
如果在微控制器上使用bare-metal（无操作系统）作为您的计算平台，请使用timestamp-based协同多任务处理 根据需要执行控制循环和其他循环，例如测量循环。在这里查看我的详细答案：.
完整的数值积分和 multi-tasking 示例
我在 bare-metal 系统上使用我的 CREATE_TASK_TIMER() 我的 Full coulomb counter example in code 中的宏。 在我看来，这是一个很好的学习演示。
卡尔曼滤波器
为了进行稳健的测量，您可能需要卡尔曼滤波器，也许是“无味卡尔曼滤波器”或 UKF，因为显然它们是“无味的”，因为它们“不臭”。
我想大多数人都知道如何在计算机编程中进行数值推导，（作为极限 --> 0；阅读：“作为极限接近零”）。
//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;
这是大多数推导程序的基本构建块。
我如何使用积分来做到这一点？我是使用 for 循环并添加还是什么？
物理、制图、机器人、游戏、dead-reckoning 和控制的代码中的数值推导和集成
请注意我在下面使用“估计”和“测量”这两个词的地方。区别很重要。
- 测量值 是来自传感器的直接读数。
- 例如：GPS 直接测量 position（米），速度计直接测量 speed（m/s）。
- Estimates 是 计算出的预测 您可以通过积分和推导（导出）测量值来获得。 例如：您可以 推导 相对于时间的位置测量值 (m) 以获得速度或速度 (m/s) 估计值，并且您可以相对于时间积分速度或速度测量值（m/s）以获得位置或位移（m）估计.
例如下面的table是正确的。例如，阅读第二行：“如果你对速度测量值求导数，你会得到一个加速度估计值，如果你求其积分，你会得到一个位置估计值。”
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]
对于急动、啪嗒声或颠簸、噼啪声和砰砰声，请参见：https://en.wikipedia.org/wiki/Fourth,_fifth,_and_sixth_derivatives_of_position。
1。数值推导
请记住，推导会在 x-y 图上获得直线 dy/dx 的 斜率 。一般形式为(y_new - y_old)/(x_new - x_old).
为了从您获得重复位置测量的系统中获得速度估计（例如：您正在获取 GPS 读数周期性地），您必须随着时间的推移以数字方式推导您的位置测量值。你的y-axis是位置，你的x-axis是时间，所以dy/dx就是(position_new - position_old)/(time_new - time_old).单位检查显示这可能是 meters/sec，这确实是速度单位。
在代码中，对于仅在一维中测量位置的系统，它看起来像这样：
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。数值积分
数值积分在 x-y 图上获得 曲线下的面积 、dy*dx。执行此操作的最佳方法之一称为 梯形积分 ，其中您取平均 dy 读数并乘以 dx。这看起来像这样：(y_old + y_new)/2 * (x_new - x_old).
为了从您正在获得重复速度测量值的系统中获得位置估计（例如：您正在尝试估计仅读取汽车上的速度表时行驶的距离），您必须随时间对速度测量值进行数值积分。你的y-axis是速度，你的x-axis是时间，所以(y_old + y_new)/2 * (x_new - x_old)就是velocity_old + velocity_new)/2 * (time_new - time_old).单位检查显示这可能是 meters/sec * sec = meters，这确实是距离单位。
在代码中，它看起来像这样。请注意，数值积分获得了在那个微小的时间间隔内行进的距离。要获得总 行进距离的估计值，您必须将所有单独的行进距离估计值相加。
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;
}
另请参阅：https://en.wikipedia.org/wiki/Numerical_integration。
更进一步：
high-resolution 时间戳
要执行上述操作，您需要一种获取时间戳的好方法。以下是我使用的各种技术：
在 C++ 中，使用 my uint64_t nanos() function here。
如果在 C 或 C++ 中使用 Linux，请在此处使用 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 存储库：
这是来自 timing.h 的 NS_TO_SEC() 宏：
#define NS_PER_SEC (1000000000L)
/// Convert nanoseconds to seconds
#define NS_TO_SEC(ns) ((ns)/NS_PER_SEC)
如果使用微控制器，您需要从定时器或计数器寄存器中读取递增周期计数器，您已将其配置为以稳定、固定的速率递增。例如：在 Arduino 上：使用 micros() 获取具有 4 微秒分辨率的微秒时间戳（默认情况下，它可以更改）。在 STM32 或其他平台上，您需要配置自己的 timer/counter.
使用高数据采样率
在样本循环中尽可能快地获取数据样本是个好主意，因为这样您可以对许多样本进行平均以实现：
- 降低噪声：对许多原始样本进行平均可降低传感器的噪声。
- Higher-resolution：平均许多原始样本实际上会增加测量系统的分辨率。这就是所谓的 过采样。
- 我把它写在我的个人网站上：ElectricRCAircraftGuy.com: Using the Arduino Uno’s built-in 10-bit to 16+-bit ADC (Analog to Digital Converter)。
- 并且 Atmel/Microchip 在他们的 white-paper 中写道：Application Note AN8003: AVR121: Enhancing ADC resolution by oversampling。
- 采用
4^n样本可将样本分辨率提高n位分辨率。例如：
参见：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
因此，以 高采样率 进行采样是好的。您可以对这些示例进行基本过滤。
如果您以高速率处理原始样本，对 high-sample-rate 个原始样本进行 数值推导 最终会推导很多噪声，它会产生噪声导数估计。这不太好。最好对过滤后的样本进行推导：例如：100 或 1000 个快速样本的平均值。然而，对 high-sample-rate 原始样本进行 数值积分 很好，因为
然而，仅使用过滤后的样本进行数值积分和数值推导就可以了。
使用合理控制循环速率
控制循环速率不应太快。 采样率越高越好，因为您可以过滤它们以减少噪音。然而，控制循环率越高，不一定越好，因为控制循环率有一个最佳点。如果你的控制回路速率太慢，系统的频率响应就会很慢，并且对环境的响应速度不够快，如果控制回路速率太快，它最终只会响应样本 噪音而不是测量数据的真实变化。
因此，例如，即使您的 采样率 为 1 kHz，要过采样和过滤数据， 控制循环 不需要这么快，因为在非常短的时间间隔内，真实传感器读数的噪声会太大。在 10 Hz ~ 100 Hz 之间的任何位置使用控制回路，对于具有干净数据的简单系统，可能高达 400+ Hz。在某些情况下，您可以走得更快，但 50 Hz 在控制系统中非常常见。 more-complicated 系统 and/or more-noisy 传感器测量，通常， 较慢 控制回路必须下降到大约 1~10 Hz左右。 Self-driving 汽车，例如，非常复杂，经常在 control loops of only 10 Hz.
运行循环计时和multi-tasking
为了完成上述任务，独立的测量和过滤循环，以及控制循环，您需要一种执行方法精确高效的循环计时和multi-tasking.
如果需要在 C 或 C++Linux 中执行精确的重复循环 Linux，请使用我上面 timinglib 中的 sleep_until_ns() 函数.我在 Linux 中有一个 sleep_until_us() 函数 in-use 的演示，以获得 repetitive loops as fast as 1 KHz to 100 kHz here.
如果在微控制器上使用bare-metal（无操作系统）作为您的计算平台，请使用timestamp-based协同多任务处理 根据需要执行控制循环和其他循环，例如测量循环。在这里查看我的详细答案：
完整的数值积分和 multi-tasking 示例
我在 bare-metal 系统上使用我的 CREATE_TASK_TIMER() 我的 Full coulomb counter example in code 中的宏。 在我看来，这是一个很好的学习演示。
卡尔曼滤波器
为了进行稳健的测量，您可能需要卡尔曼滤波器，也许是“无味卡尔曼滤波器”或 UKF，因为显然它们是“无味的”，因为它们“不臭”。