Ejemplo n.º 1
0
def find_ground_level(world, pos, percent_ground=0.65, search_size=3):
    """Returns a location in the y axis where the percentage of non
    air blocks to air blocks exceeds the specified value. Searches
    from the specified x, z coordinates from the top of the world
    downward."""

    if len(pos) > 2:
        raise ValueError("Only specify x and z coordinates for position")

    bb= world.getWorldBounds()

    # Make posistion for view such that the specified position will be in
    # the middle. View extends from bottom to top of world
    pos = array((pos[0], bb.miny, pos[1]))
    pos += array((-round(search_size/2), 0, -round(search_size/2)))

    # Create a view around the search location
    search_view = MapView(world, (search_size, bb.maxy, search_size), pos)
    search_data = search_view.map_data()

    # First index in y dimension where the search view is not all air
    where_no_air = where(search_view.map_data() != 0)
    ground_idx = min(where_no_air[1]) - 1
    ground_per = 0
    while ground_idx < bb.maxy and ground_per < percent_ground:
        ground_per = len(where(search_data[:, ground_idx, :] != 0)[0]) / float(search_size*search_size)
        ground_idx += 1

    return search_view.view[0, ground_idx, 0][1]
Ejemplo n.º 2
0
 def __init__(self, mc):
     self.mc = mc
     self.calculator = DistanceCalculator()
     myMotionCommander = MotionCommander(self.mc)
     self.cap = cv2.VideoCapture("video_localizacao.webm")
     CenterLimitH = [305, 335]
     self.savedX = 50
     self.savedY = 0
     self.savedAlpha = 180
     self.savedBeta = 90
     self.map_view = MapView()
    def __init__(self, X, P):
        '''
            self.X = posição do robo e dos marcadores
            self.P = covariancia dos robos e marcadores
            self.markers = marcadores descobertos e salvos
        '''
        self.X = X  #Estado inicial
        self.X_pred = self.X.copy()
        self.P = P
        self.calculator = DistanceCalculator()
        self.markers = []  #lista de marcadores salvos

        self.mapView = MapView()
Ejemplo n.º 4
0
def player_tunnel(world, player, **kwargs):
    """Make a tunnel starting at player locating in the same visual direction"""

    # Extract player posistion and yaw from NBT file
    player_pos, player_yaw = get_player_pos_yaw(player)

    # Get closest yaw to something divisible by 90 deg
    player_yaw = radians(90*round(degrees(player_yaw)/90))

    # Set up view object we will use to move through the world
    cave_view = MapView(world, tunnel_pattern.shape, player_pos, yaw=player_yaw)

    # Move starting point centered in front of player, 
    # a bit in front and a bit down
    cave_view.translate_relative((floor(tunnel_pattern.shape[2]/2.0), -floor(tunnel_pattern.shape[1]/2.0), tunnel_pattern.shape[0]))

    make_cave(cave_view, tunnel_pattern, **kwargs)
Ejemplo n.º 5
0
class Trajetoria():
    def __init__(self, mc):
        self.mc = mc
        self.calculator = DistanceCalculator()
        myMotionCommander = MotionCommander(self.mc)
        self.cap = cv2.VideoCapture("video_localizacao.webm")
        CenterLimitH = [305, 335]
        self.savedX = 50
        self.savedY = 0
        self.savedAlpha = 180
        self.savedBeta = 90
        self.map_view = MapView()

    def video(self):
        cont = 0
        while (self.cap.isOpened()):
            ret, frame = self.cap.read()

            if (frame is None):
                break

            if (cont % 3 == 0):
                thread = threading.Thread(target=self.calculator.processImage,
                                          args=(frame, ))
                #set time
                thread.setDaemon(True)
                thread.start()

            if (cont % 15 == 0):
                self.calculator.mediaDistance()
                self.map_view.updateMap(self.calculator.distance_x,
                                        self.calculator.distance_y,
                                        self.calculator.alpha)

            self.calculator.writeDistance(frame)
            self.frame = frame
            #cv2.imshow('frame',frame)

            cont += 1
            time.sleep(0.04)
            #if cv2.waitKey(10) & 0xFF == ord('q'):
            #break

        cv2.waitKey()
        cv2.destroyAllWindows()

    def savePosition(self):
        self.savedX = self.calculator.distance_x
        self.savedY = self.calculator.distance_y
        self.savedAlpha = self.calculator.alpha
        self.savedBeta = self.calculator.beta

    def correctPosition(self):
        delta_x = self.savedX - self.calculator.distance_x
        delta_y = self.savedY - self.calculator.distance_y
        delta_alpha = self.savedAlpha - self.calculator.alpha

        print(delta_x, delta_y, self.calculator.alpha)
        alpha_rad = self.calculator.alpha * math.pi / 180
        # mudança de coordenadas para o drone
        frente = delta_x * math.cos(alpha_rad) + delta_y * math.sin(alpha_rad)
        esquerda = -delta_x * math.sin(alpha_rad) + delta_y * math.cos(
            alpha_rad)
        print(frente, esquerda, delta_alpha)
        mc.move_distance(frente / 100, esquerda / 100, 0, velocity=0.3)
        time.sleep(1)
        if (delta_alpha > 0):
            mc.turn_left(delta_alpha)
        if (delta_alpha < 0):
            mc.turn_right(-delta_alpha)
        time.sleep(1)
Ejemplo n.º 6
0
        alpha_rad = self.calculator.alpha * math.pi / 180
        # mudança de coordenadas para o drone
        frente = delta_x * math.cos(alpha_rad) + delta_y * math.sin(alpha_rad)
        esquerda = -delta_x * math.sin(alpha_rad) + delta_y * math.cos(
            alpha_rad)
        print(frente, esquerda, delta_alpha)
        mc.move_distance(frente / 100, esquerda / 100, 0, velocity=0.3)
        time.sleep(1)
        if (delta_alpha > 0):
            mc.turn_left(delta_alpha)
        if (delta_alpha < 0):
            mc.turn_right(-delta_alpha)
        time.sleep(1)


if __name__ == '__main__':
    trajetoria = Trajetoria(None)

    mapView = MapView()

    thread = threading.Thread(target=trajetoria.video)
    thread.setDaemon(True)
    thread.start()
    time.sleep(3)

    while (True):
        cv2.imshow("frame", trajetoria.frame)
        cv2.imshow("mapa", trajetoria.map_view.map)
        if cv2.waitKey(100) & 0xFF == ord('q'):
            break
class Ekf:
    def __init__(self, X, P):
        '''
            self.X = posição do robo e dos marcadores
            self.P = covariancia dos robos e marcadores
            self.markers = marcadores descobertos e salvos
        '''
        self.X = X  #Estado inicial
        self.X_pred = self.X.copy()
        self.P = P
        self.calculator = DistanceCalculator()
        self.markers = []  #lista de marcadores salvos

        self.mapView = MapView()

    def predict(self, U):
        '''
================================================================================
            X = F*X + U
            P' = F*P'*F_t + Q


            X = posição dos robos e marcadores
            P = covariancia dos robos e marcadores
                P' = matrix 3x3, no início de P
            F = função de próximo Estado
            U = deslocamento
===============================================================================
        '''
        delta_t = U[0]
        theta = self.X[2] * 3.14 / 180
        F = np.array([[1., 0., 0], [0., 1., 0],
                      [0., 0., 1.]])  #Função de próximo estado
        Q = np.array([[3., 0., 0], [0., 3., 0], [0., 0.,
                                                 5.]])  #Incerteza do movimento

        #atualiza posição
        self.X[:3] = np.dot(F, self.X[:3]) + U

        #atualiza Covariancia do robo
        P_rr = self.P[:3, [0, 1, 2]]  #matrix 3x3
        self.P[:3, [0, 1, 2]] = np.dot(np.dot(F, P_rr), F.transpose()) + Q

        #atualiza Covariancia dos marcadores
        for j in range(len(self.markers)):
            P_ri = self.P[:3, [3 + 2 * j, 4 + 2 * j]]
            self.P[:3, [3 + 2 * j, 4 + 2 * j]] = np.dot(F, P_ri)

            self.P[3 + 2 * j:5 + 2 * j, [0, 1, 2]] = np.dot(F,
                                                            P_ri).transpose()

        self.X_pred = self.X[:3].copy()

    def update(self, medidas, foundMarker):
        '''
================================================================================
            S = H*P*H_t + R
            K = P*H_t*S_inv
            Y = Z - H*X_pred
            X = X + K*Y
            P = (I - K*H)*P

            H = função de medida
            R = Erro na medida
            S = covariancia de inovacao
            K = Ganho de kalman
            Z = distancia e angulo do marcador
            Y = Erro

================================================================================
        '''
        newMarker = []
        newMarkerPose = []
        R = np.array([[5., 0., 0], [0., 5., 0], [0., 0., 5]])  #Erro na medida

        for i in range(len(foundMarker)):
            I = np.identity(self.P.shape[0])  #identidade
            print(foundMarker)
            #marcador conhecido
            if foundMarker[i] in self.markers:

                #definição das matrizes
                Z = medidas[i][np.newaxis].transpose()
                H = self.defineH(foundMarker[i])
                Z_pred = self.measurePredict(foundMarker[i])

                print("Z: ", Z)
                print("Z_pred", Z_pred)

                #equações do EKF
                S = np.dot(np.dot(H, self.P), H.transpose()) + R
                K = np.dot(np.dot(self.P, H.transpose()), inv(S))
                Y = Z - Z_pred
                self.X = self.X + np.dot(K, Y)
                self.P = np.dot((I - np.dot(K, H)), self.P)

            #marcador desconhecido
            else:
                print("novo marcador", foundMarker[i])
                newMarker.append(foundMarker[i])
                newMarkerPose.append(self.findMarkerPose(self.X, medidas[i]))

        self.addNewMarker(newMarker, newMarkerPose)

    def addNewMarker(self, newMarker, newMarkerPose):
        '''
===============================================================================
            Adiciona novos marcadores

            X = [X] + [x_n, y_n]

            P_nn = Jxr* P'* Jxr_t +
                    Jz* R* Jz_t
            P_rn = P' *Jxr_t
            P_in = P_ir * Jxr_t
===============================================================================
        '''
        R = np.array([[5., 0., 0], [0., 5., 0], [0., 0., 5]])  #Erro na medida

        theta = self.X[2] * 3.14 / 180
        J_xr = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1]])  #
        J_z = np.array([[math.cos(theta), 0., 0], [math.sin(theta), 1., 0],
                        [0, 1., -1]])  #

        for i in range(len(newMarker)):
            n_markers = len(self.markers)
            '''
                adiciona em X
            '''
            self.X = np.append(self.X,
                               [[newMarkerPose[i][0]], [newMarkerPose[i][1]],
                                [newMarkerPose[i][2]]],
                               axis=0)
            '''
                adiciona coluna na matrix P
            '''
            P_rr = self.P[:3, [0, 1, 2]]  #matrix 3x3
            P_rn = np.dot(P_rr, J_xr.transpose())
            P_coluna = P_rn

            for j in range(n_markers):
                P_ir = self.P[3 + 2 * j:6 + 2 * j, [0, 1, 2]]
                P_in = np.dot(P_ir, J_xr.transpose())
                P_coluna = np.append(P_coluna, P_in, axis=0)

            self.P = np.append(self.P, P_coluna, axis=1)
            '''
                Adiciona linha na matrix P
            '''

            #P_nn = J_xr*P_rr*Jt_xr + J_z*R*Jt_z
            P_nn = (np.dot(np.dot(J_xr, P_rr), J_xr.transpose()) +
                    np.dot(np.dot(J_z, R), J_z.transpose()))

            P_linha = P_coluna.transpose()
            P_linha = np.append(P_linha, P_nn, axis=1)

            self.P = np.append(self.P, P_linha, axis=0)

            #adiciona o novo marcador na lista de marcadores salvo
            self.markers.append(newMarker[i])

    def defineH(self, id):
        '''
================================================================================
            Dada a identidade de um marcador, devolve a matriz H
            referente a esse marcador. O marcador já deve ter
            sido contabilizado em X
================================================================================
        '''
        n_marker = self.markers.index(id)

        x_marker = self.X[3 + 3 * n_marker][0]
        y_marker = self.X[4 + 3 * n_marker][0]
        x = self.X[0][0]
        y = self.X[1][0]
        r = math.sqrt((x - x_marker) * (x - x_marker) + (y - y_marker) *
                      (y - y_marker))

        a = (x - x_marker) / r
        b = (y - y_marker) / r
        d = (y_marker - y) / (r * r)
        e = (x_marker - x) / (r * r)

        H = np.array([[a, b, 0.], [d, e, -1], [0., 0.,
                                               -1.]])  #Função de medida

        for i in range(n_marker):
            A = np.zeros((3, 3))
            H = np.append(H, A, axis=1)

        A = np.array([[-a, -b, 0], [-d, -e, -1], [0., 0., 0]])
        H = np.append(H, A, axis=1)

        for i in range(len(self.markers) - n_marker - 1):
            A = np.zeros((3, 3))
            H = np.append(H, A, axis=1)

        return H

    def findMarkerPose(self, X, D):
        '''
================================================================================
            Dado a posição do drone, e a distância relativa do marcador com
            o drone, devolve a posição global do marcador
================================================================================
        '''
        # print("D", D)
        alpha = X[2] * 3.14 / 180
        theta = D[1] * 3.14 / 180

        #vetor do marcador na coordenada do drone
        x_drone = D[0] * math.cos(theta)
        y_drone = D[0] * math.sin(theta)

        #vetor do marcador na coordenada do mundo
        x_world = x_drone * math.cos(alpha) - y_drone * math.sin(alpha)
        y_world = x_drone * math.sin(alpha) + y_drone * math.cos(alpha)

        beta = D[2] + X[2] - 180
        markerPose = [X[0][0] + x_world, X[1][0] + y_world, beta]

        return markerPose

    def measurePredict(self, id):
        '''
================================================================================
        dado um id de um marcador, calcula a medida esperada
        do robo em relação a esse marcador
================================================================================
        '''
        n_marker = self.markers.index(id)
        # print(n_marker)

        x = self.X[0][0]
        y = self.X[1][0]
        alpha = self.X[2][0]
        x_marker = self.X[3 + 3 * n_marker][0]
        y_marker = self.X[4 + 3 * n_marker][0]
        beta = 180 - alpha + self.X[5 + 3 * n_marker][0]

        print("x_marker", x_marker)
        print("y_marker", y_marker)
        print("x", x)
        print("y", y)
        r = math.sqrt((x - x_marker) * (x - x_marker) + (y - y_marker) *
                      (y - y_marker))

        if (x_marker > x):
            theta_aux = math.atan((y_marker - y) / (x_marker - x))
            theta_aux = theta_aux * 180 / 3.14
            theta = theta_aux - self.X[2][0]
        elif (x_marker < x):
            theta_aux = math.atan((y_marker - y) / (x - x_marker))
            theta_aux = theta_aux * 180 / 3.14
            theta = (180 - theta_aux) - self.X[2][0]

        Z_pred = [[r], [theta], [beta]]

        return Z_pred

    def useEkf(self, img, desl):

        medidas, ids = self.calculator.processImage(img)

        print("move forward")
        self.predict(desl)
        print("predict\n", self.X)
        #print(ekf.P)
        self.mapView.updateMap(self.X)
        #input("tecle ENTER")

        self.update(medidas, ids)
        print("update\n", self.X)
        #print(ekf.P)
        self.mapView.updateMap(self.X)
        input("tecle ENTER")
class Ekf:
    def __init__(self, X, P):
        """
            self.X = posição do robo e dos marcadores
            self.P = covariancia dos robos e marcadores
            self.markers = marcadores descobertos e salvos
        """
        self.X = X  #Estado inicial
        self.X_pred = self.X.copy()
        self.P = P
        self.calculator = DistanceCalculator()
        self.markers = []  #lista de marcadores salvos
        self.cont = 0

        self.mapView = MapView()

    def predict(self, desl):
        """
================================================================================
            X = F*X + U
            P' = F*P'*F_t + Q


            X = posição dos robos e marcadores
            P = covariancia dos robos e marcadores
                P' = matrix 3x3, no início de P
            F = função de próximo Estado
            U = deslocamento
===============================================================================
        """
        U = self.transform_deslocamento(desl)
        theta = self.X[2] * 3.14 / 180
        A = np.array([[1., 0., 0], [0., 1., 0], [0., 0., 1.]])
        c = 0.1
        '''
        Q = c*np.array([[U[0][0]*U[0][0],    U[0][0]*U[1][0],           U[0][0]*U[2][0]  ],
                       [U[0][0]*U[1][0],             U[1][0]*U[1][0],   U[1][0]*U[2][0] ],
                       [U[0][0]*U[2][0],             U[1][0]*U[2][0],   U[2][0]*U[2][0]]]) #Incerteza do movimento
        '''
        Q = np.abs(
            np.array([[U[0][0] * c, 0., 0], [0., U[1][0] * c, 0],
                      [0., 0., U[2][0] * c]]))  #Incerteza do movimento

        #atualiza posição
        self.X[:3] = self.X[:3] + U

        #atualiza Covariancia do robo
        P_rr = self.P[:3, [0, 1, 2]]  #matrix 3x3
        self.P[:3, [0, 1, 2]] = np.dot(np.dot(A, P_rr), A.transpose()) + Q

        #atualiza Covariancia dos marcadores com relação ao robo
        for j in range(len(self.markers)):
            P_ri = self.P[:3, [3 + 2 * j, 4 + 2 * j, 5 + 2 * j]]
            P_line = np.dot(np.dot(A, P_ri), A.transpose())
            self.P[:3, [3 + 2 * j, 4 + 2 * j, 5 + 2 * j]] = P_line
            self.P[3 + 2 * j:6 + 2 * j, [0, 1, 2]] = P_line.transpose()

        self.X_pred = self.X.copy()

    def update(self, medidas, foundMarker):
        """
================================================================================
            S = H*P*H_t + R
            K = P*H_t*S_inv
            Y = Z - H*X_pred
            X = X + K*Y
            P = (I - K*H)*P

            H = função de medida
            R = Erro na medida
            S = covariancia de inovacao
            K = Ganho de kalman
            Z = distancia e angulo do marcador
            Y = Erro

================================================================================
        """
        newMarker = []
        newMarkerPose = []
        newMarkerError = []

        for i in range(len(foundMarker)):
            I = np.identity(self.P.shape[0])  #identidade
            Z = medidas[i][np.newaxis].transpose()

            #Erro na medida
            c = 0.1
            R = np.array([[Z[0][0] * c, 0., 0], [0., 5, 0], [0., 0., 5]])

            print(foundMarker)
            #marcador conhecido
            if foundMarker[i] in self.markers:

                #definição das matrizes
                H = self.defineH(foundMarker[i])
                Z_pred = self.measurePredict(foundMarker[i])

                print("Z: ", Z)
                print("Z_pred", Z_pred)

                #equações do EKF
                S = np.dot(np.dot(H, self.P), H.transpose()) + R
                K = np.dot(np.dot(self.P, H.transpose()), inv(S))
                Y = Z - Z_pred
                self.X = self.X + np.dot(K, Y)
                self.P = np.dot((I - np.dot(K, H)), self.P)

            #marcador desconhecido
            else:
                print("novo marcador", foundMarker[i])
                newMarker.append(foundMarker[i])
                newMarkerPose.append(self.findMarkerPose(self.X, medidas[i]))
                newMarkerError.append(R)

        return newMarker, newMarkerPose, newMarkerError

    def addNewMarker(self, newMarker, newMarkerPose, newMarkerError):
        """
===============================================================================
            Adiciona novos marcadores

            X = [X] + [x_n, y_n]

            P_nn = Jxr* P'* Jxr_t +
                    Jz* R* Jz_t
            P_rn = P' *Jxr_t
            P_in = P_ir * Jxr_t
===============================================================================
        """

        theta = self.X[2] * 3.14 / 180
        J_xr = np.array([[1., 0., 0.], [0., 1., 0.], [0., 0., 1]])  #
        J_z = np.array([[math.cos(theta), 0., 0], [math.sin(theta), 1., 0],
                        [0, 0., -1]])  #

        for i in range(len(newMarker)):
            R = newMarkerError[i]
            n_markers = len(self.markers)
            """
                adiciona em X
            """
            self.X = np.append(self.X,
                               [[newMarkerPose[i][0]], [newMarkerPose[i][1]],
                                [newMarkerPose[i][2]]],
                               axis=0)
            """
                adiciona coluna na matrix P
            """
            P_rr = self.P[:3, [0, 1, 2]]  #matrix 3x3
            P_rn = np.dot(P_rr, J_xr.transpose())
            P_coluna = P_rn

            for j in range(n_markers):
                P_ir = self.P[3 + 3 * j:6 + 3 * j, [0, 1, 2]]
                P_in = np.dot(P_ir, J_xr.transpose())
                P_coluna = np.append(P_coluna, P_in, axis=0)

            self.P = np.append(self.P, P_coluna, axis=1)
            """
                Adiciona linha na matrix P
            """

            #P_nn = J_xr*P_rr*Jt_xr + J_z*R*Jt_z
            P_nn = (np.dot(np.dot(J_xr, P_rr), J_xr.transpose()) +
                    np.dot(np.dot(J_z, R), J_z.transpose()))

            P_linha = P_coluna.transpose()
            P_linha = np.append(P_linha, P_nn, axis=1)

            self.P = np.append(self.P, P_linha, axis=0)

            #adiciona o novo marcador na lista de marcadores salvo
            self.markers.append(newMarker[i])

    def defineH(self, id):
        """
================================================================================
            Dada a identidade de um marcador, devolve a matriz H
            referente a esse marcador. O marcador já deve ter
            sido contabilizado em X
================================================================================
        """
        n_marker = self.markers.index(id)

        x_marker = self.X[3 + 3 * n_marker][0]
        y_marker = self.X[4 + 3 * n_marker][0]
        x = self.X[0][0]
        y = self.X[1][0]
        r = math.sqrt((x - x_marker) * (x - x_marker) + (y - y_marker) *
                      (y - y_marker))

        a = (x - x_marker) / r
        b = (y - y_marker) / r
        d = (y_marker - y) / (r * r)
        e = (x_marker - x) / (r * r)

        H = np.array([[a, b, 0.], [d, e, -1], [0., 0.,
                                               -1.]])  #Função de medida

        for i in range(n_marker):
            A = np.zeros((3, 3))
            H = np.append(H, A, axis=1)

        A = np.array([[-a, -b, 0], [-d, -e, 1], [0., 0., 1]])
        H = np.append(H, A, axis=1)

        for i in range(len(self.markers) - n_marker - 1):
            A = np.zeros((3, 3))
            H = np.append(H, A, axis=1)

        return H

    def findMarkerPose(self, X, D):
        """
================================================================================
            Dado a posição do drone, e a distância relativa do marcador com
            o drone, devolve a posição global do marcador
================================================================================
        """

        alpha = X[2] * 3.14 / 180
        theta = D[1] * 3.14 / 180

        #vetor do marcador na coordenada do drone
        x_drone = D[0] * math.cos(theta)
        y_drone = D[0] * math.sin(theta)

        #vetor do marcador na coordenada do mundo
        x_world = x_drone * math.cos(alpha) - y_drone * math.sin(alpha)
        y_world = x_drone * math.sin(alpha) + y_drone * math.cos(alpha)

        beta = D[2] + X[2][0] - 180
        markerPose = [X[0][0] + x_world, X[1][0] + y_world, beta]

        return markerPose

    def measurePredict(self, id):
        """
================================================================================
        dado um id de um marcador, calcula a medida esperada
        do robo em relação a esse marcador
================================================================================
        """
        n_marker = self.markers.index(id)
        # print(n_marker)

        x = self.X[0][0]
        y = self.X[1][0]
        alpha = self.X[2][0]
        x_marker = self.X[3 + 3 * n_marker][0]
        y_marker = self.X[4 + 3 * n_marker][0]
        beta = 180 - alpha + self.X[5 + 3 * n_marker][0]

        print("x_marker", x_marker)
        print("y_marker", y_marker)
        print("x", x)
        print("y", y)
        r = math.sqrt((x - x_marker) * (x - x_marker) + (y - y_marker) *
                      (y - y_marker))

        theta_aux = math.atan((y_marker - y) / (x_marker - x))
        theta_aux = theta_aux * 180 / 3.14
        theta = theta_aux - self.X[2][0]

        while (theta < -90):
            theta += 180
        while (theta > 90):
            theta -= 180

        Z_pred = [[r], [theta], [beta]]

        return Z_pred

    def transform_deslocamento(self, desl):
        """
================================================================================
        transforma o deslocamento dada em coordenadas do drone para
        coordenadas absolutas
        drone:
        frente +
        esquerda +
        sentido horário

        absoluta:
        y
        ^
        |
        |  rotação no sentido horário
        |
        ________> x
================================================================================
        """

        alpha = self.X[2][0] * 3.14 / 180  #angulo do drone
        alpha2 = (self.X[2][0] +
                  90) * 3.14 / 180  #angulo do movimento lateral do drone
        x = desl[0] * math.cos(alpha) + desl[1] * math.cos(alpha2)
        y = desl[0] * math.sin(alpha) + desl[1] * math.sin(alpha2)

        theta = -desl[2]

        print(desl)
        return [[x], [y], [theta]]

    def showP(self):
        diagonal = np.diagonal(self.P)
        cont = 0
        for i in diagonal:
            print("P[%d][%d]" % (cont, cont), i)
            cont += 1
            # if(cont == 3):
            #     break

    def useEkf(self, img, desl):
        self.cont += 1
        print("\n\n", self.cont)

        medidas, ids = self.calculator.processImage(img)

        print("move forward")
        self.predict(desl)
        #print("predict\n",self.X)
        #print(ekf.P)
        self.mapView.updateMap(self.X)
        #input("tecle ENTER")

        newMarker, newMarkerPose, newMarkerError = self.update(medidas, ids)
        self.addNewMarker(newMarker, newMarkerPose, newMarkerError)
        print("update\n", self.X)
        self.showP()
        #print(np.allclose(self.P, self.P.T, atol=1e-8))
        self.mapView.updateMap(self.X)