def update_control_errors(self):
# Update cruise speed error
self.u_error = np.clip(
(self.cruise_speed - self.vessel.relative_velocity[0]) / 2, -1, 1)
self.chi_error = 0.0
self.e = 0.0
self.upsilon_error = 0.0
self.h = 0.0
# Get path course and elevation
s = self.prog
chi_p, upsilon_p = self.path.get_direction_angles(s)
# Calculate tracking errors
SF_rotation = geom.Rzyx(0, upsilon_p, chi_p)
epsilon = np.transpose(SF_rotation).dot(self.vessel.position -
self.path(self.prog))
e = epsilon[1]
h = epsilon[2]
# Calculate course and elevation errors from tracking errors
chi_r = np.arctan2(-e, self.la_dist)
upsilon_r = np.arctan2(h, np.sqrt(e**2 + self.la_dist**2))
chi_d = chi_p + chi_r
upsilon_d = upsilon_p + upsilon_r
self.chi_error = np.clip(
geom.ssa(self.vessel.chi - chi_d) / np.pi, -1, 1)
#self.e = np.clip(e/12, -1, 1)
self.e = e
self.upsilon_error = np.clip(
geom.ssa(self.vessel.upsilon - upsilon_d) / np.pi, -1, 1)
#self.h = np.clip(h/12, -1, 1)
self.h = h
def _get_parametric_params(self):
diff = np.diff(self.waypoints, axis=0)
for i in range(self.nwaypoints-1):
derivative = diff[i] / self.segment_lengths[i]
alpha = np.arctan2(derivative[1], derivative[0])
beta = np.arctan2(-derivative[2], np.sqrt(derivative[0]**2 + derivative[1]**2))
self.azimuth_angles = np.append(self.azimuth_angles, geom.ssa(alpha))
self.elevation_angles = np.append(self.elevation_angles, geom.ssa(beta))
def _sim(self, nu_c):
#self.state += self.state_dot(nu_c)*self.step_size
w, q = odesolver45(self.state_dot, self.state, self.step_size, nu_c)
#self.state = q
self.state = w
self.state[3] = geom.ssa(self.state[3])
self.state[4] = geom.ssa(self.state[4])
self.state[5] = geom.ssa(self.state[5])
self.position_dot = self.state_dot(self.state, nu_c)[0:3]
def roll(self):
"""
Returns the roll of the AUV wrt NED.
"""
return geom.ssa(self.state[3])
def pitch(self):
"""
Returns the pitch of the AUV wrt NED.
"""
return geom.ssa(self.state[4])
def heading(self):
"""
Returns the heading of the AUV wrt true north.
"""
return geom.ssa(self.state[5])