To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit fc128316 authored by widmerl's avatar widmerl
Browse files

Kalman added

parent 422d7b77
......@@ -81,13 +81,10 @@ from imutils.video import VideoStream
#PARAMS
MARKER_SIZE = 200 #size in mm
CAMERA = 1
KALMAN = True
CAMERA = 1 #which Port is the Reciever connected to
PMKF = True #Do you want a PointMassKalmanFilter
TEXT_OUTPUT = False #Do you want text output of the raw data
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
#PMKF_Ahat = np.matrix([[0.6723, 0.0034],[-12.9648, 0.9352]])
#PMKF_Kinf = np.arange([ 0.3277,12.9648])
#OLD_STUFF
......@@ -129,24 +126,32 @@ cfcamera_pub = rospy.Publisher(node_name, CameraData, queue_size=10)
# def angle(p1, p2):
# return (math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)/WIDTH)*FOV
# def big_diff(v1, v2, mult):
# return v1/v2 < 1/mult or v1/v2 > mult
if TEXT_OUTPUT:
outt0 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t0.txt', 'w')
outt1 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t1.txt', 'w')
outt2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t2.txt', 'w')
outr0 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r0.txt', 'w')
outr1 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r1.txt', 'w')
outr2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r2.txt', 'w')
if PMKF: #PointMassKalmanFilter
Ahat_t = np.matrix([[0.8780690906,0.0292689697],[-0.2103665025,0.9929877833]])
Kinf_t = np.matrix([[0.1219309094],[0.2103665025]])
Ahat_r = np.matrix([[0.9313707507,0.0310456917],[-0.0683245284,0.9977225157]])
Kinf_r = np.matrix([[0.0686292493],[0.0683245284]])
def pmkf(state_old, input, type): #type = True for trans, False for rot
if type = True:
return Ahat_t*state_old_a+ Kinf_t*input
else
return Ahat_r*state_old_a+ Kinf_r*input
state_t = [np.matrix([[0],[0]]),np.matrix([[0],[0]]),np.matrix([[0],[0]])]
state_r = [np.matrix([[0],[0]]),np.matrix([[0],[0]]),np.matrix([[0],[0]])]
# def similar_value(v1,v2,max_diff):
# return max_diff > abs(v1-v2)
outt0 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t0.txt', 'w')
outt1 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t1.txt', 'w')
outt2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/t2.txt', 'w')
outr0 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r0.txt', 'w')
outr1 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r1.txt', 'w')
outr2 = open('/home/vision-crazyflie/work/D-FaLL-System/dfall_ws/src/dfall_pkg/r2.txt', 'w')
#initialize font
fontface = cv2.FONT_HERSHEY_SIMPLEX
fontscale = 0.5
......@@ -155,22 +160,6 @@ fontcolor = (255, 255, 255)
framecount = 0
data_out = np.array([])
#define filter
#kf = KalmanFilter(transition_matrices = [[1,0,0.033,0,0,0.0055],[0,1,0,0.033,0,0],[0,0,1,0,0.033,0],[0,0,0,1,0,0.033],[0,0,0,0,1,0],[0,0,0,0,0,1]],
#observation_matrices = np.eye(6),
#initial_state_mean = [3.14,0,0,0,0,1500],
#initial_state_covariance = [0,0,0,0,0,0],
#observation_covariance=1,
#transition_covariance=.05
#)
#numpy arrays as data storage for the kalman filter
data = np.array([[0,0,0,0,0,0]])
masked_data = np.array([[0,0,0,0,0,0]])
mask = np.array([0])
filtered = np.array([3.14, 0, 0, 0, 0, 1500])#starting values position
covariance = np.array([0, 0, 0, 0, 0, 0])#starting values covariance 'speed'
# Check for camera calibration data
......@@ -223,24 +212,45 @@ while(cam.isOpened()):
rvec = rvec[0]
tvec = tvec[0]
QueryImg = aruco.drawAxis(QueryImg, cameraMatrix, distCoeffs, rvec, tvec, 100)
cfcamera_pub.publish(rvec[0,0],rvec[0,1],rvec[0,2],tvec[0,0],tvec[0,1],tvec[0,2])
outputr = 'r: '+str(rvec[0])
outputt = 't: '+str(tvec[0])
rvec = rvec[0]
tvec = tvec[0]
if TEXT_OUTPUT:
outt0.write(str(tvec[0])+",")
outt1.write(str(tvec[1])+",")
outt2.write(str(tvec[2])+",")
outr0.write(str(rvec[0])+",")
outr1.write(str(rvec[1])+",")
outr2.write(str(rvec[2])+",")
#PointMassKalmanFilter
if PMKF:
for i in range(0,3):
state_t[i] = pmkf(state_t[i], tvec[i], True) #translation
state_r[i] = pmkf(state_r[i], tvec[i], False) #rotation
tvec[i] = state_t[i,0]
rvec[i] = state_r[i,0]
if TEXT_OUTPUT:
outt0.write(str(tvec[0]))
outt1.write(str(tvec[1]))
outt2.write(str(tvec[2]))
outr0.write(str(rvec[0]))
outr1.write(str(rvec[1]))
outr2.write(str(rvec[2]))
#publish
cfcamera_pub.publish(rvec[0],rvec[1],rvec[2],tvec[0],tvec[1],tvec[2])
#print to screen
outputr = 'r: '+str(rvec)
outputt = 't: '+str(tvec)
cv2.putText(QueryImg, outputr, (0,20), fontface, fontscale, fontcolor)
cv2.putText(QueryImg, outputt, (0,50), fontface, fontscale, fontcolor)
outt0.write(str(tvec[0,0]))
outt1.write(str(tvec[0,1]))
outt2.write(str(tvec[0,2]))
outr0.write(str(rvec[0,0]))
outr1.write(str(rvec[0,1]))
outr2.write(str(rvec[0,2]))
outt0.write('\n')
outt1.write('\n')
outt2.write('\n')
outr0.write('\n')
outr1.write('\n')
outr2.write('\n')
if TEXT_OUTPUT:
outt0.write('\n')
outt1.write('\n')
outt2.write('\n')
outr0.write('\n')
outr1.write('\n')
outr2.write('\n')
else:
print('Wrong Marker detected at Frame: ' + str(framecount))
......@@ -265,13 +275,13 @@ while(cam.isOpened()):
cv2.destroyAllWindows()
outt0.close()
outt1.close()
outt2.close()
outr0.close()
outr1.close()
outr2.close()
if TEXT_OUTPUT:
outt0.close()
outt1.close()
outt2.close()
outr0.close()
outr1.close()
outr2.close()
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment