#!/usr/bin/env python
# -*- coding: iso-8859-15 -*-
-VISULAIZE = False
+VISUALIZE = False
import threading
import struct
from nav_msgs.msg import Odometry
from wild_thumper.srv import DWM1000Center, DWM1000CenterResponse
from pyshared.simple_kalman import simple_kalman
-if VISULAIZE:
+if VISUALIZE:
import matplotlib.pyplot as plt
class Position:
def __init__(self):
- R = np.matrix([[0.045, 0.5], [0.0, 0.008]]) # Varianz des Messfehlers
- P_est = 1e-3 * np.eye(2) # Fehlerkovarianz
- Q = np.matrix([[0.001, 0.0], [0.0, 0.00001]]) # Systemrauschen
+ R = np.matrix([[0.5, 0.0], [0.0, 0.2]]) # Varianz des Messfehlers
+ P_est = 1e50 * np.eye(2) # Fehlerkovarianz
+ Q = np.matrix([[0.001, 0.0], [0.0, 0.0001]]) # Systemrauschen
self.filter_xy = simple_kalman(np.array([[1.0, 0.0]]).T, P_est, Q, R)
self.speed_x = 0
self.speed_y = 0
self.filter_xy.set_measure_cov(np.matrix([[cov_00, 0.0], [0.0, cov_11]]))
else:
pos = self.filter_xy.x_est
+ self.filter_xy.P_est = np.matrix(1e50 * np.eye(2)) # reset estimate when lost
x = pos.item((0, 0))
diff = abs(dist_left - dist_right)
if diff >= dist_l_r:
- # difference to high, correct to maximum
+ # difference too high, correct to maximum
off = diff - dist_l_r + 0.01
if dist_left > dist_right:
dist_left -= off/2
x, y = pos.filter(x, y)
- if VISULAIZE:
+ if VISUALIZE:
circle_left = plt.Circle((-dist_l_r/2, 0), dwleft.distance, color='red', fill=False)
circle_right = plt.Circle((dist_l_r/2, 0), dwright.distance, color='green', fill=False)
plt.gca().add_patch(circle_left)