Exemplo n.º 1
0
    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]))
Exemplo n.º 2
0
    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]))
Exemplo n.º 3
0
    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]