Skip to content

Commit 2169ba0

Browse files
rktessarktessa
committed
modifiche fatte
1 parent 014c7e2 commit 2169ba0

File tree

7 files changed

+716
-13
lines changed

7 files changed

+716
-13
lines changed

‎Old_stuff/Untitled.ipynb

Lines changed: 120 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -540,9 +540,128 @@
540540
},
541541
{
542542
"cell_type": "code",
543-
"execution_count": null,
543+
"execution_count": 14,
544544
"id": "8a83b07f",
545545
"metadata": {},
546+
"outputs": [
547+
{
548+
"name": "stdout",
549+
"output_type": "stream",
550+
"text": [
551+
"(array([0. , 7.19, 2.15]), array([0. , 3.62, 3.15]), array([0. , 0. , 2.15]), array([4.79, 1.85, 3.15]), array([4.79, 5.45, 2.15]), array([3. , 9.35, 3.15]))\n",
552+
"AN = [[ 0. 13.1044 9.9225]\n",
553+
" [ 0. 0. 4.6225]\n",
554+
" [22.9441 3.4225 9.9225]\n",
555+
" [22.9441 29.7025 4.6225]\n",
556+
" [ 9. 87.4225 9.9225]]\n"
557+
]
558+
}
559+
],
560+
"source": [
561+
"import numpy as np\n",
562+
"\n",
563+
"A_n1 = np.array([0.00, 7.19, 2.15])\n",
564+
"A_n2 = np.array([0.00, 3.62, 3.15])\n",
565+
"A_n3 = np.array([0.00, 0.00, 2.15])\n",
566+
"A_n4 = np.array([4.79, 1.85, 3.15])\n",
567+
"A_n5 = np.array([4.79, 5.45, 2.15])\n",
568+
"A_n6 = np.array([3.00, 9.35, 3.15])\n",
569+
"\n",
570+
"A_n = A_n1, A_n2, A_n3, A_n4, A_n5, A_n6\n",
571+
"\n",
572+
"n = len(A_n)\n",
573+
"\n",
574+
"#sum = np.sum((A_n[:][1:n].T)**2, 2)\n",
575+
"#print (\"Sum = \", sum)\n",
576+
"print(A_n)\n",
577+
"print(\"AN =\", np.square(A_n[:][1:n]))"
578+
]
579+
},
580+
{
581+
"cell_type": "code",
582+
"execution_count": 2,
583+
"id": "9c5b3223",
584+
"metadata": {},
585+
"outputs": [
586+
{
587+
"name": "stdout",
588+
"output_type": "stream",
589+
"text": [
590+
"[2 3 4 5 6 7 8 9]\n"
591+
]
592+
}
593+
],
594+
"source": [
595+
"import numpy as np\n",
596+
"\n",
597+
"a = np.array([1, 2, 3, 4, 5, 6, 7, 8, 9])\n",
598+
"index = [0]\n",
599+
"\n",
600+
"new_a = np.delete(a, index)\n",
601+
"\n",
602+
"print(new_a)"
603+
]
604+
},
605+
{
606+
"cell_type": "code",
607+
"execution_count": 12,
608+
"id": "36d4a2eb",
609+
"metadata": {},
610+
"outputs": [
611+
{
612+
"name": "stdout",
613+
"output_type": "stream",
614+
"text": [
615+
"[[0 0 0]\n",
616+
" [1 1 1]]\n"
617+
]
618+
}
619+
],
620+
"source": [
621+
"import numpy as np\n",
622+
"\n",
623+
"a = np.array((0, 0, 0))\n",
624+
"b = np.array((1,1,1))\n",
625+
"\n",
626+
"c = np.array((a, b))\n",
627+
"\n",
628+
"\n",
629+
"print(c)"
630+
]
631+
},
632+
{
633+
"cell_type": "code",
634+
"execution_count": 7,
635+
"id": "e2c8fc9c",
636+
"metadata": {},
637+
"outputs": [
638+
{
639+
"ename": "JSONDecodeError",
640+
"evalue": "Expecting value: line 1 column 1 (char 0)",
641+
"output_type": "error",
642+
"traceback": [
643+
"\u001b[1;31m---------------------------------------------------------------------------\u001b[0m",
644+
"\u001b[1;31mJSONDecodeError\u001b[0m Traceback (most recent call last)",
645+
"Cell \u001b[1;32mIn[7], line 4\u001b[0m\n\u001b[0;32m 1\u001b[0m \u001b[39mimport\u001b[39;00m \u001b[39mjson\u001b[39;00m\n\u001b[1;32m----> 4\u001b[0m data \u001b[39m=\u001b[39m json\u001b[39m.\u001b[39;49mloads(\u001b[39m'\u001b[39;49m\u001b[39mBlimp\u001b[39;49m\u001b[39m\\t\u001b[39;49;00m\u001b[39mempi.json\u001b[39;49m\u001b[39m'\u001b[39;49m)\n",
646+
"File \u001b[1;32m~\\AppData\\Local\\Programs\\Python\\Python310\\lib\\json\\__init__.py:346\u001b[0m, in \u001b[0;36mloads\u001b[1;34m(s, cls, object_hook, parse_float, parse_int, parse_constant, object_pairs_hook, **kw)\u001b[0m\n\u001b[0;32m 341\u001b[0m s \u001b[39m=\u001b[39m s\u001b[39m.\u001b[39mdecode(detect_encoding(s), \u001b[39m'\u001b[39m\u001b[39msurrogatepass\u001b[39m\u001b[39m'\u001b[39m)\n\u001b[0;32m 343\u001b[0m \u001b[39mif\u001b[39;00m (\u001b[39mcls\u001b[39m \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39mand\u001b[39;00m object_hook \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39mand\u001b[39;00m\n\u001b[0;32m 344\u001b[0m parse_int \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39mand\u001b[39;00m parse_float \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39mand\u001b[39;00m\n\u001b[0;32m 345\u001b[0m parse_constant \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39mand\u001b[39;00m object_pairs_hook \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m \u001b[39mand\u001b[39;00m \u001b[39mnot\u001b[39;00m kw):\n\u001b[1;32m--> 346\u001b[0m \u001b[39mreturn\u001b[39;00m _default_decoder\u001b[39m.\u001b[39;49mdecode(s)\n\u001b[0;32m 347\u001b[0m \u001b[39mif\u001b[39;00m \u001b[39mcls\u001b[39m \u001b[39mis\u001b[39;00m \u001b[39mNone\u001b[39;00m:\n\u001b[0;32m 348\u001b[0m \u001b[39mcls\u001b[39m \u001b[39m=\u001b[39m JSONDecoder\n",
647+
"File \u001b[1;32m~\\AppData\\Local\\Programs\\Python\\Python310\\lib\\json\\decoder.py:337\u001b[0m, in \u001b[0;36mJSONDecoder.decode\u001b[1;34m(self, s, _w)\u001b[0m\n\u001b[0;32m 332\u001b[0m \u001b[39mdef\u001b[39;00m \u001b[39mdecode\u001b[39m(\u001b[39mself\u001b[39m, s, _w\u001b[39m=\u001b[39mWHITESPACE\u001b[39m.\u001b[39mmatch):\n\u001b[0;32m 333\u001b[0m \u001b[39m\"\"\"Return the Python representation of ``s`` (a ``str`` instance\u001b[39;00m\n\u001b[0;32m 334\u001b[0m \u001b[39m containing a JSON document).\u001b[39;00m\n\u001b[0;32m 335\u001b[0m \n\u001b[0;32m 336\u001b[0m \u001b[39m \"\"\"\u001b[39;00m\n\u001b[1;32m--> 337\u001b[0m obj, end \u001b[39m=\u001b[39m \u001b[39mself\u001b[39;49m\u001b[39m.\u001b[39;49mraw_decode(s, idx\u001b[39m=\u001b[39;49m_w(s, \u001b[39m0\u001b[39;49m)\u001b[39m.\u001b[39;49mend())\n\u001b[0;32m 338\u001b[0m end \u001b[39m=\u001b[39m _w(s, end)\u001b[39m.\u001b[39mend()\n\u001b[0;32m 339\u001b[0m \u001b[39mif\u001b[39;00m end \u001b[39m!=\u001b[39m \u001b[39mlen\u001b[39m(s):\n",
648+
"File \u001b[1;32m~\\AppData\\Local\\Programs\\Python\\Python310\\lib\\json\\decoder.py:355\u001b[0m, in \u001b[0;36mJSONDecoder.raw_decode\u001b[1;34m(self, s, idx)\u001b[0m\n\u001b[0;32m 353\u001b[0m obj, end \u001b[39m=\u001b[39m \u001b[39mself\u001b[39m\u001b[39m.\u001b[39mscan_once(s, idx)\n\u001b[0;32m 354\u001b[0m \u001b[39mexcept\u001b[39;00m \u001b[39mStopIteration\u001b[39;00m \u001b[39mas\u001b[39;00m err:\n\u001b[1;32m--> 355\u001b[0m \u001b[39mraise\u001b[39;00m JSONDecodeError(\u001b[39m\"\u001b[39m\u001b[39mExpecting value\u001b[39m\u001b[39m\"\u001b[39m, s, err\u001b[39m.\u001b[39mvalue) \u001b[39mfrom\u001b[39;00m \u001b[39mNone\u001b[39m\n\u001b[0;32m 356\u001b[0m \u001b[39mreturn\u001b[39;00m obj, end\n",
649+
"\u001b[1;31mJSONDecodeError\u001b[0m: Expecting value: line 1 column 1 (char 0)"
650+
]
651+
}
652+
],
653+
"source": [
654+
"import json\n",
655+
"\n",
656+
"\n",
657+
"data = json.loads('Blimp\\tempi.json')"
658+
]
659+
},
660+
{
661+
"cell_type": "code",
662+
"execution_count": null,
663+
"id": "f93c4453",
664+
"metadata": {},
546665
"outputs": [],
547666
"source": []
548667
}

‎README.md

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,12 @@
11
# Blimp
22
A simple autonomous blimp project on Raspberry
33

4-
## Cose importanti da modificare
5-
La funzione Orientation initial devi modificarla per fare in modo che nei primi 10 secondi ti dia il valore di orientazione del blimp rispetto al reference frame assoluto degli UWB. Poi la fai andare all'inizio del codice e in questo modo ti ricavi una terna di valori di angoli di cui ruotare la lettura trovata poi nel while 1 del madgwick mentre fa andare la navigazione.
4+
## Funzione blimp_to_world_rf
5+
In prova.py e in main_blimp_01.py è visibile la routine inziale del codice ora funzionante.
6+
7+
La funzione blimp_to_world_rf crea un programma che stima con il madgwick per 10 secondi l'orientazione in cui si trova il dirigibile. In contemporanea con l'uwb viene misurata la posizione del blimp mentre si muove in linea retta nel laboratorio. La differenza delle 2 orientazioni calcolate è usata come off set per ruotare il sistema di riferimento del blimp nel absolute reference frame rappresentato dagli UWB. In questo modo posso pilotare poi per il resto del tempo il dirigibile usando angoli calcolati con il path planning nel global reference frame
8+
9+
610

711

812
## Lavori in corso
@@ -27,7 +31,7 @@ Important aspects to undestand for Kalman filter implementation:
2731

2832
- capire se la rootazione come la ho fatta va bene
2933

30-
- chiarire come salvare su csv/txt al meglio lo stream con i dati calcolati di posizione/sensori e valori del kalman
34+
- chiarire come salvare su csv/txt al meglio lo stream con i dati calcolati di posizione/sensori e valori del kalman ==> ispirarsi al codice visto e implementato in "path_planning.py" con le python cv per plottare il path del blimp
3135

3236
- implementare che il risultato del programma per il calcolo della traiettoria sia un csv/txt che viene letto e convertito in un data frame durante la navigazione.
3337

@@ -52,7 +56,7 @@ Di conseguenza da definire il max signal da passare, oltre al quale il pid satur
5256

5357
convertire target distance in metri da centimetri
5458

55-
capire dove inserire i time counter per avere l'effettivosample rate del codice che serve in:
59+
capire dove inserire i time counter per avere l'effettivo sample rate del codice che serve in:
5660
- Kalman
5761
- PID
5862
- Madgwick
@@ -74,11 +78,13 @@ Penso possa essere utile anche per la relazione di fontanelli.
7478
# Comunicazione e guida con tastiera
7579
Messo in funzione il codice implementato da Filippo, sarebbe interessante svilupparlo per sostiuire joycon con tastiera e poi per ottene le misure ogni decimo di secondo tipo e fare un plot che si aggiorna sul pc con i dati live.
7680

81+
==> Capire come implementare protocollo per trasmissione dei dati dal blimp al pc ogni secondo (?)
82+
7783

7884
# Path planner e coordinate UWB in lab di meccatronica
7985
Implementato path planner, prende in input mappa dell'ambiente disegnata e riscalata correttamente (1 pixel * 1 cm) e restituisce il percorso corrispondente, in step di 10 cm, che poi converto in metri per comodità del mio algoritmo
8086

8187
![Alt text](https://github.com/rktessa/Blimp/blob/main/phat_plannig_solution.png?raw=true "Scheme of the Alghorithm ")
8288

83-
# Cosa vuol dire che Cazzuto ha avuto problemi con il dead reckoning iniziale??
84-
Investigare
89+
90+

‎blimp_class.py

Lines changed: 138 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -359,6 +359,144 @@ def trilateration(d1,d2,d3,d4,d5,d6):
359359
return pos[0], pos[1], pos[2] #pos x,y,z
360360

361361

362+
def TDoA(ts, dt):
363+
# Time definition
364+
'''t6_rx1 = float(ts[0,0]) * 15.65e-12
365+
t1_rx1 = float(ts[1,0]) * 15.65e-12
366+
t2_rx1 = float(ts[2,0]) * 15.65e-12
367+
t3_rx1 = float(ts[3,0]) * 15.65e-12
368+
t4_rx1 = float(ts[4,0]) * 15.65e-12
369+
t5_rx1 = float(ts[5,0]) * 15.65e-12
370+
371+
t6_rx2 = float(ts[0,1]) * 15.65e-12
372+
t1_rx2 = float(ts[1,1]) * 15.65e-12
373+
t2_rx2 = float(ts[2,1]) * 15.65e-12
374+
t3_rx2 = float(ts[3,1]) * 15.65e-12
375+
t4_rx2 = float(ts[4,1]) * 15.65e-12
376+
t5_rx2 = float(ts[5,1]) * 15.65e-12 #double(1/(63.8976 * 100000000float
377+
378+
t6_tx1 = float(ts[0,2]) * 15.65e-12
379+
t1_tx1 = float(ts[1,2]) * 15.65e-12
380+
t2_tx1 = float(ts[2,2]) * 15.65e-12
381+
t3_tx1 = float(ts[3,2]) * 15.65e-12
382+
t4_tx1 = float(ts[4,2]) * 15.65e-12
383+
t5_tx1 = float(ts[5,2]) * 15.65e-12
384+
385+
t6_tx2 = float(ts[0,3]) * 15.65e-12
386+
t1_tx2 = float(ts[1,3]) * 15.65e-12
387+
t2_tx2 = float(ts[2,3]) * 15.65e-12
388+
t3_tx2 = float(ts[3,3]) * 15.65e-12
389+
t4_tx2 = float(ts[4,3]) * 15.65e-12
390+
t5_tx2 = float(ts[5,3]) * 15.65e-12
391+
'''
392+
393+
t6_rx1 = float(ts[0]) * 15.65e-12
394+
t1_rx1 = float(ts[1]) * 15.65e-12
395+
t2_rx1 = float(ts[2]) * 15.65e-12
396+
t3_rx1 = float(ts[3]) * 15.65e-12
397+
t4_rx1 = float(ts[4]) * 15.65e-12
398+
t5_rx1 = float(ts[5]) * 15.65e-12
399+
400+
t6_rx2 = float(ts[6]) * 15.65e-12
401+
t1_rx2 = float(ts[7]) * 15.65e-12
402+
t2_rx2 = float(ts[8]) * 15.65e-12
403+
t3_rx2 = float(ts[9]) * 15.65e-12
404+
t4_rx2 = float(ts[10]) * 15.65e-12
405+
t5_rx2 = float(ts[11]) * 15.65e-12 #double(1/(63.8976 * 100000000float
406+
407+
t6_tx1 = float(ts[12]) * 15.65e-12
408+
t1_tx1 = float(ts[13]) * 15.65e-12
409+
t2_tx1 = float(ts[14]) * 15.65e-12
410+
t3_tx1 = float(ts[15]) * 15.65e-12
411+
t4_tx1 = float(ts[16]) * 15.65e-12
412+
t5_tx1 = float(ts[17]) * 15.65e-12
413+
414+
t6_tx2 = float(ts[18]) * 15.65e-12
415+
t1_tx2 = float(ts[19]) * 15.65e-12
416+
t2_tx2 = float(ts[20]) * 15.65e-12
417+
t3_tx2 = float(ts[21]) * 15.65e-12
418+
t4_tx2 = float(ts[22]) * 15.65e-12
419+
t5_tx2 = float(ts[23]) * 15.65e-12
420+
421+
# Embedded Lab system anchor position
422+
A_n1 = np.array([0.00, 7.19, 2.15])
423+
A_n2 = np.array([0.00, 3.62, 3.15])
424+
A_n3 = np.array([0.00, 0.00, 2.15])
425+
A_n4 = np.array([4.79, 1.85, 3.15])
426+
A_n5 = np.array([4.79, 5.45, 2.15])
427+
A_n6 = np.array([3.00, 9.35, 3.15])
428+
429+
A_n1 = [0.00, 7.19, 2.15]
430+
A_n2 = [0.00, 3.62, 3.15]
431+
A_n3 = [0.00, 0.00, 2.15]
432+
A_n4 = [4.79, 1.85, 3.15]
433+
A_n5 = [4.79, 5.45, 2.15]
434+
A_n6 = [3.00, 9.35, 3.15]
435+
436+
A_n = np.array((A_n6, A_n1, A_n2, A_n3, A_n4, A_n5))
437+
c = 299792458 # Speed of light
438+
n = len(A_n)
439+
440+
441+
#TOF_MA = np.sqrt(np.sum)
442+
443+
# Real measurements
444+
toa_tx = np.array([[t6_tx1,t6_tx2],[t1_tx1, t1_tx2], [t2_tx1,t2_tx2], [t3_tx1,t3_tx2], [t4_tx1,t4_tx2], [t5_tx1,t5_tx2]])
445+
toa_rx = np.array([[t6_rx1,t6_rx2], [t1_rx1,t1_rx2], [t2_rx1,t2_rx2], [t3_rx1,t3_rx2], [t4_rx1,t4_rx2], [t5_rx1,t5_rx2]])
446+
447+
#Drift tag
448+
dt_new = (toa_rx[:,1]- toa_rx[:,0]) /(toa_tx[:,1] - toa_tx[:,0])
449+
450+
451+
tmp_rx = np.zeros((len(toa_rx),2))
452+
tmp_rx[:][0] = toa_rx[:][0] - toa_rx[0][0] - (toa_tx[:][0] * dt -toa_tx[0][0]*dt)
453+
tmp_rx[:][1] = toa_rx[:][1] - toa_rx[0][1] - (toa_tx[:][1] * dt -toa_tx[0][1]*dt)
454+
455+
## TDoA
456+
# tdoa = tmp_rx(:,2) - tmp_tx(:,2);
457+
tdoa = np.zeros((len(tmp_rx), 2))
458+
tdoa = tmp_rx[:, 1]
459+
tdoa = np.delete(tdoa, [0])
460+
461+
D = c*tdoa
462+
463+
#------Trilateration linear equations system-------------------
464+
A = np.array((A_n6[0] - A_n[1:n, 0], A_n6[1]-A_n[1:n, 1], A_n6[2]-A_n[1:n, 2], D)).T *2
465+
466+
b = D**2 + np.linalg.norm(A_n6)**2 - np.sum(np.square(A_n[1:n, :]), axis=1)
467+
468+
469+
470+
x_t0 = np.matmul(np.linalg.pinv(A), b.T)
471+
472+
print("x_t0 =", x_t0)
473+
#print("A =", A)
474+
#print("pinvA = ", sp.linalg.pinv(A))
475+
#-----Non linear correction (Taylor Expansion)-----------------
476+
x_t_0 = np.array((x_t0[0], x_t0[1], x_t0[2]))
477+
f = np.zeros((n-1,1))
478+
del_f = np.zeros((n-1,3))
479+
ii = 1
480+
481+
482+
483+
for ii in 1,n-1 :
484+
f[ii-1]= np.linalg.norm((x_t_0 - A_n[ii,:]), ord=2)-np.linalg.norm((x_t_0 - A_n[0,:]), ord = 2)
485+
del_f[ii-1,0] = (x_t_0[0] - A_n[ii,0])*np.linalg.norm((x_t_0 - A_n[ii,:]),ord=2)**-1 - (x_t_0[0]-A_n[0,0])*np.linalg.norm((x_t_0-A_n[0,:]), ord = 2)**-1
486+
del_f[ii-1,1] = (x_t_0[1] - A_n[ii,1])*np.linalg.norm((x_t_0 - A_n[ii,:]),ord=2)**-1 - (x_t_0[1]-A_n[0,1])*np.linalg.norm((x_t_0-A_n[0,:]), ord=2)**-1
487+
del_f[ii-1,2] = (x_t_0[2] - A_n[ii,2])*np.linalg.norm((x_t_0 - A_n[ii,:]), ord=2)**-1 - (x_t_0[2]-A_n[0,2])*np.linalg.norm((x_t_0-A_n[0,:]), ord=2)**-1
488+
489+
#print("res = ", (D-f.T) )
490+
apinv = np.linalg.pinv(del_f)
491+
abho = (D- f.T).T
492+
493+
x_t = (np.matmul(np.linalg.pinv(del_f), (D- f.T).T)).T + x_t_0
494+
495+
return x_t[0], x_t[1], x_t[2], dt_new
496+
497+
498+
499+
362500
class Astar():
363501
def __init__(self, m): #m is the only input require while definig class, it is the map of the environment
364502
self.map = m

0 commit comments

Comments
 (0)