def derivative_of(x, dt=1, radian=False): if radian: x = make_continuous_copy(x) not_nan_mask = ~np.isnan(x) masked_x = x[not_nan_mask] if masked_x.shape[-1] < 2: return np.zeros_like(x) dx = np.full_like(x, np.nan) dx[not_nan_mask] = np.ediff1d(masked_x, to_begin=(masked_x[1] - masked_x[0])) / dt return dx
时间: 2023-02-19 21:47:16 浏览: 131
这是一段 python 代码,它定义了一个名为 "derivative_of" 的函数。该函数接受三个参数:"x"、"dt" 和 "radian"。如果 "radian" 为真,它会使用 "make_continuous_copy" 函数处理 "x"。然后,它会通过 numpy 库函数 "np.isnan" 检查 "x" 中的 NaN 值,并对非 NaN 值进行处理。如果 "x" 的最后一个维度小于 2,则返回元素值都为 0 的数组,否则,使用 "np.ediff1d" 函数计算 "x" 的一阶导数,并在非 NaN 值的位置进行填充,最后返回计算结果 "dx"。
相关问题
帮我解释以下代码 PID: _kp = _ki = _kd = _integrator = _imax = 0 _last_error = _last_derivative = _last_t = 0 _RC = 1/(2 * pi * 20) def init(self, p=0, i=0, d=0, imax=0): self._kp = float(p) self._ki = float(i) self._kd = float(d) self._imax = abs(imax) self._last_derivative = float('nan') def get_pid(self, 错误,缩放器): tnow = millis() dt = tnow - self._last_t 输出 = 0 如果 self._last_t == 0 或 dt > 1000: dt = 0 self.reset_I() self._last_t = tnow delta_time = float(dt) / float(1000) 输出 += error * self._kp 如果 abs(self._kd) > 0 和 dt > 0: 如果 isnan(self._last_derivative): 导数 = 0 self._last_derivative = 0 否则: 导数 =(误差 - self._last_error) / delta_time导数 = self._last_derivative + \ ((delta_time / (self._RC + delta_time)) * \ (导数 - self._last_derivative)) self._last_error = 误差 self._last_derivative = 导数输出 += self._kd * 导数输出 *= 缩放器 如果 abs(self._ki) > 0 且 DT > 0: self._integrator += (误差 * self._ki) * 缩放器 * delta_time 如果self._integrator < -self._imax: self._integrator = -self._imax ELIF self._integrator > self._imax: self._integrator = self._imax 输出 += self._integrator 返回输出 def reset_I(自身): self._integrator = 0 self._last_derivative = float('nan')
这段代码实现了一个PID控制器,其中的变量含义如下:
- _kp, _ki, _kd:分别是比例、积分、微分系数,用于调整PID控制器的响应特性;
- _integrator:积分器,用于积累误差,并作为积分项的输出;
- _imax:积分项输出的限幅值,防止积分器饱和;
- _last_error:记录上一次的误差,用于计算微分项;
- _last_derivative:记录上一次的误差变化率,用于计算微分项;
- _last_t:记录上一次调用PID控制器的时间,用于计算时间间隔;
- _RC:低通滤波器的时间常数,用于平滑微分项的输出。
该控制器的初始化函数 init() 设置了比例、积分、微分系数以及积分项输出的限幅值,其中积分项的输出限幅值取绝对值后作为最大值,可以避免积分项过大导致控制器不稳定。
PID控制器的主要函数是 get_pid(),它接收一个误差值和一个缩放因子,返回一个控制输出。该函数首先计算时间间隔 dt,如果时间间隔过大则重置积分器,然后根据比例、积分、微分系数计算出控制输出。其中,微分项使用了一阶低通滤波器平滑输出,以避免噪声对控制器的影响。
最后,reset_I()函数用于重置积分器和微分项,以便在重新开始控制时避免积分器过度积累误差。
解释以下代码每一句作用: def get_pid(self, error, scaler): tnow = millis() dt = tnow - self._last_t output = 0 if self._last_t == 0 or dt > 1000: dt = 0 self.reset_I() self._last_t = tnow delta_time = float(dt) / float(1000) output += error * self._kp if abs(self._kd) > 0 and dt > 0: if isnan(self._last_derivative): derivative = 0 self._last_derivative = 0 else: derivative = (error - self._last_error) / delta_time derivative = self._last_derivative + \ ((delta_time / (self._RC + delta_time)) * \ (derivative - self._last_derivative)) self._last_error = error self._last_derivative = derivative output += self._kd * derivative output *= scaler if abs(self._ki) > 0 and dt > 0: self._integrator += (error * self._ki) * scaler * delta_time if self._integrator < -self._imax: self._integrator = -self._imax elif self._integrator > self._imax: self._integrator = self._imax output += self._integrator return output
这段代码是一个PID控制器的实现。下面是每一句代码的作用解释:
1. `tnow = millis()`:获取当前时间。
2. `dt = tnow - self._last_t`:计算当前时间与上一次记录时间的差值,即时间间隔。
3. `output = 0`:初始化输出值。
4. `if self._last_t == 0 or dt > 1000:`:如果是第一次运行或者时间间隔超过1秒,则将时间间隔设置为0,并重置积分项。
5. `self._last_t = tnow`:更新上一次记录时间为当前时间。
6. `delta_time = float(dt) / float(1000)`:计算时间间隔的小数形式,单位为秒。
7. `output += error * self._kp`:将比例项乘以误差值并加到输出中。
8. `if abs(self._kd) > 0 and dt > 0:`:如果存在合法的微分系数且时间间隔大于0,则执行以下操作。
9. `if isnan(self._last_derivative):`:如果上一次微分项为NaN(非数字),则将其设置为0。
10. `derivative = (error - self._last_error) / delta_time`:计算当前微分项的值。
11. `derivative = self._last_derivative + ((delta_time / (self._RC + delta_time)) * (derivative - self._last_derivative))`:根据一阶惯性滤波器的公式更新微分项。
12. `self._last_error = error`:更新上一次误差值为当前误差值。
13. `self._last_derivative = derivative`:更新上一次微分项为当前微分项。
14. `output += self._kd * derivative`:将微分项乘以微分系数并加到输出中。
15. `output *= scaler`:将输出值乘以缩放因子。
16. `if abs(self._ki) > 0 and dt > 0:`:如果存在合法的积分系数且时间间隔大于0,则执行以下操作。
17. `self._integrator += (error * self._ki) * scaler * delta_time`:根据积分项的公式更新积分项。
18. `if self._integrator < -self._imax: self._integrator = -self._imax elif self._integrator > self._imax: self._integrator = self._imax`:对积分项进行限幅。
19. `output += self._integrator`:将积分项加到输出中。
20. `return output`:返回最终的输出值。
阅读全文