Commit fb0fef64 authored by Ibrahim's avatar Ibrahim
Browse files

control.py: PID accounts for timestep length, previously assumed =1

parent 69dafbfd
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])
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment