def ReadStates(self, input_m, input_f):
'''
Reads systems states from sensors (processed data)
'''
y = input_m[0, 0]
xd = input_m[1, 0]
xdd = input_m[2, 0]
x = input_m[3, 0]
yd = input_m[4, 0]
ydd = input_m[5, 0]
theta = input_m[6, 0]
omega = input_m[7, 0]
angacc = input_m[8, 0]
Measured_Pos = OL.O_PosData(x, y, 1, 1)
BodyXY = FL.NEDtoBody(Measured_Pos, self.Pos, theta)
PathFrameXY = list([
BodyXY[0] + numpy.sum(self.states[0]),
BodyXY[1] + numpy.sum(self.states[1])
])
Measured_Speed = OL.O_PosData(xd, yd, 1, 1)
BodySpeed = FL.NEDtoBody(Measured_Speed, OL.O_PosData(0, 0, 1, 1),
theta)
'''
Wn = numpy.matrix([[PathFrameXY[0]], [BodySpeed[0]], [BodyAcc[0]], [PathFrameXY[1]], [BodySpeed[1]], [BodyAcc[1]], [theta], [omega], [angacc]])
'''
Wn = numpy.matrix([[PathFrameXY[0]], [xd], [xdd], [BodyXY[1]], [0],
[ydd], [theta], [omega], [angacc]])
# print numpy.transpose(Wn)
# print self.Pos
prev_fx = numpy.sum(self.states[0])
prev_fy = numpy.sum(self.states[1])
'''Kalman'''
Validity_matrix = input_m[:, 1]
if input_m[0, 1] == 1:
self.flip = -self.flip
self.MP = [input_m[0][0], input_m[3][0], self.flip]
self.states = self.Filter.FilterStep(input_f, Wn, Validity_matrix)
# print self.states[0]
# print self.states[1]
self.Ts = 0.1
self.v = numpy.sum(self.states[2])
self.omega = numpy.sum(self.states[4])
self.Theta = math.atan2(math.sin(numpy.sum(self.states[3])),
math.cos(numpy.sum(self.states[3])))
self.x = numpy.matrix([[self.v], [self.Theta], [omega]])
curpos = self.Pos.get_Pos()
'''
x_next = numpy.sum(self.Ts * self.v * math.sin(self.Theta) + curpos[0])
y_next = numpy.sum(self.Ts * self.v * math.cos(self.Theta) + curpos[1])
self.Pos = OL.O_PosData(x_next, y_next, math.cos(self.x[1]), math.sin(self.x[1]))
0 Yd 1 Y 2 V 3 Th 4 Om
'''
V = numpy.sum(self.states[2])
X = numpy.sum(self.states[0])
Y = numpy.sum(self.states[1])
Th = numpy.sum(self.states[3])
self.Theta = math.atan2(math.sin(numpy.sum(self.states[3])),
math.cos(numpy.sum(self.states[3])))
#self.Theta = theta
Th = theta
#self.states[1] = PathFrameXY[1]
if numpy.sum(Validity_matrix[0]):
self.correction = BodyXY[1]
x_next = (
(X - prev_fx) *
math.sin(Th)) + curpos[0] + self.correction * math.cos(Th)
y_next = (
(X - prev_fx) *
math.cos(Th)) + curpos[1] - self.correction * math.sin(Th)
else:
self.correction = 0
x_next = ((X - prev_fx) * math.sin(Th)) + curpos[0]
y_next = ((X - prev_fx) * math.cos(Th)) + curpos[1]
#self.filterlog.write(str(float(self.Pos.get_Pos_X())) + ", " + str(float(self.Pos.get_Pos_Y())) + "\r\n")
#print x_next, y_next
#x_next = ((V * self.Ts) * math.sin(Th)) + curpos[0] + self.correction * 0.1* math.cos(Th)
#y_next = ((V * self.Ts) * math.cos(Th)) + curpos[1] - self.correction * 0.1* math.sin(Th)
#print('FX', x_next, 'FY', y_next, 'FV', numpy.sum(self.states[2]), 'FT', numpy.sum(self.states[3]), 'FO', numpy.sum(self.states[4]))
self.log.write(
str(x_next) + ', ' + str(y_next) + ', ' +
str(numpy.sum(self.states[2])) + ', ' +
str(numpy.sum(self.states[3])) + ', ' +
str(numpy.sum(self.states[4])) + ", " + str(time.time()) + "\r\n")
self.Pos = OL.O_PosData(x_next, y_next, math.cos(self.x[1]),
math.sin(self.x[1]))
def ReadStates(self, input_m, input_f):
'''
Reads systems states from sensors (processed data)
'''
x = input_m[0, 0]
xd = input_m[1, 0]
xdd = input_m[2, 0]
y = input_m[3, 0]
yd = input_m[4, 0]
ydd = input_m[5, 0]
theta = input_m[6, 0]
omega = input_m[7, 0]
angacc = input_m[8, 0]
Measured_Pos = OL.O_PosData(x, y, 1, 1)
BodyXY = FL.NEDtoBody(Measured_Pos, self.Pos, theta)
PathFrameXY = list([
BodyXY[0] + numpy.sum(self.states[0]),
BodyXY[1] + numpy.sum(self.states[1])
])
Measured_Speed = OL.O_PosData(xd, yd, 1, 1)
BodySpeed = FL.NEDtoBody(Measured_Speed, OL.O_PosData(0, 0, 1, 1),
theta)
'''
Wn = numpy.matrix([[PathFrameXY[0]], [BodySpeed[0]], [BodyAcc[0]], [PathFrameXY[1]], [BodySpeed[1]], [BodyAcc[1]], [theta], [omega], [angacc]])
'''
Wn = numpy.matrix([[PathFrameXY[0]], [xd], [xdd], [BodyXY[1]], [0],
[ydd], [theta], [omega], [angacc]])
#print Wn
prev_fx = numpy.sum(self.states[0])
prev_fy = numpy.sum(self.states[1])
'''Kalman'''
Validity_matrix = input_m[:, 1]
self.states = self.Filter.FilterStep(input_f, Wn, Validity_matrix)
self.Ts = 0.1
self.v = numpy.sum(self.states[2])
self.omega = numpy.sum(self.states[4])
self.Theta = math.atan2(math.sin(numpy.sum(self.states[3])),
math.cos(numpy.sum(self.states[3])))
self.x = numpy.matrix([[self.v], [self.Theta], [omega]])
curpos = self.Pos.get_Pos()
'''
x_next = numpy.sum(self.Ts * self.v * math.sin(self.Theta) + curpos[0])
y_next = numpy.sum(self.Ts * self.v * math.cos(self.Theta) + curpos[1])
self.Pos = OL.O_PosData(x_next, y_next, math.cos(self.x[1]), math.sin(self.x[1]))
0 Yd 1 Y 2 V 3 Th 4 Om
'''
V = numpy.sum(self.states[2])
X = numpy.sum(self.states[0])
Y = numpy.sum(self.states[1])
Th = numpy.sum(self.states[3])
self.Theta = math.atan2(math.sin(numpy.sum(self.states[3])),
math.cos(numpy.sum(self.states[3])))
#self.Theta = theta
Th = theta
#self.states[1] = PathFrameXY[1]
self.correction = Y
x_next = (
(X - prev_fx) *
math.sin(Th)) + curpos[0] + self.correction * 0.005 * math.cos(Th)
y_next = (
(X - prev_fx) *
math.cos(Th)) + curpos[1] - self.correction * 0.005 * math.sin(Th)
#print x_next, y_next
#x_next = ((V * self.Ts) * math.sin(Th)) + curpos[0] + self.correction * 0.1* math.cos(Th)
#y_next = ((V * self.Ts) * math.cos(Th)) + curpos[1] - self.correction * 0.1* math.sin(Th)
print('FX', x_next, 'FY', y_next, 'FV', numpy.sum(self.states[2]),
'FT', numpy.sum(self.states[3]), 'FO', numpy.sum(self.states[4]))
self.Pos = OL.O_PosData(x_next, y_next, math.cos(self.x[1]),
math.sin(self.x[1]))
Theta = numpy.sum(states[1])
Omega = numpy.sum(states[2])
Alpha = numpy.sum(states[2] - prevstates[2]) / 0.1
GPS_X = pos[0] + numpy.sum(2 * scipy.randn(1))
GPS_Y = pos[1] + numpy.sum(2 * scipy.randn(1))
Speed_X = math.sin(numpy.sum(states[1])) * numpy.sum(
states[0]) + numpy.sum(0.01 * scipy.randn(1))
Speed_Y = math.cos(numpy.sum(states[1])) * numpy.sum(
states[0]) + numpy.sum(0.01 * scipy.randn(1))
Acc_X = math.sin(numpy.sum(
states[1])) * (numpy.sum(states[0]) - numpy.sum(prevstates[0])) / 0.1
Acc_Y = math.cos(numpy.sum(
states[1])) * (numpy.sum(states[0]) - numpy.sum(prevstates[0])) / 0.1
Measured_Acc = OL.O_PosData(Acc_X, Acc_Y, 1, 1)
BodyAcc = FL.NEDtoBody(Measured_Acc, OL.O_PosData(0, 0, 1, 1), Theta)
measurement_matrix = numpy.transpose(
numpy.matrix(
numpy.array([[
GPS_X,
numpy.sum(states[0]), BodyAcc[0], GPS_Y, 0, BodyAcc[1], Theta,
Omega, Alpha
], [1, 1, 0, 1, 1, 0, 1, 1, 1]])))
AAUSHIP.ReadStates(measurement_matrix, motor)
AAUSHIP.FtoM(motor)
#print AAUSHIP.FtoM(motor)
'''Logging'''
thoughtpos = AAUSHIP.Pos.get_Pos()
x3[i] = thoughtpos[0]