control.py: PID accounts for timestep length, previously assumed =1 (fb0fef64) · Commits · Ibrahim Ahmed / multirotor

control.py

+42 −15

Changes for control.py: 42 added lines, 15 removed lines.
Original line number	Diff line number	Diff line
		from dataclasses import dataclass

		import numpy as np
		from scipy.integrate import trapezoid

		from .simulation import Multirotor
		from .coords import euler_to_angular_rate



		@dataclass
		class PIDController:
		"""
		Proportional Integral Derivative controller. Tracks a reference signal and outputs
		a control signal to minimize output. The equation is:

		err = reference - measurement

		u = k_p * err + k_d * d(err)/dt + k_i * int(err . dt)
		"""

		k_p: np.ndarray
		"Proportional constant"
		k_i: np.ndarray
		"Integral constant"
		k_d: np.ndarray
		"Derivative constant"
		max_err_i: np.ndarray
		"Maximum accumulated error"
		dt: float
		"Simulation parameters to set timestep"

		def __post_init__(self):
		self.err_p = np.zeros_like(self.k_p)
		self.err_i = np.zeros_like(self.k_i)
		self.err_d = np.zeros_like(self.k_d)
		self.err = 0.
		self.err = np.zeros_like(self.k_p)
		if self.max_err_i is None:
		self.max_err_i = np.inf


		def reset(self):
		self.err = 0.
		self.err *= 0
		self.err_p *= 0
		self.err_i *= 0
		self.err_d *= 0
		@@ -33,8 +49,11 @@ class PIDController:
		def step(self, reference: np.ndarray, measurement: np.ndarray) -> np.ndarray:
		err = reference - measurement
		self.err_p = err
		self.err_i = np.clip(self.err_i + err, a_min=-self.max_err_i, a_max=self.max_err_i)
		self.err_d = err - self.err
		self.err_i = np.clip(
		self.err_i + trapezoid((self.err, err), dx=self.dt, axis=0),
		a_min=-self.max_err_i, a_max=self.max_err_i
		)
		self.err_d = (err - self.err) / self.dt
		self.err = err
		return self.k_p * self.err_p + self.k_i * self.err_i + self.k_d * self.err_d

		@@ -52,13 +71,7 @@ class PosController(PIDController):
		self.k_p = np.ones(2) * np.asarray(self.k_p)
		self.k_i = np.ones(2) * np.asarray(self.k_i)
		self.k_d = np.ones(2) * np.asarray(self.k_d)

		self.err_p = np.zeros_like(self.k_p)
		self.err_i = np.zeros_like(self.k_i)
		self.err_d = np.zeros_like(self.k_d)
		self.err = 0.
		if self.max_err_i is None:
		self.max_err_i = np.inf
		super().__post_init__()


		def step(self, reference, measurement):
		@@ -73,9 +86,9 @@ class PosController(PIDController):
		# desired/reference change in x/y i.e. velocity
		ref_delta_xy = rot @ np.asarray([delta_x, delta_y])
		# TODO: Explicitly track velocity, instead of deltas
		# ref_vel_xy = ref_delta_xy / self.vehicle.simulation.dt
		# abs_max_vel = np.abs((ref_vel_xy / (np.linalg.norm(ref_vel_xy) + 1e-6)) * self.max_velocity)
		# ref_vel_xy = np.clip(ref_vel_xy, a_min=-abs_max_vel, a_max=abs_max_vel)
		ref_vel_xy = ref_delta_xy / self.vehicle.simulation.dt
		abs_max_vel = np.abs((ref_vel_xy / (np.linalg.norm(ref_vel_xy) + 1e-6)) * self.max_velocity)
		ref_vel_xy = np.clip(ref_vel_xy, a_min=-abs_max_vel, a_max=abs_max_vel)
		# actual/measured velocity
		mea_delta_xy = mea_vel_xy = self.vehicle.velocity[:2]
		# desired pitch, roll
		@@ -94,9 +107,16 @@ class AttController(PIDController):
		vehicle: Multirotor


		def __post_init__(self):
		self.k_p = np.ones(3) * np.asarray(self.k_p)
		self.k_i = np.ones(3) * np.asarray(self.k_i)
		self.k_d = np.ones(3) * np.asarray(self.k_d)
		super().__post_init__()


		def step(self, reference, measurement):
		# desired change in orientation i.e. angular velocity
		ref_delta = reference - measurement
		ref_delta = euler_to_angular_rate(reference - measurement, self.vehicle.orientation)
		# actual change in orientation
		mea_delta = self.vehicle.angular_rate
		# prescribed change in velocity i.e. angular acceleration
		@@ -136,6 +156,13 @@ class Controller:
		assert self.ctrl_a.vehicle is self.ctrl_p.vehicle, "Vehicle instances different."
		assert self.ctrl_a.vehicle is self.ctrl_z.vehicle, "Vehicle instances different."


		def reset(self):
		self.ctrl_a.reset()
		self.ctrl_p.reset()
		self.ctrl_z.reset()


		def step(self, reference, measurement=None):
		# x,y,z,yaw
		pitch_roll = self.ctrl_p.step(reference[:2], self.vehicle.position[:2])

Admin message