179 lines
8.6 KiB
Python
179 lines
8.6 KiB
Python
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 |