Implementazione filtro di kalman per MPU6050

Salve a tutti e auguri di buone feste,
premetto che sto girando da giorni in rete per risolvere il problema del rumore per il mio MPU6050 senza trovare un articolo che mi spiegasse per bene come fare. In pratica sto utilizzando il giroscopio del suddetto sensore e da quello che ho capito la migliore soluzione implementabile sarebbe quella di utilizzare un filtro di Kalman per eliminare il rumore e ottenere valori stabili. Il problema è che non so proprio come fare. Il linea di massima ho capito come il filtro funzioni " teoricamente" ma non “praticamente”. C’è qualcuno così gentile da dirmi come fare postandomi un esempio di codice o un link a delle pagine che spieghino come fare la cosa passo passo? Anche perché in rete non c’è un vero e proprio how-to per questo problema e spero che questo post lo diventi. A me serve solo la velocità angolare ben filtrata e non gli angoli. Spero di essere stato chiaro e che qualcuno possa aiutarmi.

Grazie in anticipo!

Da verificare, però il DMP interno del MPU650 dovrebbe fornire anche i valori, già elaborati, dei singoli assi, il che include anche le velocità angolari del giroscopio, il DMP esegue già un Kalman pertanto è inutile perdere tempo per implementarlo su Arduino. Comunque qui dovresti trovare quello che stai cercando.

Grazie mille per la tempestiva risposta astrobeed. Comunque...mi spieghi meglio il discorso del DMP? Sono ignorante in materia...

Grazie in anticipo

Dentro gli MPU6050 c'è una unità di calcolo dedicata che si chiama DMP, Digital Motion Processor, è in grado di gestire direttamente la sensor fusion e fornire i vari dati degli assi, qui trovi una spiegazione veloce e il link dove scaricare le librerie per Arduino. Se vuoi la documentazione completa del DMP trovi tutto sul sito di invensense nella sezione developer, devi registrarti per poter scaricare il materiale.

Grazie @astrobeed. Un'altra cosa. Sono riuscito a usare le librerie che mi hai consigliato, ma ho un problema in questa ( posto solo parte del codice, è il codice d'esempio MPU6050_DPM6 di Jeff RowBerg)

        #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);
            Serial.print("\t");
            Serial.print(aaWorld.y);
            Serial.print("\t");
            Serial.println(aaWorld.z);
        #endif
    
        #ifdef OUTPUT_TEAPOT
            // display quaternion values in InvenSense Teapot demo format:
            teapotPacket[2] = fifoBuffer[0];
            teapotPacket[3] = fifoBuffer[1];
            teapotPacket[4] = fifoBuffer[4];
            teapotPacket[5] = fifoBuffer[5];
            teapotPacket[6] = fifoBuffer[8];
            teapotPacket[7] = fifoBuffer[9];
            teapotPacket[8] = fifoBuffer[12];
            teapotPacket[9] = fifoBuffer[13];
            Serial.write(teapotPacket, 14);
            teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
        #endif

in pratica posso visualizzare tanti bei valori tranne le velocità angolari del giroscopio con il DMP... sai come potrei fare??

Grazie in anticipo

apellegrino94

Penso di aver risolto aggiungendo questa linea di codice...

            // display Euler angles in degrees
      
            mpu.dmpGetGyro(gyro, fifoBuffer);

            Serial.print("gyro x y z \t");
            Serial.print(gyro[0]);
            Serial.print("\t");
            Serial.print(gyro[1]);
            Serial.print("\t");
            Serial.println(gyro[3]);

e questi sono i risultati ottenuti con la scheda ferma:

gyro x y z 3 2 19710 gyro x y z 3 2 19710 gyro x y z 3 2 19710 gyro x y z 3 2 19710

volevo sapere: 1)i valori sono quelli giusti? ovvero, rappresentano effettivamente le velocità angolari dei singoli assi? ( a me non sembra); 2)qual è il fondo scala? sempre 250°/s? in questo caso devo dividere per 131 per ottenere i valori in °/s oppure questa operazione è già effettuata dal DMP?

Attendo news

Grazie in anticipo

Credo di aver risolto :)

        #ifdef OUTPUT_READABLE_ACCGYROVALUES
            mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
            axx = ax;
            ayy = ay;
            azz = az;
            gxx = gx;
            gyy = gy;
            gzz = gz;
            Serial.print("ax,ay,az \t");
            Serial.print(axx/16384);
            Serial.print("\t");
            Serial.print(ayy/16384);
            Serial.print("\t");
            Serial.print(azz/16384);
            Serial.print("\t");
            Serial.print(" g ") ;
            Serial.print("\t");
            Serial.print("gx,gy,gz \t");
            Serial.print(gxx/131);
            Serial.print("\t");
            Serial.print(gyy/131);
            Serial.print("\t");
            Serial.print(gzz/131);
            Serial.print("\t");
            Serial.println("gradi/s");
        #endif

I valori ora sono mooooooooooolto più stabili! Attendo conferma

Puoi anche calcolare gli offset per regolare meglio il 6050. --> https://www.i2cdevlib.com/forums/topic/96-arduino-sketch-to-automatically-calculate-mpu6050-offsets/

Grazie mille Paolo...era quello che volevo fare :)