def convert_dvl_velocities(sub_quat, dvl_vel):
# TODO This transform should maybe be in a configuration file.
# Or perhaps we should configure the DVL to do it for us.
vel_body_frame = Quaternion(hpr=(0, 0, 180)) * dvl_vel
hpr = sub_quat.hpr()
vel_spitz_frame = (Quaternion(hpr=(hpr[0]%360, 0, 0)).conjugate() * sub_quat) * vel_body_frame
return vel_spitz_frame
def fx_quat(x, dt):
q_initial, ang_vel = Quaternion(q=x[:4], unit=False), x[4:]
# I don't think the below is correct.
# Because HPR != axis of angular velocity
# TODO XXX FIX
disp_quat = Quaternion(hpr=([math.degrees(vel * dt) for vel in ang_vel]))
q_final = q_initial * disp_quat
x[0] = q_final[0]
x[1] = q_final[1]
x[2] = q_final[2]
x[3] = q_final[3]
return x
def quat_pid(self):
# TODO Figure out how to formalize this and use generic PID class.
g = kalman.get()
d = desires.get()
a = Quaternion(q=[g.q0, g.q1, g.q2, g.q3])
b = Quaternion(hpr=(d.heading, d.pitch, d.roll))
current_time = time.time()
dt = (current_time - self.last_quat_time) * self.speed
self.last_quat_time = current_time
q_diff = b * a.conjugate()
if abs(abs(q_diff[0]) - 1) < 1e-15:
self.last_q_error = np.array((0, 0, 0))
return np.array((0, 0, 0))
ax = q_diff.axis()
ang = q_diff.angle()
# Ensure we are taking the shortest path around.
if ang > math.pi:
ax = -ax
ang = 2*math.pi - ang
ang_accel = ang * ax * settings_quat.kP.get()
diff = (ang * ax - self.last_q_error) / dt
self.last_q_error = ang * ax
ang_accel += settings_quat.kD.get() * diff
# added by Christopher
#self.integral = self.integral + ax * dt
#ax += settings_quat.kI.get() * self.integral
return ang_accel
def quat_pid(self):
# TODO Figure out how to formalize this and use generic PID class.
g = kalman.get()
d = desires.get()
a = Quaternion(q=[g.q0, g.q1, g.q2, g.q3])
b = Quaternion(hpr=(d.heading, d.pitch, d.roll))
current_time = time.time()
dt = (current_time - self.last_quat_time) * self.speed
self.last_quat_time = current_time
q_diff = b * a.conjugate()
if abs(abs(q_diff[0]) - 1) < 1e-15:
self.last_q_error = np.array((0, 0, 0))
return np.array((0, 0, 0))
ax = q_diff.axis()
ang = q_diff.angle()
# Ensure we are taking the shortest path around.
if ang > math.pi:
ax = -ax
ang = 2 * math.pi - ang
ang_accel = ang * ax * settings_quat.kP.get()
diff = (ang * ax - self.last_q_error) / dt
self.last_q_error = ang * ax
ang_accel += settings_quat.kD.get() * diff
# added by Christopher
#self.integral = self.integral + ax * dt
#ax += settings_quat.kI.get() * self.integral
return ang_accel
def update(self,
heading,
x_vel,
x_acc,
y_vel,
y_acc,
depth,
wench,
active_measurements=None,
thruster_vals=None,
pitch=None,
roll=None):
if thruster_vals is None:
thruster_vals = []
#TODO: use active_measurements
heading = radians(heading)
pitch = radians(pitch)
roll = radians(roll)
#Data relative to north-east
z = array([
x_vel * cos(heading) - y_vel * sin(heading),
x_acc * cos(heading) - y_acc * sin(heading),
x_vel * sin(heading) + y_vel * cos(heading),
y_acc * sin(heading) + y_acc * cos(heading), depth
]).reshape(self.m, 1)
if active_measurements is not None:
z *= active_measurements
u = zeros((8, 1))
if len(thruster_vals) > 0:
# Resolve sub/world model discrepancies for x/y/z -> n/e forces
f_x = wench[0]
f_y = wench[1]
f_z = wench[2]
kalman_g = shm.kalman.get()
sub_quat = Quaternion(
q=[kalman_g.q0, kalman_g.q1, kalman_g.q2, kalman_g.q3])
# Convert from body or sub space to world space.
world_space_forces = sub_quat * array((f_x, f_y, f_z))
north_force = world_space_forces[0]
east_force = world_space_forces[1]
# Force*dt/mass = delta_v
u[0] = 0
u[2] = 0
#u[0] = north_force * self.dt / (vehicle_weight/9.8)
#u[2] = east_force * self.dt / (vehicle_weight/9.8)
#H m-by-n relates the state to the measurement
# Okay this takes some explaining
# We want to be able to measure a bias in x,y accelerations
# but are storing our state in north-east coordinates
has_dvl = 1 if dvl_present else 0
c = cos(heading)
s = sin(heading)
self.H = array([[has_dvl, 0, 0, 0, 0, 0, 0, 0],
[0, 1, 0, 0, c, -s, 0, 0],
[0, 0, has_dvl, 0, 0, 0,
0, 0], [0, 0, 0, 1, s, c, 0, 0],
[0, 0, 0, 0, 0, 0, 1, 0]]).reshape(self.m, self.n)
if active_measurements is not None:
self.H *= active_measurements
#self.H = array([ [0, 0, 0, 0, 0, 0, 0, 0],
# [0, 1, 0, 0, c,-s, 0, 0],
# [0, 0, 0, 0, 0, 0, 0, 0],
# [0, 0, 0, 1, s, c, 0, 0],
# [0, 0, 0, 0, 0, 0, 1, 0] ]).reshape(self.m,self.n)
# Iterate
xHat, P = self.Iterate(u, z)
vel_n = xHat[0][0]
acc_n = xHat[1][0]
vel_e = xHat[2][0]
acc_e = xHat[3][0]
depth = xHat[6][0]
depth_rate = xHat[7][0]
vel_x = vel_n * c + vel_e * s
vel_y = -vel_n * s + vel_e * c
acc_x = acc_n * c + acc_e * s
acc_y = -acc_n * s + acc_e * c
#print "biases: %0.2e %0.2e"%(xHat[4][0], xHat[5][0])
#We need to convert back to local (sub) coordinates here
self.forward += self.dt * vel_x
self.sway += self.dt * vel_y
#Already in north-east coordinates, no need to convert
self.north += self.dt * vel_n
self.east += self.dt * vel_e
return dict(north=self.north,
east=self.east,
forward=self.forward,
sway=self.sway,
velx=vel_x,
vely=vel_y,
accelx=acc_x,
accely=acc_y,
depth=depth,
depth_rate=depth_rate)
def get_orientation(quat_values):
q_offset = Quaternion(hpr=(shm.kalman_settings.heading_offset.get(), 0, 0))
quat = Quaternion(q=quat_values)
return q_offset * quat
import numpy as np
import shm
from auv_math.quat import Quaternion
from auv_python_helpers.angles import abs_heading_sub_degrees
from conf.vehicle import sensors, dvl_present, gx_hpr, dvl_offset
from settings import dt
from kalman_unscented import UnscentedKalmanFilter
from kalman_position import PositionFilter
from conf.vehicle import is_mainsub
# Offset for GX4 mounting orientation
GX_ORIENTATION_OFFSET = Quaternion(hpr=gx_hpr)
rec_get_attr = lambda s: \
reduce(lambda acc, e: getattr(acc, e),
s.split('.'), shm)
# Thruster array allows access to thruster values
thrusters = ['port', 'starboard', 'sway_fore', 'sway_aft']
heading_rate_var = rec_get_attr(sensors["heading_rate"])
pitch_rate_var = rec_get_attr(sensors["pitch_rate"])
roll_rate_var = rec_get_attr(sensors["roll_rate"])
depth_var = rec_get_attr(sensors["depth"])
depth_offset_var = rec_get_attr(sensors["depth_offset"])
quat_group = rec_get_attr(sensors["quaternion"])
def get_sub_quaternion(kalman=None):
if kalman is None:
kalman = shm.kalman.get()
return Quaternion(q=[kalman.q0, kalman.q1, kalman.q2, kalman.q3])
def accel_gyro_filter(state, accel, gyro, mags): acc_predict = predict_from_accel(accel) mag_predict = predict_from_mag(mags) state_predict = predict(state) gyro_predict = get_quat_ang_vel(state_predict)[0] return make_state(Quaternion(q=0.9*gyro_predict.q + 0.1*acc_predict.q), gyro)
def get_quat_ang_vel(state): return Quaternion(state[:4]), state[4:]
import shm
from LUTlinearizer import linearize
#from PolyLinearizer import linearize
from auv_math.quat import Quaternion
heading_input = shm.threedmg.heading
pitch_input = shm.threedmg.pitch
roll_input = shm.threedmg.roll
heading_output = shm.linear_heading.heading
quat_output = [
shm.linear_heading.q0, shm.linear_heading.q1, shm.linear_heading.q2,
shm.linear_heading.q3
]
watcher = shm.watchers.watcher()
watcher.watch(shm.threedmg)
group = shm.linear_heading
while (1):
watcher.wait()
linear_heading = linearize(heading_input.get())
hpr = [linear_heading, pitch_input.get(), roll_input.get()]
heading_output.set(linear_heading)
quat = Quaternion(hpr=hpr)
for i in range(0, 4):
quat_output[i].set(quat[i])
if dvl_present:
# Rotate DVL velocities
tmp = vel[0]
vel[0] = -vel[1]
vel[1] = tmp
vel = convert_dvl_velocities(sub_quat, vel)
return vel
def get_depth():
return depth_var.get() - depth_offset_var.get()
sub_quat = Quaternion(q=quat_orientation_filter.x_hat[:4])
x_vel, y_vel, z_vel = get_velocity(sub_quat)
depth = get_depth()
kalman_xHat = np.array([[x_vel, 0, y_vel, 0, 0, 0, depth, 0]]).reshape(8, 1)
# Pass in ftarray, shared memory handle to controller
position_filter = PositionFilter(kalman_xHat)
start = time.time()
iteration = 0
while True:
# TODO Should we wait on gx4 group write?
while iteration * dt < time.time() - start:
# Avoid timing errors due to time jumps on startup.
if time.time() - start - iteration * dt > 60: