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]