swangnice/rotor_py_control
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Init

Browse Source
This commit is contained in:
spencerfolk
2023-03-15 15:38:14 -04:00
commit 4d7fca10e4
62 changed files with 5891 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
rotorpy/estimators/README.md Normal file
View File

@@ -0,0 +1,13 @@
# Estimator Module
Estimators take measurements from sensors and an internal model of the dynamcis to provide filtered estimates of states or quantities of interest.
Currently we provide two examples of estimators that estimate the local wind vector acting on the vehicle center of mass based on measurements from the IMU and external motion capture--one is a hand-implemented EKF and the other is a UKF that uses the `FilterPy` library for easy implementation.
Both models use a simplified model of the dynamics for their process models. The filter states are: $$\boldsymbol{x}_{filter} = [\psi, \theta, \phi, v_x, v_y, v_z, w_x, w_y, w_z]^\top $$ where $[\psi, \theta, \phi]$ are the ZYX Euler angles, $[v_x, v_y, v_z]$ are translational velocities in the body frame, and $[w_x, w_y, w_z]$ are the wind components in the body frame. Small angle approximations near hover are used to simplify the process model. The model assumes the predominant form of drag is parasitic, or quadratic with airspeed. In other words, rotor drag is neglected. These lumped drag coefficients can be identified using flight data in the absence of wind.
The inputs to the process model are the commanded thrust from the controller, $f_c$, and the body rates measured by the external motion capture sensor, $\Omega$.
Noisy measurements of acceleration are provided by the IMU sensor, and attitude and body velocities are measured by the external motion capture sensor.
If you'd like to write your own estimator, whether it is for parameter or state estimation, you can reference `nullestimator.py` for a template.

0
rotorpy/estimators/__init__.py Normal file
View File

77
rotorpy/estimators/nullestimator.py Normal file
View File

@@ -0,0 +1,77 @@
import numpy as np
from scipy.spatial.transform import Rotation
import copy
"""
This is the null estimator, which outputs nothing. This code serves two purposes:
1) When the user does not supply an estimator, the null estimator is the default "estimator" for the simulator.
2) If a user wants to write their own estimator, the null estimator is good stub code because it specifies the input/output structure used by the simulator.
"""
class NullEstimator:
"""
NullEstimator
Description of your filter goes here.
State space:
Describe your filter state space here, being as specific as necessary and ideally specifying types, too.
e.g. X = [x,y,z,xdot,ydot,zdot]
Measurement space:
Describe your filter measurement space here, being as specific as necessary and ideally specifying types, too.
e.g. Y = [imu_x, imu_y, imu_z]
"""
def __init__(self):
"""
Here you would specify important design parameters for your estimator, such as noise matrices, initial state, or other tuning parameters.
For the null estimator, the initial state and covariance are specified (but are default values anyways)
"""
def step(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
"""
The step command will update the filter based on the following.
Inputs:
ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors.
controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model.
imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
is specific acceleration, i.e., total force minus gravity.
mocap_measurement, provides noisy measurements of pose and twist.
Outputs:
A dictionary with the following keys:
filter_state, containing the current filter estimate.
covariance, containing the current covariance matrix.
The ground truth state is supplied in case you would like to have "knowns" in your filter, or otherwise manipulate the state to create a custom measurement of your own desires.
IMU measurements are commonly used in filter setups, so we already supply these measurements as an input into the system.
Motion capture measurements are useful if you want noisy measurements of the pose and twist of the vehicle.
"""
self.propagate(ground_truth_state, controller_command)
self.update(ground_truth_state, controller_command, imu_measurement, mocap_measurement)
return {'filter_state': [], 'covariance': []}
""" OPTIONAL """
def propagate(self, ground_truth_state, controller_command):
"""
Propagate occurs whenever an action u is taken.
Inputs:
ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors.
controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model.
"""
return {'filter_state': [], 'covariance': []}
""" OPTIONAL """
def update(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
"""
Update the estimate based on new sensor measurments.
Inputs:
ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors.
controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model.
imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
is specific acceleration, i.e., total force minus gravity.
mocap_measurement, provides noisy measurements of pose and twist.
"""
return {'filter_state': [], 'covariance': []}

239
rotorpy/estimators/wind_ekf.py Normal file
View File

@@ -0,0 +1,239 @@
import numpy as np
from scipy.spatial.transform import Rotation
import copy
class WindEKF:
"""
Wind EKF:
Given approximate dynamics near level flight, the wind EKF will produce an estimate of the local wind vector acting on the body.
It requires knowledge of the effective drag coefficient on each axis, which would be determined either from real flight or computed in simulation, and the mass of the vehicle.
The inputs to the filter are the mass normalized collective thrust and the body rates on each axis.
Measurements of body velocity, Euler angles, and acceleration are provided to the vehicle.
The filter estimates the Euler angles, body velocities, and wind velocities.
State space:
xhat = [phi, theta, psi, u, v, w, windx, windy, windz]
Input space:
u = [T/m, p, q, r]
"""
def __init__(self, quad_params, Q=np.diag(np.concatenate([0.5*np.ones(3),0.7*np.ones(3),0.1*np.ones(3)])),
R=np.diag(np.concatenate([0.0005*np.ones(3),0.0010*np.ones(3),np.sqrt(100/2)*(0.38**2)*np.ones(3)])),
xhat0=np.array([0,0,0, 0.1,0.05,0.02, 1.5,1.5,1.5]),
P0=1*np.eye(9),
dt=1/100):
"""
Inputs:
quad_params, dict with keys specified in quadrotor_params.py
Q, the process noise covariance
R, the measurement noise covariance
x0, the initial filter state
P0, the initial state covariance
dt, the time between predictions
"""
# Quadrotor physical parameters.
# Inertial parameters
self.mass = quad_params['mass'] # kg
# Frame parameters
self.c_Dx = quad_params['c_Dx'] # drag coeff, N/(m/s)**2
self.c_Dy = quad_params['c_Dy'] # drag coeff, N/(m/s)**2
self.c_Dz = quad_params['c_Dz'] # drag coeff, N/(m/s)**2
self.g = 9.81 # m/s^2
# Filter parameters
self.Q = Q
self.R = R
self.xhat = xhat0
self.P = P0
self.innovation = np.zeros((9,))
self.dt = dt
# Initialize the Jacobians at starting position and assuming hover thrust.
self.computeJacobians(self.xhat, np.array([self.g, 0, 0, 0]))
def step(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
"""
This will perform both a propagate and update step in one for the sake of readability in other parts of the code.
"""
self.propagate(ground_truth_state, controller_command)
self.update(ground_truth_state, controller_command, imu_measurement, mocap_measurement)
return self.pack_results()
def propagate(self, ground_truth_state, controller_command):
"""
Propagate occurs whenever an action u is taken.
Inputs:
ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors.
controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model.
Outputs:
xhat, the current state estimate after propagation
P, the current covariance matrix after propagation
"""
# Extract the appropriate u vector based on the controller commands.
uk = self.construct_control_vector(ground_truth_state, controller_command)
# First propagate the dynamics using the process model
self.xhat = self.process_model(self.xhat, uk)
# Update the covariance matrix using the linearized version of the dynamics
self.computeJacobians(self.xhat, uk)
self.P = self.A@self.P@(self.A.T) + self.Q
return self.pack_results
def update(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
"""
Update the estimate based on new sensor measurments.
Inputs:
ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors.
controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model.
imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
is specific acceleration, i.e., total force minus gravity.
mocap_measurement, provides noisy measurements of pose and twist.
Outputs:
xhat, the current state estimate after measurement update
P, the current covariance matrix after measurement update
"""
# Extract the appropriate u vector based on the controller commands.
uk = self.construct_control_vector(ground_truth_state, controller_command)
# Construct the measurement vector yk
orientation = Rotation.from_quat(copy.deepcopy(mocap_measurement['q']))
euler_angles = orientation.as_euler('zyx', degrees=False) # Get Euler angles from current orientation
inertial_speed = mocap_measurement['v']
body_speed = (orientation.as_matrix()).T@inertial_speed
yk = np.array([euler_angles[0], # phi
euler_angles[1], # theta
euler_angles[2], # psi
body_speed[0], # vx
body_speed[1], # vy
body_speed[2], # vz
imu_measurement['accel'][0], # body x acceleration
imu_measurement['accel'][1], # body y acceleration
imu_measurement['accel'][2] # body z acceleration
])
# First linearize the measurement model.
self.computeJacobians(self.xhat, uk)
# Now compute the Kalman gain
K = self.P@(self.C.T)@np.linalg.inv(self.C@self.P@(self.C.T) + self.R)
# Next compute the posteriori distribution
self.innovation = yk - self.measurement_model(self.xhat, uk)
self.xhat = self.xhat + K@self.innovation
self.P = (np.eye(self.xhat.shape[0]) - K@self.C)@self.P
return self.pack_results()
def process_model(self, xk, uk):
"""
Process model
"""
va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2) # Compute the norm of the airspeed vector
# The process model is integrated using forward Euler.
xdot = np.array([uk[1] + xk[0]*xk[1]*uk[2] + xk[2]*uk[3],
uk[2] - xk[0]*uk[3],
xk[0]*uk[2] + uk[3],
-self.c_Dx/self.mass*(xk[3]-xk[6])*va + self.g*xk[1] + xk[4]*uk[3] - xk[5]*uk[1],
-self.c_Dy/self.mass*(xk[4]-xk[7])*va - self.g*xk[0] + xk[5]*uk[1] - xk[3]*uk[3],
uk[0] - self.c_Dz/self.mass*(xk[5]-xk[8])*va - self.g + xk[3]*uk[2] - xk[4]*uk[1],
0,
0,
0])
xkp1 = xk + xdot*self.dt
return xkp1
def measurement_model(self, xk, uk):
"""
Measurement model
"""
h = np.zeros(xk.shape)
va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2) # Compute the norm of the airspeed vector
h[0:3] = np.hstack((np.eye(3), np.zeros((3,6))))@(xk)
h[3:6] = np.hstack((np.zeros((3,3)), np.eye(3), np.zeros((3,3))))@(xk)
h[6:] = np.array([-self.c_Dx/self.mass*(xk[3]-xk[6])*va,
-self.c_Dy/self.mass*(xk[4]-xk[7])*va,
uk[0]-self.c_Dz/self.mass*(xk[5]-xk[8])*va])
return h
def computeJacobians(self, x, u):
"""
Compute the Jacobians of the process and measurement model at the operating points x and u.
"""
va = np.sqrt((x[3]-x[6])**2 + (x[4]-x[7])**2 + (x[5]-x[8])**2) # Compute the norm of the airspeed vector
# Partial derivatives of va for chain rule
dvadu = (x[3]-x[6])/va
dvadv = (x[4]-x[7])/va
dvadw = (x[5]-x[8])/va
dvadwx = -dvadu
dvadwy = -dvadv
dvadwz = -dvadw
kx = self.c_Dx/self.mass
ky = self.c_Dy/self.mass
kz = self.c_Dz/self.mass
vax = x[3] - x[6]
vay = x[4] - x[7]
vaz = x[5] - x[8]
self.A = np.array([[x[1]*u[2], x[0]*u[2] + u[3], 0, 0, 0, 0, 0, 0, 0],
[-u[3], 0, 0, 0, 0, 0, 0, 0, 0],
[ u[2], 0, 0, 0, 0, 0, 0, 0, 0],
[0, self.g, 0, -kx*(dvadu*vax + va), u[3] - kx*(dvadv*vax), -u[1] - kx*(dvadw*vax), -kx*(dvadwx*vax-va), -kx*(dvadwy*vax), -kx*(dvadwz*vax)],
[-self.g, 0, 0, -ky*(dvadu*vay)-u[3], -ky*(dvadv*vay + va), u[1]-ky*(dvadw*vay), -ky*(dvadwx*vay), -ky*(dvadv*vay - va), -ky*(dvadw*vay)],
[0, 0, 0, u[2] - kz*(dvadu*vaz), -u[1] - kz*(dvadv*vaz), -kz*(dvadw*vaz + va), -kz*(dvadwx*vaz), -kz*(dvadwy*vaz), -kz*(dvadwz*vaz - va)],
[0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 0, 0, 0, 0]])
self.C = np.array([[1, 0, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, 0, 0, 0, 0, 0],
[0, 0, 1, 0, 0, 0, 0, 0, 0],
[0, 0, 0, 1, 0, 0, 0, 0, 0],
[0, 0, 0, 0, 1, 0, 0, 0, 0],
[0, 0, 0, 0, 0, 1, 0, 0, 0],
[0, 0, 0, -kx*(dvadu*vax + va), -kx*(dvadv*vax), -kx*(dvadw*vax), -kx*(dvadwx*vax-va), -kx*(dvadwy*vax), -kx*(dvadwz*vax)],
[0, 0, 0, -ky*(dvadu*vay), -ky*(dvadv*vay + va), -ky*(dvadw*vay), -ky*(dvadwx*vay), -ky*(dvadv*vay - va), -ky*(dvadw*vay)],
[0, 0, 0, -kz*(dvadu*vaz), -kz*(dvadv*vaz), -kz*(dvadw*vaz + va), -kz*(dvadwx*vaz), -kz*(dvadwy*vaz), -kz*(dvadwz*vaz - va)]])
return
def construct_control_vector(self, ground_truth_state, controller_command):
"""
Constructs control vector
"""
uk = np.array([controller_command['cmd_thrust']/self.mass, # Compute mass normalized thrust from the command thrust, note that this is not the actual thrust...
ground_truth_state['w'][0], # Body rate in x axis
ground_truth_state['w'][1], # Body rate in y axis
ground_truth_state['w'][2]] # Body rate in z axis
)
return uk
def pack_results(self):
# return {'euler_est': self.xhat[0:3], 'v_est': self.xhat[3:6], 'wind_est': self.xhat[6:9],
# 'covariance': self.P, 'innovation': self.innovation}
return {'filter_state': self.xhat, 'covariance': self.P}

179
rotorpy/estimators/wind_ukf.py Normal file
View File

@@ -0,0 +1,179 @@
import numpy as np
from scipy.spatial.transform import Rotation
import copy
from filterpy.kalman import UnscentedKalmanFilter
from filterpy.kalman import MerweScaledSigmaPoints
"""
The Wind UKF uses the same model as the EKF found in wind_ekf.py, but instead applies the Unscented Kalman Filter. The benefit
of this approach is the accuracy of the UKF is 3rd order (compared to EKF's 1st order), and Jacobians do not need to be computed.
"""
class WindUKF:
"""
WindUKF
Given approximate dynamics near level flight, the wind EKF will produce an estimate of the local wind vector acting on the body.
It requires knowledge of the effective drag coefficient on each axis, which would be determined either from real flight or computed in simulation, and the mass of the vehicle.
The inputs to the filter are the mass normalized collective thrust and the body rates on each axis.
Measurements of body velocity, Euler angles, and acceleration are provided to the vehicle.
The filter estimates the Euler angles, body velocities, and wind velocities.
State space:
xhat = [psi, theta, phi, u, v, w, windx, windy, windz]
Measurement space:
u = [T/m, p, q, r]
"""
def __init__(self, quad_params,
Q=np.diag(np.concatenate([0.05*np.ones(3),0.07*np.ones(3),0.01*np.ones(3)])),
R=np.diag(np.concatenate([0.0005*np.ones(3),0.0010*np.ones(3),np.sqrt(100/2)*(0.38**2)*np.ones(3)])),
xhat0=np.array([0,0,0, 0.1,0.05,0.02, 1.5,1.5,1.5]),
P0=1*np.eye(9),
dt=1/100,
alpha=0.1,
beta=2.0,
kappa=-1):
"""
Inputs:
quad_params, dict with keys specified in quadrotor_params.py
Q, the process noise covariance
R, the measurement noise covariance
x0, the initial filter state
P0, the initial state covariance
dt, the time between predictions
"""
self.mass = quad_params['mass'] # kg
# Frame parameters
self.c_Dx = quad_params['c_Dx'] # drag coeff, N/(m/s)**2
self.c_Dy = quad_params['c_Dy'] # drag coeff, N/(m/s)**2
self.c_Dz = quad_params['c_Dz'] # drag coeff, N/(m/s)**2
self.g = 9.81 # m/s^2
# Filter parameters
self.xhat = xhat0
self.P = P0
self.dt = dt
self.N = self.xhat.shape[0]
self.points = MerweScaledSigmaPoints(self.N, alpha=alpha, beta=beta, kappa=kappa)
self.uk = np.array([self.g, 0, 0, 0])
self.filter = UnscentedKalmanFilter(dim_x=self.N, dim_z=self.N, dt=dt, fx=self.f, hx=self.h, points=self.points)
self.filter.x = xhat0
self.filter.P = P0
self.filter.R = R
self.filter.Q = Q
def step(self, ground_truth_state, controller_command, imu_measurement, mocap_measurement):
"""
The step command will update the filter based on the following.
Inputs:
ground_truth_state, the ground truth state is mainly there if it's necessary to compute certain portions of the state, e.g., actual thrust produced by the rotors.
controller_command, the controller command taken, this has to be converted to the appropriate control vector u depending on the filter model.
imu_measurement, contains measurements from an inertial measurement unit. These measurements are noisy, potentially biased, and potentially off-axis. The measurement
is specific acceleration, i.e., total force minus gravity.
mocap_measurement, provides noisy measurements of pose and twist.
Outputs:
A dictionary with the following keys:
filter_state, containing the current filter estimate.
covariance, containing the current covariance matrix.
The ground truth state is supplied in case you would like to have "knowns" in your filter, or otherwise manipulate the state to create a custom measurement of your own desires.
IMU measurements are commonly used in filter setups, so we already supply these measurements as an input into the system.
Motion capture measurements are useful if you want noisy measurements of the pose and twist of the vehicle.
"""
# Extract the appropriate u vector based on the controller commands.
self.uk = self.construct_control_vector(ground_truth_state, controller_command)
# Construct the measurement vector yk
orientation = Rotation.from_quat(copy.deepcopy(mocap_measurement['q']))
euler_angles = orientation.as_euler('zyx', degrees=False) # Get Euler angles from current orientation
inertial_speed = mocap_measurement['v']
body_speed = (orientation.as_matrix()).T@inertial_speed
zk = np.array([euler_angles[0], # phi
euler_angles[1], # theta
euler_angles[2], # psi
body_speed[0], # vx
body_speed[1], # vy
body_speed[2], # vz
imu_measurement['accel'][0], # body x acceleration
imu_measurement['accel'][1], # body y acceleration
imu_measurement['accel'][2] # body z acceleration
])
self.filter.predict()
self.filter.update(zk)
return {'filter_state': self.filter.x, 'covariance': self.filter.P}
def f(self, xk, dt):
"""
Process model
"""
va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2) # Compute the norm of the airspeed vector
# The process model is integrated using forward Euler. Below assumes Euler angles are given in order of [phi, theta, psi] (XYZ)
# xdot = np.array([self.uk[1] + xk[0]*xk[1]*self.uk[2] + xk[2]*self.uk[3],
# self.uk[2] - xk[0]*self.uk[3],
# xk[0]*self.uk[2] + self.uk[3],
# -self.c_Dx/self.mass*(xk[3]-xk[6])*va + self.g*xk[1] + xk[4]*self.uk[3] - xk[5]*self.uk[1],
# -self.c_Dy/self.mass*(xk[4]-xk[7])*va - self.g*xk[0] + xk[5]*self.uk[1] - xk[3]*self.uk[3],
# self.uk[0] - self.c_Dz/self.mass*(xk[5]-xk[8])*va - self.g + xk[3]*self.uk[2] - xk[4]*self.uk[1],
# 0,
# 0,
# 0])
# The process model, below assumes Euler angles are given in the order of [psi, theta, phi] (ZYX)
xdot = np.array([xk[2]*self.uk[2] + self.uk[3],
self.uk[2] - xk[2]*self.uk[3],
self.uk[1] + xk[2]*xk[1]*self.uk[2] + xk[0]*self.uk[3],
-self.c_Dx/self.mass*(xk[3]-xk[6])*va + self.g*xk[1] + xk[4]*self.uk[3] - xk[5]*self.uk[1],
-self.c_Dy/self.mass*(xk[4]-xk[7])*va - self.g*xk[2] + xk[5]*self.uk[1] - xk[3]*self.uk[3],
self.uk[0] - self.c_Dz/self.mass*(xk[5]-xk[8])*va - self.g + xk[3]*self.uk[2] - xk[4]*self.uk[1],
0,
0,
0])
xkp1 = xk + xdot*dt
return xkp1
def h(self, xk):
"""
Measurement model
"""
h = np.zeros(xk.shape)
va = np.sqrt((xk[3]-xk[6])**2 + (xk[4]-xk[7])**2 + (xk[5]-xk[8])**2) # Compute the norm of the airspeed vector
h[0:3] = np.hstack((np.eye(3), np.zeros((3,6))))@(xk)
h[3:6] = np.hstack((np.zeros((3,3)), np.eye(3), np.zeros((3,3))))@(xk)
h[6:] = np.array([-self.c_Dx/self.mass*(xk[3]-xk[6])*va,
-self.c_Dy/self.mass*(xk[4]-xk[7])*va,
self.uk[0]-self.c_Dz/self.mass*(xk[5]-xk[8])*va])
return h
def construct_control_vector(self, ground_truth_state, controller_command):
"""
Constructs control vector
"""
uk = np.array([controller_command['cmd_thrust']/self.mass, # Compute mass normalized thrust from the command thrust, note that this is not the actual thrust...
ground_truth_state['w'][0], # Body rate in x axis
ground_truth_state['w'][1], # Body rate in y axis
ground_truth_state['w'][2]] # Body rate in z axis
)
return uk