Sistemato Kalman, aggiunto calcolo distanza testa, modificata visualizzazione punto osservato