def __init__(self, init_position, log):
'''
Initializes the parameters of the ship
- Parameters to calculate proper turn paths
- Initial sensor parameters for the control
- Initial start waypoint
- Navigation parameters (FollowDistance)
'''
self.Pos = init_position
self.retpos = self.Pos
self.NextSWP = self.Pos
self.Speed = 0
self.Sigma_max = 0.5
self.Kappa_max = 0.5
self.counter1 = 0
self.LastWP = 3
self.SWP = 0
self.SegmentCoords = 0
self.Current_SWP = 0
self.NextSWP_No = 0
self.NextSWP_validity = 0
self.FollowDistance = 2
self.v = 0
self.omega = 0
self.Theta = 0
self.x = numpy.matrix([[0], [0], [0]])
self.mark = 0
self.EndPath = 0
self.WPsEnded = 0
self.Filter = KF.Filter()
self.correction = 0
self.log = log
'''
The control matrices
'''
self.N = numpy.matrix([[1.2196566e+001, -2.2165773e-016],
[2.9639884e-017, 9.6263068e-001]])
self.F = numpy.matrix(
[[6.8421661e+000, -2.2165773e-016, -9.5435368e-017],
[2.9639884e-017, 9.6263068e-001, 1.4310741e+000]])
self.states = numpy.matrix([[0], [0], [0], [0], [0]])
def __init__(self, init_position):
'''
Initializes the parameters of the ship
- Parameters to calculate proper turn paths
- Initial sensor parameters for the control
- Initial start waypoint
- Navigation parameters (FollowDistance)
'''
self.Pos = init_position
self.retpos = self.Pos
self.NextSWP = self.Pos
self.Speed = 0
self.Sigma_max = 0.3
self.Kappa_max = 0.3
self.counter1 = 0
self.LastWP = 3
self.SWP = 0
self.SegmentCoords = 0
self.Current_SWP = 0
self.NextSWP_No = 0
self.NextSWP_validity = 0
self.FollowDistance = 2
self.v = 0
self.omega = 0
self.Theta = 0
self.x = numpy.matrix([[0], [0], [0]])
self.mark = 0
self.EndPath = 0
self.WPsEnded = 0
self.Filter = KF.Filter()
self.correction = 0
'''
The control matrices
'''
self.N = numpy.matrix([[2.1366547e+001, 1.5569542e-016],
[8.3542070e-016, 2.2764131]])
self.F = numpy.matrix(
[[1.6012147e+001, 1.5569542e-016, 3.3824418e-016],
[8.3542070e-016, 2.2764131e+000, 2.2219859e+000]])
self.states = numpy.matrix([[0], [0], [0], [0], [0]])
def __init__(self, init_position,statelog,swplog):
'''
Initializes the parameters of the ship
- Parameters to calculate proper turn paths
- Initial sensor parameters for the control
- Initial start waypoint
- Navigation parameters (FollowDistance)
'''
self.log = statelog
self.swplog = swplog
self.Pos = init_position
self.retpos = self.Pos
self.NextSWP = self.Pos
self.Speed = 0;
self.Sigma_max = 1;
self.Kappa_max = 1;
self.counter1 = 0;
self.LastWP = 3;
self.SWP = 0;
self.SegmentCoords = 0;
self.Current_SWP = 0;
self.NextSWP_No = 0;
self.NextSWP_validity = 0;
self.FollowDistance = 4
self.v = 0
self.omega = 0
self.Theta = 0
self.x = numpy.matrix([[0],[0],[0]])
self.mark = 0
self.EndPath = 0
self.WPsEnded = 0
self.Filter = KF.Filter()
self.correction = 0
# self.log = log
# self.statelog = statelog
self.virtvel = 0
'''
The control matrices
'''
# self.N = numpy.matrix([[1.2196566e+001, -2.2165773e-016], [ 2.9639884e-017, 9.6263068e-001]])
# self.N = numpy.matrix([[14.1834, 0],[0,1.8752]])
self.N = numpy.matrix([[19.5956,0],[0,2.2743]]) # ts = 0.33
# self.F = numpy.matrix([[6.8421661e+000, -2.2165773e-016, -9.5435368e-017], [2.9639884e-017, 9.6263068e-001, 1.4310741e+000]])
#self.F = numpy.matrix([[5.2835,0,0],[0,1.8752,0.5713]])
self.F = numpy.matrix([[10.6956, 0, 0],[0, 2.2743, 0.6681]]) # ts = 0.33
self.states = numpy.matrix([[0],[0],[0],[0],[0]])
