mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-05-10 06:59:54 +08:00
add python impl of 2nd order attitude reference model
This commit is contained in:
Antoine Drouin
committed by
Felix Ruess
Felix Ruess
parent
4bfab9d050
commit
88baf18c6c
@@ -0,0 +1,279 @@
|
||||
#
|
||||
# Copyright (C) 2014 Antoine Drouin
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 2, or (at your option)
|
||||
# any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi; see the file COPYING. If not, write to
|
||||
# the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
# Boston, MA 02111-1307, USA.
|
||||
#
|
||||
"""
|
||||
This is a reference implementation of the control system to serve as specifications
|
||||
for the C version
|
||||
"""
|
||||
|
||||
import math, logging, pdb
|
||||
import numpy as np
|
||||
from scipy import integrate
|
||||
|
||||
import pat.utils as pu
|
||||
import pat.algebra as pa
|
||||
import pat.control as pc
|
||||
|
||||
LOG = logging.getLogger('control')
|
||||
# LOG.setLevel(logging.ERROR)
|
||||
LOG.setLevel(logging.DEBUG)
|
||||
|
||||
|
||||
class att_ref:
|
||||
"""Default second order attitude reference model, Python implementation"""
|
||||
name = 'Python default'
|
||||
|
||||
def __init__(self, **kwargs):
|
||||
self.omega = kwargs.get('omega', 6.)
|
||||
self.xi = kwargs.get('xi', 0.8)
|
||||
self.t = kwargs.get('t0', 0.)
|
||||
for p in ['omega', 'xi']:
|
||||
self.ensure_vect(p)
|
||||
self.euler, self.quat = np.zeros(3), pa.quat_zero()
|
||||
self.vel, self.accel = np.zeros(3), np.zeros(3)
|
||||
|
||||
def ensure_vect(self, p):
|
||||
if type(getattr(self, p)) == float:
|
||||
setattr(self, p, getattr(self, p) * np.ones(3))
|
||||
|
||||
def set_param(self, p, v):
|
||||
if type(v) == float:
|
||||
v = v * np.ones(3)
|
||||
setattr(self, p, v)
|
||||
|
||||
def set_quat(self, quat, vel=np.zeros(3), accel=np.zeros(3)):
|
||||
self.quat = quat
|
||||
self.vel = vel
|
||||
self.accel = accel
|
||||
self.euler = pa.euler_of_quat(self.quat)
|
||||
|
||||
def set_euler(self, euler, vel=np.zeros(3), accel=np.zeros(3)):
|
||||
self.euler = euler
|
||||
self.vel = vel
|
||||
self.accel = accel
|
||||
self.quat = pa.quat_of_euler(self.euler)
|
||||
|
||||
def update_quat(self, setpoint, dt):
|
||||
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
|
||||
self.vel += dt * self.accel
|
||||
err_quat = pa.quat_wrap_shortest(pa.quat_inv_comp(setpoint, self.quat))
|
||||
self.accel = -2. * self.xi * self.omega * self.vel \
|
||||
- 2. * self.omega * self.omega * err_quat[1:]
|
||||
return self.quat, self.vel, self.accel
|
||||
|
||||
def update_euler(self, setpoint, dt):
|
||||
self.update_quat(pa.quat_of_euler(setpoint), dt)
|
||||
self.euler = pa.euler_of_quat(self.quat)
|
||||
|
||||
def run(self, t, sp_euler=None, sp_quat=None):
|
||||
dt = t - self.t
|
||||
self.t = t
|
||||
if sp_euler is not None:
|
||||
self.update_euler(sp_euler, dt)
|
||||
elif sp_quat is not None:
|
||||
self.update_quat(sp_quat, dt)
|
||||
|
||||
|
||||
class att_ref_sat_naive(att_ref):
|
||||
"""Attitude reference model with naive saturation"""
|
||||
name = 'Python Sat Naive'
|
||||
|
||||
def __init__(self, **kwargs):
|
||||
att_ref.__init__(self, **kwargs)
|
||||
self.sat_vel = kwargs.get('sat_vel', pu.rad_of_deg(100))
|
||||
self.sat_accel = kwargs.get('sat_accel', pu.rad_of_deg(500))
|
||||
for p in ['sat_vel', 'sat_accel']:
|
||||
self.ensure_vect(p)
|
||||
|
||||
def update_quat(self, setpoint, dt):
|
||||
att_ref.update_quat(self, setpoint, dt)
|
||||
self.accel = np.clip(self.accel, -self.sat_accel, self.sat_accel)
|
||||
for i in range(0, 3):
|
||||
if self.vel[i] >= self.sat_vel[i]:
|
||||
self.vel[i] = self.sat_vel[i]
|
||||
if self.accel[i] > 0:
|
||||
self.accel[i] = 0
|
||||
elif self.vel[i] <= -self.sat_vel[i]:
|
||||
self.vel[i] = -self.sat_vel[i]
|
||||
if self.accel[i] < 0:
|
||||
self.accel[i] = 0
|
||||
return self.quat, self.vel, self.accel
|
||||
|
||||
|
||||
class att_ref_sat_nested(att_ref_sat_naive):
|
||||
"""Nested saturation"""
|
||||
name = 'Python Sat Nested'
|
||||
|
||||
def __init__(self, **kwargs):
|
||||
att_ref_sat_naive.__init__(self, **kwargs)
|
||||
self._compute_auxiliary()
|
||||
|
||||
def update_quat(self, sp, dt):
|
||||
err_quat = 2. * np.array(pa.quat_wrap_shortest(pa.quat_inv_comp(sp, self.quat)))
|
||||
self.e = [err_quat[1:], self.vel, self.accel]
|
||||
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
|
||||
self.vel += dt * self.accel
|
||||
self.accel = np.zeros(3)
|
||||
self.accel = self.M[0] * np.clip(self.K[0] / self.CM[0] * self.e[0] + self.accel, -1., 1.)
|
||||
self.accel = self.M[1] * np.clip(self.K[1] / self.CM[1] * self.e[1] + self.accel, -1., 1.)
|
||||
# pdb.set_trace()
|
||||
return self.quat, self.vel, self.accel
|
||||
|
||||
def set_param(self, p, v):
|
||||
att_ref_sat_naive.set_param(self, p, v)
|
||||
self._compute_auxiliary()
|
||||
|
||||
def _compute_auxiliary(self):
|
||||
# self.K = pc.butterworth(2, self.omega[0])
|
||||
omega, xi = self.omega[0], self.xi[0]
|
||||
omega_d = omega * math.sqrt(1 - xi ** 2)
|
||||
coefs = -np.poly([complex(-omega * xi, omega_d), complex(-omega * xi, -omega_d)])[::-1]
|
||||
self.K = np.real(coefs[:-1])
|
||||
LOG.debug('sat_nested.__init__ omega: {} K:{} poles:{} '.format(self.omega[0], self.K, pc.poles(self.K)))
|
||||
self.sats = np.array([self.sat_vel, self.sat_accel])
|
||||
LOG.debug(' sats:\n{}'.format(self.sats))
|
||||
self.M = np.array(self.sats)
|
||||
self.M[0] *= self.K[1]
|
||||
self.M[0] /= np.prod(self.M[1:], axis=0)
|
||||
LOG.debug(' M:\n{}'.format(self.M))
|
||||
self.CM = np.cumprod(self.M[::-1], axis=0)[::-1]
|
||||
LOG.debug(' CM:\n{}\n'.format(self.CM))
|
||||
|
||||
|
||||
def my_K(omega, xi=0.8):
|
||||
omega_d = omega * math.sqrt(1 - xi ** 2)
|
||||
p1 = complex(-omega * xi, omega_d)
|
||||
p2 = complex(-omega * xi, -omega_d)
|
||||
p3 = -1.5 * omega
|
||||
poles = [p1, p2, p3]
|
||||
coefs = -np.poly(poles)[::-1]
|
||||
return np.real(coefs[:-1])
|
||||
|
||||
|
||||
class att_ref_sat_nested2(att_ref_sat_naive):
|
||||
"""Nested saturation"""
|
||||
name = 'Python Sat Nested2'
|
||||
|
||||
def __init__(self, **kwargs):
|
||||
att_ref_sat_naive.__init__(self, **kwargs)
|
||||
self.sat_jerk = kwargs.get('sat_jerk', pu.rad_of_deg(3000))
|
||||
self.ensure_vect('sat_jerk')
|
||||
self.jerk = np.zeros(3)
|
||||
self._compute_auxiliary()
|
||||
|
||||
def update_quat(self, sp, dt):
|
||||
err_quat = 2. * np.array(pa.quat_wrap_shortest(pa.quat_inv_comp(sp, self.quat)))
|
||||
self.e = [err_quat[1:], self.vel, self.accel]
|
||||
|
||||
self.quat = pa.quat_integrate(self.quat, self.vel, dt)
|
||||
self.vel += dt * self.accel
|
||||
self.accel += dt * self.jerk
|
||||
self.jerk = np.zeros(3)
|
||||
self.jerk = self.M[0] * np.clip(self.K[0] / self.CM[0] * self.e[0] + self.jerk, -1., 1.)
|
||||
self.jerk = self.M[1] * np.clip(self.K[1] / self.CM[1] * self.e[1] + self.jerk, -1., 1.)
|
||||
self.jerk = self.M[2] * np.clip(self.K[2] / self.CM[2] * self.e[2] + self.jerk, -1., 1.)
|
||||
# pdb.set_trace()
|
||||
return self.quat, self.vel, self.accel
|
||||
|
||||
def set_param(self, p, v):
|
||||
att_ref_sat_naive.set_param(self, p, v)
|
||||
self._compute_auxiliary()
|
||||
|
||||
def _compute_auxiliary(self):
|
||||
# self.K = pc.butterworth(3, self.omega[0])
|
||||
self.K = my_K(self.omega[0], self.xi[0])
|
||||
LOG.debug('sat_nested.__init__ omega: {} K:{} poles:{} '.format(self.omega[0], self.K, pc.poles(self.K)))
|
||||
self.sats = np.array([self.sat_vel, self.sat_accel, self.sat_jerk])
|
||||
LOG.debug(' sats:\n{}'.format(self.sats))
|
||||
self.M = np.array(self.sats)
|
||||
# pdb.set_trace()
|
||||
for i in range(0, 3):
|
||||
self.M[0:-1, i] *= self.K[1:]
|
||||
for j in range(0, 2):
|
||||
self.M[1 - j, i] /= np.prod(self.M[2 - j:, i])
|
||||
LOG.debug(' M:\n{}'.format(self.M))
|
||||
self.CM = np.cumprod(self.M[::-1], axis=0)[::-1]
|
||||
LOG.debug(' CM:\n{}\n'.format(self.CM))
|
||||
|
||||
|
||||
class att_ref_native(att_ref):
|
||||
""" The C implementations"""
|
||||
|
||||
def __init__(self, _type):
|
||||
att_ref.__init__(self)
|
||||
self.name = 'native_' + _type
|
||||
self.c_impl = __import__('reference_' + _type)
|
||||
self.c_impl.init()
|
||||
|
||||
def update_euler(self, setpoint, dt):
|
||||
ref = np.zeros(9)
|
||||
self.c_impl.update(setpoint, ref, dt)
|
||||
self.euler, self.vel, self.accel = ref[0:3], ref[3:7], ref[7:10]
|
||||
|
||||
|
||||
class att_ref_analytic_disc(att_ref):
|
||||
""" Scalar discrete time second order LTI"""
|
||||
|
||||
def __init__(self, axis=0):
|
||||
att_ref.__init__(self)
|
||||
self.name = 'Scalar Analytic discrete time'
|
||||
self.axis = axis
|
||||
|
||||
def update_euler(self, setpoint, dt):
|
||||
omega, xi = self.omega[self.axis], self.xi[self.axis]
|
||||
omega_d = omega * math.sqrt(1 - xi ** 2)
|
||||
tan_phi = xi / math.sqrt(1 - xi ** 2)
|
||||
omd_dt = omega_d * dt
|
||||
s_omd_dt = math.sin(omd_dt)
|
||||
c_omd_dt = math.cos(omd_dt)
|
||||
a1 = tan_phi * s_omd_dt + c_omd_dt
|
||||
a2 = 1. / omega_d * s_omd_dt
|
||||
a3 = -omega / math.sqrt(1 - xi ** 2) * s_omd_dt
|
||||
a4 = -tan_phi * s_omd_dt + c_omd_dt
|
||||
emxiomdt = math.exp(-xi * omega * dt)
|
||||
A = emxiomdt * np.array([[a1, a2], [a3, a4]])
|
||||
B = np.array([[-emxiomdt * (tan_phi * s_omd_dt + c_omd_dt) + 1],
|
||||
[omega / math.sqrt(1 - xi ** 2) * emxiomdt * s_omd_dt]])
|
||||
Xi = np.array([[self.euler[self.axis]], [self.vel[self.axis]]])
|
||||
Xi1 = np.dot(A, Xi) + np.dot(B, setpoint[self.axis])
|
||||
self.euler[self.axis], self.vel[self.axis] = Xi1
|
||||
|
||||
|
||||
class att_ref_analytic_cont(att_ref):
|
||||
""" Scalar continuous time second order LTI"""
|
||||
|
||||
def __init__(self, axis=0):
|
||||
att_ref.__init__(self)
|
||||
self.name = 'Scalar Analytic continuous time'
|
||||
self.axis = axis
|
||||
|
||||
def update_euler(self, setpoint, dt):
|
||||
omega, xi = self.omega[self.axis], self.xi[self.axis]
|
||||
|
||||
def dyn_cont(X, t, U):
|
||||
Ac = np.array([[0, 1], [-omega ** 2, -2. * xi * omega]])
|
||||
Bc = np.array([[0], [omega ** 2]])
|
||||
return np.dot(Ac, X) + np.dot(Bc, U)
|
||||
|
||||
Xi = np.array([self.euler[self.axis], self.vel[self.axis]])
|
||||
foo, Xi1 = integrate.odeint(dyn_cont, Xi, [0., dt], args=([setpoint[self.axis]],))
|
||||
self.euler[self.axis], self.vel[self.axis] = Xi1
|
||||
Xi1d = dyn_cont(Xi1, 0., [setpoint[self.axis]])
|
||||
self.accel[self.axis] = Xi1d[1]
|
||||
@@ -0,0 +1,271 @@
|
||||
import math
|
||||
import numpy as np
|
||||
import numpy.linalg
|
||||
|
||||
# euler angles
|
||||
e_phi = 0
|
||||
e_theta = 1
|
||||
e_psi = 2
|
||||
e_size = 3
|
||||
|
||||
# rotational velocities
|
||||
r_p = 0
|
||||
r_q = 1
|
||||
r_r = 2
|
||||
r_size = 3
|
||||
|
||||
# quaternions
|
||||
q_i = 0
|
||||
q_x = 1
|
||||
q_y = 2
|
||||
q_z = 3
|
||||
q_size = 4
|
||||
|
||||
# vectors
|
||||
v_x = 0
|
||||
v_y = 1
|
||||
v_z = 2
|
||||
v_size = 3
|
||||
|
||||
|
||||
def rmat_of_euler(e):
|
||||
cphi = math.cos(e[e_phi])
|
||||
sphi = math.sin(e[e_phi])
|
||||
ctheta = math.cos(e[e_theta])
|
||||
stheta = math.sin(e[e_theta])
|
||||
cpsi = math.cos(e[e_psi])
|
||||
spsi = math.sin(e[e_psi])
|
||||
return np.array([[ctheta * cpsi, ctheta * spsi, -stheta],
|
||||
[sphi * stheta * cpsi - cphi * spsi, sphi * stheta * spsi + cphi * cpsi, sphi * ctheta],
|
||||
[cphi * stheta * cpsi + sphi * spsi, cphi * stheta * spsi - sphi * cpsi, cphi * ctheta]])
|
||||
|
||||
|
||||
def euler_of_quat(q):
|
||||
dcm00 = 1.0 - 2. * (q[q_y] ** 2 + q[q_z] ** 2)
|
||||
dcm01 = 2. * (q[q_x] * q[q_y] + q[q_i] * q[q_z])
|
||||
dcm02 = 2. * (q[q_x] * q[q_z] - q[q_i] * q[q_y])
|
||||
dcm12 = 2. * (q[q_y] * q[q_z] + q[q_i] * q[q_x])
|
||||
dcm22 = 1.0 - 2. * (q[q_x] ** 2 + q[q_y] ** 2)
|
||||
return [math.atan2(dcm12, dcm22),
|
||||
-math.asin(dcm02),
|
||||
math.atan2(dcm01, dcm00)]
|
||||
|
||||
|
||||
def euler_of_quat_v(q):
|
||||
nr, nc = q.shape
|
||||
eu = np.zeros((nr, e_size))
|
||||
for i in range(0, nr):
|
||||
eu[i] = euler_of_quat(q[i])
|
||||
return eu
|
||||
|
||||
|
||||
#
|
||||
# bad (earth relative)
|
||||
#
|
||||
|
||||
|
||||
def rmat_of_euler_1(e):
|
||||
cphi = math.cos(e[e_phi])
|
||||
sphi = math.sin(e[e_phi])
|
||||
ctheta = math.cos(e[e_theta])
|
||||
stheta = math.sin(e[e_theta])
|
||||
cpsi = math.cos(e[e_psi])
|
||||
spsi = math.sin(e[e_psi])
|
||||
return np.array([[ctheta * cpsi, -ctheta * spsi, -stheta],
|
||||
[-sphi * stheta * cpsi + cphi * spsi, sphi * stheta * spsi + cphi * cpsi, -sphi * ctheta],
|
||||
[cphi * stheta * cpsi + sphi * spsi, -cphi * stheta * spsi + sphi * cpsi, cphi * ctheta]])
|
||||
|
||||
|
||||
def rvel_of_eulerd_1(eu, eud):
|
||||
sph = math.sin(eu[e_phi])
|
||||
cph = math.cos(eu[e_phi])
|
||||
sth = math.sin(eu[e_theta])
|
||||
cth = math.cos(eu[e_theta])
|
||||
a = 2 * (cph ** 2) - 1
|
||||
return [eud[e_phi] - sth * eud[e_psi],
|
||||
cph / a * eud[e_theta] - sph * cth / a * eud[e_psi],
|
||||
-sph / a * eud[e_theta] + cph * cth / a * eud[e_psi]]
|
||||
|
||||
|
||||
def euler_derivatives_1(eu, om):
|
||||
sph = math.sin(eu[e_phi])
|
||||
cph = math.cos(eu[e_phi])
|
||||
tth = math.tan(eu[e_theta])
|
||||
cth = math.cos(eu[e_theta])
|
||||
return [om[r_p] + sph * tth * om[r_q] + cph * tth * om[r_r],
|
||||
cph * om[r_q] + sph * om[r_r],
|
||||
sph / cth * om[r_q] + cph / cth * om[r_r]]
|
||||
|
||||
|
||||
#
|
||||
# good (body relative)
|
||||
#
|
||||
def rvel_of_eulerd(eu, eud):
|
||||
sph = math.sin(eu[e_phi])
|
||||
cph = math.cos(eu[e_phi])
|
||||
sth = math.sin(eu[e_theta])
|
||||
cth = math.cos(eu[e_theta])
|
||||
return [eud[e_phi] - sth * eud[e_psi],
|
||||
cph * eud[e_theta] + sph * cth * eud[e_psi],
|
||||
-sph * eud[e_theta] + cph * cth * eud[e_psi]]
|
||||
|
||||
|
||||
def euler_derivatives(eu, om):
|
||||
sph = math.sin(eu[e_phi])
|
||||
cph = math.cos(eu[e_phi])
|
||||
tth = math.tan(eu[e_theta])
|
||||
cth = math.cos(eu[e_theta])
|
||||
|
||||
return [om[r_p] + sph * tth * om[r_q] + cph * tth * om[r_r],
|
||||
cph * om[r_q] - sph * om[r_r],
|
||||
sph / cth * om[r_q] + cph / cth * om[r_r]]
|
||||
|
||||
|
||||
#
|
||||
# Quaternions
|
||||
#
|
||||
|
||||
def quat_zero():
|
||||
return np.array([1., 0., 0., 0.])
|
||||
|
||||
|
||||
def quat_of_euler(e):
|
||||
s_phi2 = math.sin(e[e_phi] / 2)
|
||||
c_phi2 = math.cos(e[e_phi] / 2)
|
||||
s_theta2 = math.sin(e[e_theta] / 2)
|
||||
c_theta2 = math.cos(e[e_theta] / 2)
|
||||
s_psi2 = math.sin(e[e_psi] / 2)
|
||||
c_psi2 = math.cos(e[e_psi] / 2)
|
||||
|
||||
return [c_phi2 * c_theta2 * c_psi2 + s_phi2 * s_theta2 * s_psi2,
|
||||
-c_phi2 * s_theta2 * s_psi2 + s_phi2 * c_theta2 * c_psi2,
|
||||
c_phi2 * s_theta2 * c_psi2 + s_phi2 * c_theta2 * s_psi2,
|
||||
c_phi2 * c_theta2 * s_psi2 - s_phi2 * s_theta2 * c_psi2]
|
||||
|
||||
|
||||
def quat_of_axis_angle(u, a):
|
||||
sa2 = math.sin(a / 2.)
|
||||
ca2 = math.cos(a / 2.)
|
||||
return [ca2, sa2 * u[0], sa2 * u[1], sa2 * u[2]]
|
||||
|
||||
|
||||
def quat_comp(a2b, b2c):
|
||||
return [
|
||||
a2b[q_i] * b2c[q_i] - a2b[q_x] * b2c[q_x] - a2b[q_y] * b2c[q_y] - a2b[q_z] * b2c[q_z],
|
||||
a2b[q_i] * b2c[q_x] + a2b[q_x] * b2c[q_i] + a2b[q_y] * b2c[q_z] - a2b[q_z] * b2c[q_y],
|
||||
a2b[q_i] * b2c[q_y] - a2b[q_x] * b2c[q_z] + a2b[q_y] * b2c[q_i] + a2b[q_z] * b2c[q_x],
|
||||
a2b[q_i] * b2c[q_z] + a2b[q_x] * b2c[q_y] - a2b[q_y] * b2c[q_x] + a2b[q_z] * b2c[q_i]]
|
||||
|
||||
|
||||
def quat_inv_comp(a2b, a2c):
|
||||
return [
|
||||
a2b[q_i] * a2c[q_i] + a2b[q_x] * a2c[q_x] + a2b[q_y] * a2c[q_y] + a2b[q_z] * a2c[q_z],
|
||||
a2b[q_i] * a2c[q_x] - a2b[q_x] * a2c[q_i] - a2b[q_y] * a2c[q_z] + a2b[q_z] * a2c[q_y],
|
||||
a2b[q_i] * a2c[q_y] + a2b[q_x] * a2c[q_z] - a2b[q_y] * a2c[q_i] - a2b[q_z] * a2c[q_x],
|
||||
a2b[q_i] * a2c[q_z] - a2b[q_x] * a2c[q_y] + a2b[q_y] * a2c[q_x] - a2b[q_z] * a2c[q_i]]
|
||||
|
||||
|
||||
def quat_derivative(q, om):
|
||||
return [
|
||||
0.5 * (-om[r_p] * q[q_x] - om[r_q] * q[q_y] - om[r_r] * q[q_z]),
|
||||
0.5 * (om[r_p] * q[q_i] + om[r_r] * q[q_y] - om[r_q] * q[q_z]),
|
||||
0.5 * (om[r_q] * q[q_i] - om[r_r] * q[q_x] + om[r_p] * q[q_z]),
|
||||
0.5 * (om[r_r] * q[q_i] + om[r_q] * q[q_x] - om[r_p] * q[q_y])
|
||||
]
|
||||
|
||||
|
||||
def quat_wrap_shortest(q):
|
||||
if q[q_i] < 0:
|
||||
return [-q[q_i], -q[q_x], -q[q_y], -q[q_z]]
|
||||
else:
|
||||
return q
|
||||
|
||||
|
||||
def quat_mormalize(q):
|
||||
nq = np.linalg.norm(q)
|
||||
return np.array(1. / nq * q)
|
||||
|
||||
|
||||
def quat_vmul(q, v):
|
||||
qi2 = q[q_i] * q[q_i]
|
||||
qiqx = q[q_i] * q[q_x]
|
||||
qiqy = q[q_i] * q[q_y]
|
||||
qiqz = q[q_i] * q[q_z]
|
||||
qx2 = q[q_x] * q[q_x]
|
||||
qxqy = q[q_x] * q[q_y]
|
||||
qxqz = q[q_x] * q[q_z]
|
||||
qy2 = q[q_y] * q[q_y]
|
||||
qyqz = q[q_y] * q[q_z]
|
||||
qz2 = q[q_z] * q[q_z]
|
||||
m00 = qi2 + qx2 - qy2 - qz2
|
||||
m01 = 2 * (qxqy + qiqz)
|
||||
m02 = 2 * (qxqz - qiqy)
|
||||
m10 = 2 * (qxqy - qiqz)
|
||||
m11 = qi2 - qx2 + qy2 - qz2
|
||||
m12 = 2 * (qyqz + qiqx)
|
||||
m20 = 2 * (qxqz + qiqy)
|
||||
m21 = 2 * (qyqz - qiqx)
|
||||
m22 = qi2 - qx2 - qy2 + qz2
|
||||
|
||||
return [
|
||||
m00 * v[v_x] + m01 * v[v_y] + m02 * v[v_z],
|
||||
m10 * v[v_x] + m11 * v[v_y] + m12 * v[v_z],
|
||||
m20 * v[v_x] + m21 * v[v_y] + m22 * v[v_z]]
|
||||
|
||||
|
||||
def quat_inv_vmul(q, v):
|
||||
qi2 = q[q_i] * q[q_i]
|
||||
qiqx = q[q_i] * q[q_x]
|
||||
qiqy = q[q_i] * q[q_y]
|
||||
qiqz = q[q_i] * q[q_z]
|
||||
qx2 = q[q_x] * q[q_x]
|
||||
qxqy = q[q_x] * q[q_y]
|
||||
qxqz = q[q_x] * q[q_z]
|
||||
qy2 = q[q_y] * q[q_y]
|
||||
qyqz = q[q_y] * q[q_z]
|
||||
qz2 = q[q_z] * q[q_z]
|
||||
m00 = qi2 + qx2 - qy2 - qz2
|
||||
m01 = 2 * (qxqy + qiqz)
|
||||
m02 = 2 * (qxqz - qiqy)
|
||||
m10 = 2 * (qxqy - qiqz)
|
||||
m11 = qi2 - qx2 + qy2 - qz2
|
||||
m12 = 2 * (qyqz + qiqx)
|
||||
m20 = 2 * (qxqz + qiqy)
|
||||
m21 = 2 * (qyqz - qiqx)
|
||||
m22 = qi2 - qx2 - qy2 + qz2
|
||||
|
||||
return [
|
||||
m00 * v[v_x] + m10 * v[v_y] + m20 * v[v_z],
|
||||
m01 * v[v_x] + m11 * v[v_y] + m21 * v[v_z],
|
||||
m02 * v[v_x] + m12 * v[v_y] + m22 * v[v_z]]
|
||||
|
||||
|
||||
def quat_integrate(q_im1, om_im1, dt):
|
||||
no = np.linalg.norm(om_im1)
|
||||
if no == 0.:
|
||||
codt2 = 1.
|
||||
sodt2o = 1.
|
||||
else:
|
||||
codt2 = math.cos(dt / 2. * no)
|
||||
sodt2o = math.sin(dt / 2. * no) / no
|
||||
|
||||
p = -om_im1[0]
|
||||
q = -om_im1[1]
|
||||
r = -om_im1[2]
|
||||
# WTF wrong formula. inverted signs instead
|
||||
Phi = np.array([[codt2, p * sodt2o, q * sodt2o, r * sodt2o],
|
||||
[-p * sodt2o, codt2, -r * sodt2o, q * sodt2o],
|
||||
[-q * sodt2o, r * sodt2o, codt2, -p * sodt2o],
|
||||
[-r * sodt2o, -q * sodt2o, p * sodt2o, codt2]])
|
||||
q_i = np.dot(Phi, q_im1)
|
||||
return q_i
|
||||
|
||||
|
||||
def cross_product(v1, v2):
|
||||
return np.array([v1[v_y] * v2[v_z] - v1[v_z] * v2[v_y],
|
||||
v1[v_z] * v2[v_x] - v1[v_x] * v2[v_z],
|
||||
v1[v_x] * v2[v_y] - v1[v_y] * v2[v_x]])
|
||||
|
||||
|
||||
def dot_product(v1, v2):
|
||||
return np.sum(v1 * v2)
|
||||
@@ -0,0 +1,112 @@
|
||||
from __future__ import division
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
|
||||
|
||||
class LinRef:
|
||||
""" Linear Reference Model (with first order integration)"""
|
||||
|
||||
def __init__(self, K):
|
||||
"""K: coefficients of the caracteristic polynomial, in ascending powers order,
|
||||
highesr order ommited (normalized to -1)"""
|
||||
self.K = K
|
||||
self.order = len(K)
|
||||
self.X = np.zeros(self.order + 1)
|
||||
|
||||
def run(self, dt, sp):
|
||||
self.X[:self.order] += self.X[1:self.order + 1] * dt
|
||||
e = np.array(self.X[:self.order])
|
||||
e[0] -= sp
|
||||
self.X[self.order] = np.sum(e * self.K)
|
||||
return self.X
|
||||
|
||||
|
||||
class SatRef:
|
||||
""" Nested Saturations Reference Model (with first order integration)"""
|
||||
|
||||
def __init__(self, K, sats):
|
||||
"""
|
||||
K: coefficients of the caracteristic polynomial, in ascending power order,
|
||||
highest power ommited (normalized to -1)
|
||||
sats: saturations for each order in ascending order
|
||||
"""
|
||||
self.K = K
|
||||
self.M = np.array(sats)
|
||||
self.M[0:-1] *= K[1:]
|
||||
for i in range(0, len(self.M) - 1):
|
||||
self.M[len(self.M) - 2 - i] /= np.prod(self.M[len(self.M) - 1 - i:])
|
||||
self.CM = np.cumprod(self.M[::-1])[::-1]
|
||||
print 'M', self.M, 'CM', self.CM
|
||||
self.order = len(K)
|
||||
self.X = np.zeros(self.order + 1)
|
||||
|
||||
def run(self, dt, sp):
|
||||
self.X[:self.order] += self.X[1:self.order + 1] * dt
|
||||
e = np.array(self.X[:self.order])
|
||||
e[0] -= sp
|
||||
self.X[self.order] = 0
|
||||
for i in range(0, self.order):
|
||||
self.X[self.order] = self.M[i] * np.clip(self.K[i] / self.CM[i] * e[i] + self.X[self.order], -1., 1.)
|
||||
return self.X
|
||||
|
||||
|
||||
class SatRefNaiveSecOrder:
|
||||
""" Second Order (brutally) Saturated Reference Model"""
|
||||
|
||||
def __init__(self, K, sats):
|
||||
"""
|
||||
K: coefficients of the caracteristic polynomial, in ascending power order,
|
||||
highest power ommited (normalized to -1)
|
||||
sats: saturations for each order in ascending order
|
||||
"""
|
||||
self.K = K
|
||||
self.sats = sats
|
||||
self.order = len(K)
|
||||
self.X = np.zeros(self.order + 1)
|
||||
|
||||
def run(self, dt, sp):
|
||||
self.X[:self.order] += self.X[1:self.order + 1] * dt
|
||||
e = np.array(self.X[:self.order])
|
||||
e[0] -= sp
|
||||
self.X[self.order] = np.sum(e * self.K)
|
||||
self.X[self.order] = np.clip(self.X[self.order], -self.sats[1], self.sats[1]) # saturate accel
|
||||
if self.X[self.order - 1] >= self.sats[0]: # saturate vel, trim accel
|
||||
self.X[self.order - 1] = self.sats[0]
|
||||
if self.X[self.order] > 0:
|
||||
self.X[self.order] = 0
|
||||
elif self.X[self.order - 1] <= -self.sats[0]:
|
||||
self.X[self.order - 1] = -self.sats[0]
|
||||
if self.X[self.order] < 0:
|
||||
self.X[self.order] = 0
|
||||
return self.X
|
||||
|
||||
|
||||
def butterworth(n, omega=1):
|
||||
"""
|
||||
returns the coefficients of a butterworth polynomial of order n in ascending powers order
|
||||
see: http://en.wikipedia.org/wiki/Butterworth_filter
|
||||
"""
|
||||
poles = [omega * np.exp(complex(0, 2 * k + n - 1) * math.pi / 2 / n) for k in range(1, n + 1)]
|
||||
coefs = -np.poly(poles)[::-1]
|
||||
return np.real(coefs[:-1])
|
||||
|
||||
|
||||
def bessel(n, omega=1):
|
||||
"""
|
||||
returns the coefficients of a bessel polynomial of order n in ascending powers order
|
||||
see http://en.wikipedia.org/wiki/Bessel_filter
|
||||
"""
|
||||
coefs = [-math.factorial(2 * n - k) / (math.pow(2, n - k) * math.factorial(k) * math.factorial(n - k)) for k in
|
||||
range(0, n)]
|
||||
return coefs[::-1]
|
||||
|
||||
|
||||
def poles(K):
|
||||
""" returns the roots of a polynomial whose coefficients are provided in ascending order,
|
||||
highest power coefficients normalized to -1
|
||||
"""
|
||||
p = np.zeros(len(K) + 1)
|
||||
p[0] = -1
|
||||
p[1:] = K[::-1]
|
||||
return np.roots(p)
|
||||
@@ -0,0 +1,259 @@
|
||||
#
|
||||
# Copyright 2013-2014 Antoine Drouin (poinix@gmail.com)
|
||||
#
|
||||
# This file is part of PAT.
|
||||
#
|
||||
# PAT is free software: you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation, either version 3 of the License, or
|
||||
# (at your option) any later version.
|
||||
#
|
||||
# PAT is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with PAT. If not, see <http://www.gnu.org/licenses/>.
|
||||
#
|
||||
|
||||
"""
|
||||
Utility functions
|
||||
"""
|
||||
import math
|
||||
import numpy as np
|
||||
import numpy.linalg as linalg
|
||||
import pdb
|
||||
|
||||
"""
|
||||
Unit convertions
|
||||
"""
|
||||
|
||||
|
||||
def rad_of_deg(d):
|
||||
return d / 180. * math.pi
|
||||
|
||||
|
||||
def sqrad_of_sqdeg(d):
|
||||
return d / (180. * math.pi) ** 2
|
||||
|
||||
|
||||
def deg_of_rad(r):
|
||||
return r * 180. / math.pi
|
||||
|
||||
|
||||
def sqdeg_of_sqrad(r):
|
||||
return r * (180. / math.pi) ** 2
|
||||
|
||||
|
||||
def rps_of_rpm(r):
|
||||
return r * 2. * math.pi / 60.
|
||||
|
||||
|
||||
def rpm_of_rps(r):
|
||||
return r / 2. / math.pi * 60.
|
||||
|
||||
|
||||
# http://en.wikipedia.org/wiki/Nautical_mile
|
||||
def m_of_NM(nm):
|
||||
return nm * 1852.
|
||||
|
||||
|
||||
def NM_of_m(m):
|
||||
return m / 1852.
|
||||
|
||||
|
||||
# http://en.wikipedia.org/wiki/Knot_(speed)
|
||||
def mps_of_kt(kt):
|
||||
return kt * 0.514444
|
||||
|
||||
|
||||
def kt_of_mps(mps):
|
||||
return mps / 0.514444
|
||||
|
||||
|
||||
# http://en.wikipedia.org/wiki/Foot_(unit)
|
||||
def m_of_ft(ft):
|
||||
return ft * 0.3048
|
||||
|
||||
|
||||
def ft_of_m(m):
|
||||
return m / 0.3048
|
||||
|
||||
|
||||
# feet per minute to/from meters per second
|
||||
def ftpm_of_mps(mps):
|
||||
return mps * 60. * 3.28084
|
||||
|
||||
|
||||
def mps_of_ftpm(ftpm):
|
||||
return ftpm / 60. / 3.28084
|
||||
|
||||
|
||||
"""
|
||||
Cliping
|
||||
"""
|
||||
|
||||
|
||||
def norm_angle_0_2pi(a):
|
||||
while a > 2. * math.pi:
|
||||
a -= 2. * math.pi
|
||||
while a <= 0:
|
||||
a += 2. * math.pi
|
||||
return a
|
||||
|
||||
|
||||
def norm_angle_mpi_pi(a):
|
||||
while a > math.pi:
|
||||
a -= 2. * math.pi
|
||||
while a <= -math.pi:
|
||||
a += 2. * math.pi
|
||||
return a
|
||||
|
||||
|
||||
#
|
||||
def saturate(_v, _min, _max):
|
||||
if _v < _min:
|
||||
return _min
|
||||
if _v > _max:
|
||||
return _max
|
||||
return _v
|
||||
|
||||
|
||||
"""
|
||||
Plotting
|
||||
"""
|
||||
import matplotlib
|
||||
import matplotlib.pyplot as plt
|
||||
import matplotlib.gridspec as gridspec
|
||||
|
||||
my_title_spec = {'color': 'k', 'fontsize': 20}
|
||||
|
||||
|
||||
def save_if(filename):
|
||||
if filename: matplotlib.pyplot.savefig(filename, dpi=80)
|
||||
|
||||
|
||||
def prepare_fig(fig=None, window_title=None, figsize=(20.48, 10.24), margins=None):
|
||||
if fig is None:
|
||||
fig = plt.figure(figsize=figsize)
|
||||
# else:
|
||||
# plt.figure(fig.number)
|
||||
if margins:
|
||||
left, bottom, right, top, wspace, hspace = margins
|
||||
fig.subplots_adjust(left=left, right=right, bottom=bottom, top=top,
|
||||
hspace=hspace, wspace=wspace)
|
||||
if window_title:
|
||||
fig.canvas.set_window_title(window_title)
|
||||
return fig
|
||||
|
||||
|
||||
def decorate(ax, title=None, xlab=None, ylab=None, legend=None, xlim=None, ylim=None):
|
||||
ax.xaxis.grid(color='k', linestyle='-', linewidth=0.2)
|
||||
ax.yaxis.grid(color='k', linestyle='-', linewidth=0.2)
|
||||
if xlab:
|
||||
ax.xaxis.set_label_text(xlab)
|
||||
if ylab:
|
||||
ax.yaxis.set_label_text(ylab)
|
||||
if title:
|
||||
ax.set_title(title, my_title_spec)
|
||||
if legend is not None:
|
||||
ax.legend(legend, loc='best')
|
||||
if xlim is not None:
|
||||
ax.set_xlim(xlim[0], xlim[1])
|
||||
if ylim is not None:
|
||||
ax.set_ylim(ylim[0], ylim[1])
|
||||
|
||||
|
||||
def ensure_ylim(ax, yspan):
|
||||
ymin, ymax = ax.get_ylim()
|
||||
if ymax - ymin < yspan:
|
||||
ym = (ymin + ymax) / 2
|
||||
ax.set_ylim(ym - yspan / 2, ym + yspan / 2)
|
||||
|
||||
|
||||
def write_text(nrows, ncols, plot_number, text, colspan=1, loc=[[0.5, 9.7]], filename=None):
|
||||
# ax = plt.subplot(nrows, ncols, plot_number)
|
||||
gs = gridspec.GridSpec(nrows, ncols)
|
||||
row, col = divmod(plot_number - 1, ncols)
|
||||
ax = plt.subplot(gs[row, col:col + colspan])
|
||||
plt.axis([0, 10, 0, 10])
|
||||
ax.get_xaxis().set_visible(False)
|
||||
ax.get_yaxis().set_visible(False)
|
||||
for i in range(0, len(text)):
|
||||
plt.text(loc[i][0], loc[i][1], text[i], ha='left', va='top')
|
||||
save_if(filename)
|
||||
|
||||
|
||||
def plot_in_grid(time, plots, ncol, figure=None, window_title="None", legend=None, filename=None,
|
||||
margins=(0.04, 0.08, 0.93, 0.96, 0.20, 0.34)):
|
||||
nrow = math.ceil(len(plots) / float(ncol))
|
||||
figsize = (10.24 * ncol, 2.56 * nrow)
|
||||
figure = prepare_fig(figure, window_title, figsize=figsize, margins=margins)
|
||||
# pdb.set_trace()
|
||||
for i, (title, ylab, data) in enumerate(plots):
|
||||
ax = figure.add_subplot(nrow, ncol, i + 1)
|
||||
ax.plot(time, data)
|
||||
decorate(ax, title=title, ylab=ylab)
|
||||
if legend is not None:
|
||||
ax.legend(legend, loc='best')
|
||||
save_if(filename)
|
||||
return figure
|
||||
|
||||
|
||||
"""
|
||||
Misc
|
||||
"""
|
||||
|
||||
|
||||
def num_jacobian(X, U, P, dyn):
|
||||
s_size = len(X)
|
||||
i_size = len(U)
|
||||
epsilonX = (0.1 * np.ones(s_size)).tolist()
|
||||
dX = np.diag(epsilonX)
|
||||
A = np.zeros((s_size, s_size))
|
||||
for i in range(0, s_size):
|
||||
dx = dX[i, :]
|
||||
delta_f = dyn(X + dx / 2, 0, U, P) - dyn(X - dx / 2, 0, U, P)
|
||||
delta_f = delta_f / dx[i]
|
||||
# print delta_f
|
||||
A[:, i] = delta_f
|
||||
|
||||
epsilonU = (0.1 * np.ones(i_size)).tolist()
|
||||
dU = np.diag(epsilonU)
|
||||
B = np.zeros((s_size, i_size))
|
||||
for i in range(0, i_size):
|
||||
du = dU[i, :]
|
||||
delta_f = dyn(X, 0, U + du / 2, P) - dyn(X, 0, U - du / 2, P)
|
||||
delta_f = delta_f / du[i]
|
||||
B[:, i] = delta_f
|
||||
|
||||
return A, B
|
||||
|
||||
|
||||
def saturate(V, Sats):
|
||||
Vsat = np.array(V)
|
||||
for i in range(0, len(V)):
|
||||
if Vsat[i] < Sats[i, 0]:
|
||||
Vsat[i] = Sats[i, 0]
|
||||
elif Vsat[i] > Sats[i, 1]:
|
||||
Vsat[i] = Sats[i, 1]
|
||||
return Vsat
|
||||
|
||||
|
||||
def print_lti_dynamics(A, B, txt=None, print_original_form=False, print_modal_form=False):
|
||||
if txt:
|
||||
print txt
|
||||
if print_original_form:
|
||||
print "A\n", A
|
||||
print "B\n", B
|
||||
w, M = np.linalg.eig(A)
|
||||
print "modes \n", w
|
||||
if print_modal_form:
|
||||
# print "eigen vectors\n", M
|
||||
# invM = np.linalg.inv(M)
|
||||
# print "invM\n", invM
|
||||
# Amod = np.dot(np.dot(invM, A), M)
|
||||
# print "Amod\n", Amod
|
||||
for i in range(len(w)):
|
||||
print w[i], "->", M[:, i]
|
||||
Executable
+104
@@ -0,0 +1,104 @@
|
||||
#!/usr/bin/env python
|
||||
# -*- coding: utf-8 -*-
|
||||
#
|
||||
# Copyright (C) 2014 Antoine Drouin
|
||||
#
|
||||
# This file is part of paparazzi.
|
||||
#
|
||||
# paparazzi is free software; you can redistribute it and/or modify
|
||||
# it under the terms of the GNU General Public License as published by
|
||||
# the Free Software Foundation; either version 2, or (at your option)
|
||||
# any later version.
|
||||
#
|
||||
# paparazzi is distributed in the hope that it will be useful,
|
||||
# but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
# GNU General Public License for more details.
|
||||
#
|
||||
# You should have received a copy of the GNU General Public License
|
||||
# along with paparazzi; see the file COPYING. If not, write to
|
||||
# the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
# Boston, MA 02111-1307, USA.
|
||||
#
|
||||
|
||||
import math
|
||||
import numpy as np
|
||||
import scipy.signal
|
||||
import matplotlib.pyplot as plt
|
||||
|
||||
import pat.utils as pu
|
||||
import pat.algebra as pa
|
||||
|
||||
import control as ctl
|
||||
|
||||
|
||||
def random_setpoint(time, dt_step=2):
|
||||
tf = time[0]
|
||||
sp = np.zeros((len(time), 3))
|
||||
sp_i = [0, 0, 0]
|
||||
for i in range(0, len(time)):
|
||||
if time[i] >= tf:
|
||||
ui = np.random.rand(3) - [0.5, 0.5, 0.5];
|
||||
ai = np.random.rand(1)
|
||||
n = np.linalg.norm(ui)
|
||||
if n > 0:
|
||||
ui /= n
|
||||
sp_i = pa.euler_of_quat(pa.quat_of_axis_angle(ui, ai))
|
||||
tf += dt_step
|
||||
sp[i] = sp_i
|
||||
return sp
|
||||
|
||||
|
||||
def test_ref(r, time, setpoint):
|
||||
ref = np.zeros((len(time), 9))
|
||||
for i in range(1, time.size):
|
||||
r.update_euler(setpoint[i], time[i] - time[i - 1])
|
||||
ref[i] = np.concatenate((r.euler, r.vel, r.accel))
|
||||
return ref
|
||||
|
||||
|
||||
def plot_ref(time, xref=None, sp=None, figure=None):
|
||||
margins = (0.05, 0.05, 0.98, 0.96, 0.20, 0.34)
|
||||
figure = pu.prepare_fig(figure, window_title='Reference', figsize=(20.48, 10.24), margins=margins)
|
||||
plots = [("$\phi$", "deg"), ("$\\theta$", "deg"), ("$\\psi$", "deg"),
|
||||
("$p$", "deg/s"), ("$q$", "deg/s"), ("$r$", "deg/s"),
|
||||
("$\dot{p}$", "deg/s2"), ("$\dot{q}$", "deg/s2"), ("$\dot{r}$", "deg/s2")]
|
||||
for i, (title, ylab) in enumerate(plots):
|
||||
ax = plt.subplot(3, 3, i + 1)
|
||||
if xref is not None: plt.plot(time, pu.deg_of_rad(xref[:, i]))
|
||||
pu.decorate(ax, title=title, ylab=ylab)
|
||||
if sp is not None and i < 3:
|
||||
plt.plot(time, pu.deg_of_rad(sp[:, i]))
|
||||
return figure
|
||||
|
||||
|
||||
dt = 1. / 512.
|
||||
time = np.arange(0., 4, dt)
|
||||
sp = np.zeros((len(time), 3))
|
||||
sp[:, 0] = pu.rad_of_deg(45.) * scipy.signal.square(math.pi / 2 * time + math.pi)
|
||||
# sp[:, 1] = pu.rad_of_deg(5.)*scipy.signal.square(math.pi/2*time)
|
||||
# sp[:, 2] = pu.rad_of_deg(45.)
|
||||
# sp = random_setpoint(time)
|
||||
|
||||
# rs = [ctl.att_ref_analytic_disc(axis=0), ctl.att_ref_analytic_cont(axis=0), ctl.att_ref(), ctl.att_ref_native('floating_point')]
|
||||
# rs = [ctl.att_ref(), ctl.att_ref_native('floating_point'), ctl.att_ref_native('fixed_point')]
|
||||
|
||||
|
||||
args = {'omega': 10., 'xi': 0.7, 'sat_vel': pu.rad_of_deg(150.), 'sat_accel': pu.rad_of_deg(1800),
|
||||
'sat_jerk': pu.rad_of_deg(27000)}
|
||||
# rs = [ctl.att_ref(**args)]
|
||||
# rs = [ctl.att_ref_sat_naive(**args)]
|
||||
# rs = [ctl.att_ref_sat_nested(**args)]
|
||||
# rs = [ctl.att_ref(**args), ctl.att_ref_sat_naive(**args), ctl.att_ref_sat_nested(**args)]
|
||||
# rs = [ctl.att_ref_sat_naive(**args), ctl.att_ref_sat_nested2(**args)]
|
||||
# rs = [ctl.att_ref_sat_nested(**args), ctl.att_ref_sat_nested2(**args)]
|
||||
rs = [ctl.att_ref_sat_naive(**args), ctl.att_ref_sat_nested(**args), ctl.att_ref_sat_nested2(**args)]
|
||||
|
||||
xrs = [test_ref(r, time, sp) for r in rs]
|
||||
figure = None
|
||||
for xr in xrs: figure = plot_ref(time, xr, None, figure)
|
||||
figure = plot_ref(time, None, sp, figure)
|
||||
legends = [r.name for r in rs] + ['Setpoint']
|
||||
plt.subplot(3, 3, 3)
|
||||
plt.legend(legends)
|
||||
plt.show()
|
||||
Reference in New Issue
Block a user