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 4de68738 authored by Lars's avatar Lars
Browse files

changed stuff

parent b46d8fc5
......@@ -76,13 +76,19 @@ import os
import pickle
import matplotlib.pyplot as plt
from operator import itemgetter
from pykalman import KalmanFilter
from imutils.video import VideoStream
#PARAMS
MARKER_SIZE = 200 #size in mm
CAMERA = 1
KALMAN = True
# 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
# WIDTH = 640
......@@ -133,6 +139,14 @@ cfcamera_pub = rospy.Publisher(node_name, CameraData, queue_size=10)
# 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
......@@ -142,13 +156,13 @@ 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),
#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]])
......@@ -215,7 +229,18 @@ while(cam.isOpened()):
outputt = 't: '+str(tvec[0])
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')
else:
print('Wrong Marker detected at Frame: ' + str(framecount))
......@@ -227,7 +252,7 @@ while(cam.isOpened()):
#kalman filter-----------------------------------------------------------------------------------------------------------
masked_data = np.ma.masked_array(data, mask) #mask missing measurements
filtered, covariance = (kf.filter_update(filtered, covariance, masked_data[0]))
#filtered, covariance = (kf.filter_update(filtered, covariance, masked_data[0]))
data_out = filtered
output = 'flt' + str(np.round(data_out, decimals = 2))
#cv2.putText(QueryImg, output, (0,50), fontface, fontscale, fontcolor)
......@@ -241,7 +266,12 @@ while(cam.isOpened()):
cv2.destroyAllWindows()
outt0.close()
outt1.close()
outt2.close()
outr0.close()
outr1.close()
outr2.close()
......
......@@ -129,6 +129,7 @@ std::string m_namespace_to_coordinator_parameter_service;
// data" array is expected to match the name in
// "m_context", (negative numbers indicate unknown)
int m_poseDataIndex = -1;
int m_poseDataMothershipIndex = -1;
// > Boolen indicating if the Mocap data is availble
bool m_isAvailable_mocap_data = false;
// > Boolen indicating if the object is "long term" occuled
......
......@@ -120,7 +120,7 @@ integratorGain_forTauYaw : 0.05
# 2 - Point Mass Per Dimension Method
# Uses a 2nd order random walk estimator independently for
# each of (x,y,z,roll,pitch,yaw)
estimator_method : 1
estimator_method : 2
# THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
# > For the (x,y,z) position
......
2.638880853903569
2.6387335204134312
2.6385916313941493
2.59112678977462
2.6430662379383705
2.6449967873074067
2.611440278464601
2.635051347452915
2.6727653365640345
2.673109068745226
2.6907700237397365
2.6763193165133288
2.671354489815311
2.691797377001282
2.6718094465310127
2.65154817863983
2.718196301615324
2.6631319770518425
2.639417205630576
2.677311558904022
2.6421066178185604
2.6121968608831083
2.6320251080377957
2.6514686157888105
2.6530571337824025
2.6591119390738513
2.58662186334064
2.6198609046823353
2.6568572722617083
2.642631233654523
2.628160155434454
2.60345410861707
2.639836905273593
2.63310603485465
2.6386476049041168
2.542617029022998
2.6291351530185647
2.7178560706179056
2.6477257408362163
2.6045173879297527
2.6667934690952695
2.736048981634034
2.683207849516372
2.634693549005649
2.6788239095144672
2.6589452895792394
2.6690531248081286
2.6494818463202066
2.6413102316366537
2.7127837891603086
2.6618828159876244
2.630787545200528
2.6586406899572315
2.656767616788941
2.62431106195649
2.592211054607024
2.59736614402327
2.6175393401654565
2.6072575976019996
2.6052905136080726
2.622001826099384
2.658870135711785
2.6509692978171673
2.6352916792740095
2.6057167764464704
2.6171511655713546
2.645102641193897
2.633044273829098
2.5510817152523053
2.6226681302176162
2.6474449473808863
2.626806666155195
2.6352050738183914
2.5968987643346897
2.6609861175642124
2.6498434091167633
2.636804307497766
2.528858956381399
2.6570106186541533
2.675461683553537
2.630005723397177
2.632294182819833
2.5917029357801775
2.7649476460016857
2.637842397670151
2.639124220254178
2.6322766681757845
2.67751733133127
2.6758476333886496
2.5966900340819543
2.725001845177766
2.6942721595424333
2.7641896340703562
2.628932449119802
2.820580209937588
2.7559012719212643
2.725853422408575
2.742531200372641
2.7108623261131712
2.6604641779627043
2.7312655331317437
2.728449812658737
2.676757908921698
2.6854970833512826
2.7504453769343855
2.888407742158144
-2.8715355750559572
2.828274320593062
-2.7822563673452483
2.8762840737626734
2.8853043122192954
3.202279781639603
2.8184109693936197
2.8708287018362104
2.8680367573809806
2.800635712832648
2.806643821889358
3.1576946205839436
3.0379550477758515
3.186814651987694
3.2258582126619904
3.1240481984656836
2.867692452853091
3.0510805055518206
3.111940231918892
3.131426039238504
3.026418087729767
2.957923390958943
3.035553706199851
3.1151076163797944
3.0571582760488
3.002219932809141
3.0332754517013525
3.093769183222454
3.1117499977257115
2.997257825506419
3.024753328723411
3.084028831690844
3.0740904932971955
3.0180600539106903
3.0004015319557324
3.045131019617963
3.065134224756548
3.025294474213185
2.9328824397434667
3.0053864837671145
3.0555967006033513
3.0614967852265016
2.9806678921472964
2.9834605123112636
3.0329336070803534
3.062193394290876
3.03198347433474
2.9703097306345607
2.9963177575604227
3.064601462447604
2.7538671991929236
2.9979908197835616
3.07707079936413
2.7524372322631354
2.795367750706751
2.8439285500529263
2.644507476721683
2.614612735998787
2.6425579794049963
2.6521480444733787
2.584774888285082
2.594976973691894
2.602326173555167
2.587223221621761
2.621523393187522
2.583510322861357
2.5622023910957354
2.6006463498880223
2.620137296290383
2.5912374284596122
2.5954338279738165
2.5915215903205238
2.578645217631823
2.5617598487468998
2.6292975008062167
2.5603347457652688
2.681789259262507
2.6168057849884576
2.6618208527134923
2.553716214400755
2.617300477943107
2.6680570094514446
2.6209652987405496
-3.602322861175054
2.7223701978894925
2.659104187603589
2.8212467425503855
-3.5662759948222966
-3.4897032606660097
2.7608240944000646
2.72951565837428
-2.8868112750649986
2.819388960346572
2.77257055810235
-2.8889035701779435
2.849171031044938
2.6348268142327598
-3.4528736392084087
2.8123207808778887
-2.943251314819208
2.9176087207714874
-2.8272031009916736
2.59289533674707
-3.2913724086876384
3.1585980317826654
3.110911922839835
2.9455334821222032
3.3119043461323097
-3.033991371669115
3.0630887921276635
-3.216980720554245
-3.2684590850819824
2.943439911531895
2.9432641988358728
3.0812341578144538
2.923475841707992
2.9246907723576925
2.8206260895471407
-3.2275700935793075
2.7424027558895263
-3.0245853644726712
2.83473547189072
2.8731638608753216
2.7977540442468767
2.7898487362584956
2.6752666502603017
2.851591609392616
-3.365364902024607
2.784941091492354
2.645517208175459
-3.2960273583728137
-3.1241656584265622
-3.2577558470598045
2.654062960149539
2.6534059503175524
2.7645595771091354
2.6551909178958257
2.593331523234663
-3.5052789152477315
-3.4941428226994287
2.676151738245399
2.692037925841239
2.686662978852901
2.5937490515280865
2.7291048581278226
-3.599046165874113
2.4264916174955062
2.6740470117590918
2.6780192143934514
-2.9901432856940784
2.6716715233401436
2.6524674742022274
2.6050776532049498
2.631241504731569
2.6037103555473835
2.615226782653849
2.5592224509965646
2.603083113920908
2.5918379938392198
2.617262077932131
2.537521259649022
2.5959613666366206
2.5571787889581
2.601487819365105
2.538206717455092
2.685670495722548
2.685612796069982
2.5204168633400332
2.638510915564837
2.5793209553330687
2.568633004804247
2.5537779743968523
2.593878411045499
2.667461325460967
3.428328452099148
2.5461436024244177
2.5392357634967557
2.5717146052696056
2.569338826498502
2.5945582486861505
-2.8186060889875857
2.5352892406109384
2.5892284105109438
2.5433551890046973
2.609849371126215
2.53049115402406
2.5523370632927356
2.5898858812335313
2.564130970822468
2.6417176630410895
3.4185900930230075
3.391940475553632
-3.160816350919697
3.186593506570058
3.1318271523769425
2.790992251635758
2.9399660602036257
2.867238765001072
2.9249616593850623
2.871723717016042
2.8899377818892296
2.933210081128982
2.939318397119804
2.9137266963136286
2.9545022746893825
3.038501227763753
3.1402577063049675
2.9223303897760133
3.0187612693230665
3.0245444490991704
3.078567059352755
3.0130239011141953
2.9858397706916238
3.0370534568224516
3.109927655783819
3.217328650365739
3.0577573658356165
3.1188925100220377
3.130163089647532
3.079148289137235
3.015250420978255
2.963716144869199
3.007079991558823
3.1755039909065856
3.0699076955183333
2.809636351654163
3.1791539524883556
3.0589402675788486
3.0595858643310816
3.1299944594845255
3.166460343513459
-2.9490280029195697
-3.0186708115292347
3.1539048549206186
3.1825790558036653
2.9129341930953045
2.9938336431779606
3.1759043724131835
2.804722799371708
2.953025180130626
2.8434607164785075
2.703690209203285
-2.8763706375877467
2.9848159113333854
3.065568595349615
2.8568872771235565
2.6886602262584143
2.7199829265470177
2.9782123146594977
3.330238624210072
3.118228477692025
3.371402643718373
2.7860155707082006
3.199301715779964
-2.8971183455482477
2.5514067754663894
3.1262387281922934
2.4829314214922915
2.555390523108251
3.338108326239409
-2.96047212908061
3.523356756020793
3.498121714661833
2.613649487182719
2.5100540212083784
2.5696022856378833
2.5933083538915107
2.5348332566889007
2.6550886825721025
2.5700225609886487
2.5051526728246105
2.43212799987802
2.553499322263704
2.6076818655600476
2.601007480673961
2.599174454918114
2.508032365871611
2.407640471857052
2.5129170801682004
2.4477827084851596
-2.818539556311607
2.3972433172469865
2.5517173087673246
2.693623609035394
2.6014496338094166
2.4349317490607745
-3.003791309331309
2.5001995936894614
2.5063171097388177
2.711289513019528
-2.781114377289476
3.65266190398904
2.818864493920025
3.5559414074789264
2.52017519255583
2.5392864083557534
2.530599845472361
2.6832547664045525
2.5910443805115095
-2.933629812323787
3.458573354487815
2.6691126926132154
3.392083446840999
3.5055837588177976
3.4651927198688672
3.446189072181404
3.4662560076961104
2.639909451734464
2.5680220548621553
2.6495772981259575
2.611082632252337
2.5259718293085647
3.4793111256769107
3.431181815459135
3.393931596280697
2.686337433514266
2.6090711585345416
-2.8178730197798996
3.2753263697722588
3.511067441796453
-2.812514896759017
2.83425083431768
3.263470537496603
2.6695043001465875
3.4001646127260967
3.4167700198971342
-2.862165092198307
3.148928717878331
3.37114870637006
-2.837032223838502
3.2970269993084016
3.3947665625333756
3.3753732651458837
3.346678438223077
3.278672842107969
3.2463809236264547
3.0851655390179116
3.1146533589476526
-2.8618470008619696
3.2915569489174583
3.207766477369653
-2.911584103264275
3.3135461751733906
3.1889238390258337
-3.0333426347793004
-2.9973701020267347
-2.9928956883350315
-2.9228347296262394
-2.957103269306323
3.1137941651423002
-2.9811980717655615
-3.013475494054068
-2.9972105547527947
-2.969087402068
-2.9779118638965016
3.1762958712633345
-2.940601753522049
-2.9688413882854903
-2.9466424675525875
-2.9077078580158564
-2.8800138079931896
-2.8616100883475526
-2.8505576159352692
3.214194459164349
3.2030670988717267
3.201189014376481
3.2172503942189103
3.2084351213027005
3.2167989676473145
3.2064030066742877
3.2078546236410856
3.23197158027129
3.220738757742875
3.2403290314853193
3.2348297335178358
3.2429686606876964
3.2744937304771398
3.253225626682992
3.187000740650761
3.234443881197398
3.264953730912202
3.2781372240662128
3.292416305205585
3.3120784449409126
3.3152679739683633
3.326798170250013
3.1605563051933614
3.317090457245958
3.3108815676317658
3.327305271820042
3.3308124532253864
3.3244362602186524
3.3186885644150608
3.2731715070948604
3.280241356523434
2.6496549029494143
3.0700075225126517
3.214225900517584
3.194260007254219
2.766134152932983
2.630241267807376
3.2615170069400925
2.9672085187929165
2.633527809888925
2.5776988786490995
2.6225309642819683
2.5667984150580776
2.608997410883693
3.3624195404807966
3.366974233041677
2.5656282297949584
3.364314779105767
3.3649826340280873
3.2918222873795684
2.588187332157791
3.3741139688890005
2.5176172595757227
3.200994168807785
3.3114445755321715
-2.8394762314119935
-2.8116306484736806
3.419863595087258