chi mi spiega gli algoritmi di sensor fusion?

ciao a tutti, sono qui per chiedere spiegazioni riguardo algoritmi di sensor fusion e come applicarli al mondo dei microcontrollori.

naturalmente non è che vengo qui con pretese del tipo "vi do la nomina di professori insegnatemi tutto" infatti qualche testo trovato in internet l'ho letto, in particolare sul direction cosine matrix ma sinceramente mi sono incasinato più di prima :sweat_smile:
in pratica alla fine ho solo capito che l'algoritmo utilizza seni e coseni per limitare il suo errore nel tempo ma potrei sbagliarmi, per me tutto il resto è solo casino XD

ciò che chiedo è qualche spiegazione abbastanza "terra terra" in quanto credo di essere a digiuno anche della matematica necessaria (e mi direte "allora aspetta qualche anno e fai qualche univeristà", giustamente ma a me ha intrippato troppo questa cosa della sensor fusion quindi voglio consceeeenzaaaa :smiley: ) anche postando link a testi (l'inglese non è un problema)

grazie a tutti coloro che interranno :wink:

Non ho trovato molto per la mia poca originalità nel cercare, comunque un punto di partenza per una ricerca forse in questa pagina la trovi, se ho capito bene cosa cerchi.

http://www.appuntielettronica.altervista.org/sensori-di-movimento.html

carino il link, affronta i problemi principali che possono dare i sesori, ma non affronta come "pulirli" nè tantomeno il passo successivo, ovvero il vero e proprio algoriutmo di sensor fusion.

Ti posso dire a grandio linee l'idea: l'integrazione del giroscopio è l'unica affidabile per tutti gli assi, in un dato momento, quindi parti da quella. Poi usi la stima del giroscopio per eliminare l'accelerazione dall'accelerometro e quindi trovare il vettore gravità, che ti da quindi un valore affidabile per pith e roll, ma non per yaw.

Il magnetometro è molto lento rispetto agli altri sensori, ma da un valore affidabile per x, y e z, quindi...

grazie ad entrambi per i link me li sto spulciando per bene.

però lesto non ho capito una cosa:
quando nella matrice porta i vettori del corpo nel sistema globale indica (prendo un valore) cos(I.i) (i vettore del corpo ed I vettore globale)
ma quel coseno sarebbe un coseno di similitudine?

uhh la matematica dietro queste cose la conosco a grandi linee, ed infatti la parte in cui faccio fatica è proprio quella del settaggio degli assi e rappresentazione grafica della DCM.

Però io direi che quello è il coseno tra vettori, (v·w)/(|v|*|w|) (con i = v e I =w , bhe l'ordine non cambia). poi però semplifica ulteriormente rendendo il tutto un prodotto scalare sfruttando il fatto che quelli sono vettori direzionali; hanno lunghezza sempre 1, perchè rappresentano una direzione, non un vero e proprio vettore: qualsiasi vettore è dato dalla sua lunghezza per il suo vettore direzionale

Where |i| is the norm (length) of the i unity vector and cos(I,i) is the cosine of the angle formed by the vectors I and i. Using the fact that |I| = 1 and |i| = 1 (they are unit vectors by definition). We can write:

ixG = cos(I,i) = |I||i| cos(I,i) = I.i

Where I.i. is the scalar (dot) product of vectors I and i

io mi sono passato metà pomeriggio a ripassare la matematica e capire tutta la parte delle matrici XD
comunque ho capito che con I,i in pratica indica l'angolo tra l'asse di riferimento ed il nostro vettore.

diciamo che mi si sta facendo una gran confusione in testa e pian piano si sta mettendo a posto XD poi il lavoro duro sarà anche portare il codice in c++ (avevo trovato un modo per fare prodotti tra matrici coi puntatori)

comunque vediamo se ho capito:
col dcm noi creiamo una matrice i cui dati all'interno sono semplicemente i valori dei coseni tra i nostri assi di riferimento ed i vettori reali che noi abbiamo sulla nostra imu.
tutto questo poi ci farà calcolare il vettore r ma questo vettore ancora non ho capito a che serve :sweat_smile:
e comunque questo sistema alla fine serve solo per essere poi "amalgamato" con i dati da accelerometri per avere lo spostamento di tutto il sistema nello spazio, giusto? in pratica noi i valori dei vari coseni andremo a sommarli per ogni asse globale e poi a sottrarre questo valore dalla corrispondente proiezione del valore dell'acelerometro su quell'asse?

allora, premetto che ne so una ben poco, e imparo anche io.

ci sono due terne, Oxyz, rappresentata dai versori (vettore unitario) i,j,k e OXYZ rapprestentata dai versori I,J, K

Oxyz e OXYZ hanno sempre la stessa origine, ovvero il cetro del frame: MA OXYZ rappresenta l'orientamento del "pianeta" (okok, parlo terra terra), quindi è "fissa" e rappresenta il nostro punto di riferimento, mentre Oxyz rappresenta l'orientamento della IMU.
Ora abbaimo qualcosa che rappresenta il nostro orientamento rispetto ad un punto "fisso" (quì un minimo di relatività: quel punto non è assolutamente fisso, è fisso relativamente al pianeta, ma si muove, per esempio, relativamente al sole o alla galassia etc.. non esiste uyn punto fisso "assoluto", e quind abbiamo scelto un punto relativamente fisso per i nostri scopi detto OXYZ.. volendo potremmo usare il sole, o il centro della galassia.. molto più complesso, ma se vuoi guidare unsa sonda ai limiti dello spazio noto... :grin:)

Poi inizia a dire, vogliamo rappresentare la nostra terna "corpo" rispetto (ovvero relativamente) alla terna "mondo", e dice (con quella spigazione che non capisco bene, in cui tira in mezzo il coseno tra vettori) che basta fareil prodottoi scalare tra i versori della terna "corpo" e la terna "mondo", il che crea la matrice di coseni in figura (la DCM "da frame a mondo")

Poi mostra una matrice simile: solo che questa serve per rapèpresentare le coordinate globali rispetto a quelle locali. (la DCM "da mondo a frame")

Ora puoi trasformare un vettore (nell'esempio r) RELATIVO AL MONDO verso il FRAME e viceversa, semplicemente moltiplicando il vettore per la DCM "da frame a mondo" o "da mondo a frame"

In oltre fa notare come una DCM sia la TRASPOSTA dell'altra (in pratica inverti righe e colonne). Matrice trasposta - Wikipedia
Altra particolarità è che il prodotto scalare tra le due matrici da la matrice identità (in pratica la diagonale principale, da in alto a sinistra a in basso a destra è 1, notare che può esistere solo su matrici quadrate, ovvero conlo stesso numero di righe e colonne), e dunque sono ortogonali

Fine parte 1

edit: io non sono partito dalla matematica, come leggi sono "igniorante assai",ed ho usato un algoritmo pre-cotto. Open source IMU and AHRS algorithms – x-io Technologies originariamente di Sebastian Madgwick e rielaborato da Robert Mayhony. è lo stesso usato dalla freeIMU e varie.

Ok fin qui c'ero, quella spiegazione che non capisci è quella |I|*|i|*cos(i,I)?
Quello è il prodotto scalare tra il vettore di riferimento globale (della terra) con la proiezione del vettore che abbiamo sullo stesso asse del vettore della terra.

Io mi sono perso solo all'ultimo passaggio eq. 1.5 e 1.4

Poi la parte relativa alle velocità angolari devo ancora vederla na così a vista mi pare la faccia complessa. Mi chiedo se non basta la solita a = va * dt (a angolo, va velocità angolare e dt il delta tempo)

dipende.
a = va * dt funziona se a è relativa alla posizione precedente del frame
a = a + va * dt funziona se a è relativa alla posizione iniziale (o del pianeta, nel nostro caso)

ma come fai a passare da a relativa al frame ad a relativa al pianeta? bhe, con quello che ti spiga la parte 1 dell'articolo. prendendo il vettore formato dagli angoli ax, ay, az (che poi sarebbe il vettore che lui chiama i) e facendo il prodotto scalare per la DCM "da frame a mondo"

(e comunque la risposta è no, vedi articolo linkato prima sui giroscopi, perchè così usi solo i giroscopi, lo sai? prova a farlo, è interessante!)

Ti chiederai, ma a che mi serve? bhe, ancora non ho letto il resto, ma immagino: i giroscopi danno angoli rispetto AL FRAME. Gli accelerometri e magnetometri RISPETTO AL MONDO. l'angolazione finale ci interessa RISPETTO AL MONDO, o meglio la differenza tra frame e mondo

lesto:
dipende.
a = va * dt funziona se a è relativa alla posizione precedente del frame
a = a + va * dt funziona se a è relativa alla posizione iniziale (o del pianeta, nel nostro caso)

ma come fai a passare da a relativa al frame ad a relativa al pianeta? bhe, con quello che ti spiga la parte 1 dell'articolo. prendendo il vettore formato dagli angoli ax, ay, az (che poi sarebbe il vettore che lui chiama i) e facendo il prodotto scalare per la DCM "da frame a mondo"

(e comunque la risposta è no, vedi articolo linkato prima sui giroscopi, perchè così usi solo i giroscopi, lo sai? prova a farlo, è interessante!)

Ti chiederai, ma a che mi serve? bhe, ancora non ho letto il resto, ma immagino: i giroscopi danno angoli rispetto AL FRAME. Gli accelerometri e magnetometri RISPETTO AL MONDO. l'angolazione finale ci interessa RISPETTO AL MONDO, o meglio la differenza tra frame e mondo

beh il solo giroscopio nel tempo accumula errori ovviamente..

ma quindi noi volessimo usare il dcm solo coi giroscopi dovremmo prendere la matrice iniziale (da frame a mondo) come

{
  {1, 0, 0},
  {0, 1, 0},
  {0, 0, 1}
}

quindi andare a sostituire i coseni ottenuti dagli angoli a loro volta ottenuti dai giroscopi ma come facciamo a "filtrare" i risultati del giroscopio nel tempo?

(inizio a pensare che userò solo equazioni già pronte in rete :sweat_smile: )

no, quella è la matrice identità, solo una particolarità che dice tornare utile dopo.

quello che vuoi usare è

mettendo al posto di i, j, k i versori dal giroscopio (credo i={x, 0, 0} j={0,y,0} k={0,0,z}), e I = {1,0,0} T, J={0,1,0} T , K = {0,0,1} T

ma come facciamo a "filtrare" i risultati del giroscopio nel tempo?

la DCM non filtra, ma alla fine quando integra usa un PID, o meglio solo la parte PI.

La sua vera forza sta nel correggere l'errore accumulato prorio fondendo assieme i dati dai sensori, infatti per funzionare DEVE avere una lettura giroscopi E alemno un'altra lettura. Tra l'altro tempo fa trovai un paper di uno che al posto del magnetometro usava il GPS: ma nulla vieta di usare sia magnetometro che gps! certo bisogna capire bene come fare per non incasinare tutto,ma credo che ogni sensore sia fondamentalmente indipendente.

allego l'algoritmo usato dalla freeimu, pulito dalle condizioni 6/9DOF

void  FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
  float recipNorm;
  float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
  float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
  float qa, qb, qc;

  // Auxiliary variables to avoid repeated arithmetic
  q0q0 = q0 * q0;
  q0q1 = q0 * q1;
  q0q2 = q0 * q2;
  q0q3 = q0 * q3;
  q1q1 = q1 * q1;
  q1q2 = q1 * q2;
  q1q3 = q1 * q3;
  q2q2 = q2 * q2;
  q2q3 = q2 * q3;
  q3q3 = q3 * q3;

  // Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
  if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
    float hx, hy, bx, bz;
    float halfwx, halfwy, halfwz;
    
    // Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;
    
    // Reference direction of Earth's magnetic field
    hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
    hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
    bx = sqrt(hx * hx + hy * hy);
    bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));
    
    // Estimated direction of magnetic field
    halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
    halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
    halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);
    
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex = (my * halfwz - mz * halfwy);
    halfey = (mz * halfwx - mx * halfwz);
    halfez = (mx * halfwy - my * halfwx);
  }

  // Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
  if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
    float halfvx, halfvy, halfvz;
    
    // Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;
    
    // Estimated direction of gravity
    halfvx = q1q3 - q0q2;
    halfvy = q0q1 + q2q3;
    halfvz = q0q0 - 0.5f + q3q3;
  
    // Error is sum of cross product between estimated direction and measured direction of field vectors
    halfex += (ay * halfvz - az * halfvy);
    halfey += (az * halfvx - ax * halfvz);
    halfez += (ax * halfvy - ay * halfvx);
  }

  // Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
  if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
    // Compute and apply integral feedback if enabled
    if(twoKi > 0.0f) {
      integralFBx += twoKi * halfex * (1.0f / sampleFreq);  // integral error scaled by Ki
      integralFBy += twoKi * halfey * (1.0f / sampleFreq);
      integralFBz += twoKi * halfez * (1.0f / sampleFreq);
      gx += integralFBx;  // apply integral feedback
      gy += integralFBy;
      gz += integralFBz;
    }
    else {
      integralFBx = 0.0f; // prevent integral windup
      integralFBy = 0.0f;
      integralFBz = 0.0f;
    }

    // Apply proportional feedback
    gx += twoKp * halfex;
    gy += twoKp * halfey;
    gz += twoKp * halfez;
  }
  
  // Integrate rate of change of quaternion
  gx *= (0.5f * (1.0f / sampleFreq));   // pre-multiply common factors
  gy *= (0.5f * (1.0f / sampleFreq));
  gz *= (0.5f * (1.0f / sampleFreq));
  qa = q0;
  qb = q1;
  qc = q2;
  q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);
  
  // Normalise quaternion
  recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
  q0 *= recipNorm;
  q1 *= recipNorm;
  q2 *= recipNorm;
  q3 *= recipNorm;
}

come vedi lui calcola la gravità/campo magnetico RISPETTO AL FRAME che ci aspettiamo, dopo di che in halfex, halfey, halfez salva la somma dell'errore dal valore che si è invece letto. Notare che "normalizza" le misure, ovvero divide il vettore per la sua lunghezza: in questo modo ottiene il vettore di lunghezza 1, ovvero il famoso Versore!

Ora, lui somma questo errore al valore attuale del giroscopio, usando un PI, dopo di che aggiorna il quaternione.

un quaternione è un metodo alternativo per memorizzare le rotazioni. x y e z sono soggetti al gimbal-lock, quindi si usavano le matrici di rotazione (che sono matrici 3x3), ma poi ci si è accorti che usando i numeri irreali, ed aggiungendo quindi una dimensione, si otteneva un sistema senza gimbal-lock comunque stabile e che richiede meno spazio e meno cpu, tanto che ormai quasi tutti i motori grafici stanno abbanando le matrici in favore dei quaternioni. Lo stesso vale per le matrici di rototraslazione (4x4, rotazione + posizione), che sono sostitute da queternione + x y z :slight_smile:

x iscrizione.

ok inizio a venirne più o meno fuori tuttavia al lato pratico vorrei poi capire come utilizzare tutto questo.

il mio scopo finale è creare un sistema di gestione completamente automatizzato di un aeromodello (non farei con arduino il "cervello" perchè so che ha limiti in questo campo) e quindi io una volta che ho il quaternione con dentro tutti i dati delle rotazioni e posizioni io dovrei sommare un altro quaternione contenente il vettore di dove voglio che vada il mio aereo?
quindi questi dati al lato pratico come li utilizzo?
se il mio scopo finale fosse attaccare dei sevi alla scheda e poter comandare l'aereo devo metterci di mezzo un'altro algoritmo di pid giusto? ma da dove vado a prendere i dati? XD

ok inizio a venirne più o meno fuori tuttavia al lato pratico vorrei poi capire come utilizzare tutto questo.

eddai siamo ancora alla parte 1!!

il mio scopo finale è creare un sistema di gestione completamente automatizzato di un aeromodello

non farei con arduino il "cervello" perchè so che ha limiti in questo campo

e fin quì ci siamo

e quindi io una volta che ho il quaternione con dentro tutti i dati delle rotazioni e posizioni io dovrei sommare un altro quaternione contenente il vettore di dove voglio che vada il mio aereo

Il quaternione contiene la ROTAZIONE, non la traslazione (spostamento), quindi ci servirà una nuova terna (visto che le terne attuale SI MUOVONO con il modello, dato che devono avere la stessa origine) che sarà orientata come la terna OXYZ ma come ORIGINE avrà il punto di partenza del modello. Per gli spostamenti è tutto molto più facile, vistro che si tratta di somme e sottrazioni di vettori... certo bisogna calcolare il vettore velocità attuale, calcolare in base ad esso qual'è il range degli eventuali vettori di ACCELERAZIONE (in pratica, l'effetto curva a sinistra non è lo stesso a 10 e a 20 all'ora, e di certo non puoi passare da 100 a 0 all'ora in un secondo), quindi applicarli.

Come puoi vedere ci sono 3 grossi step:
la rotazione (con la DCM)
la traslazione (integrando le accelerazioni "pulite" dalla gravità stimata grazie alla DCM, GPS etc..)
calcolo di rotta (fisica, fisica, fisica! e test sul campo per scoprire i comportamenti del mezzo)

Visto che il fine è lo stesso, ti linko il modello di aereo RC che intendo automatizzare, è un trainer "gommolone" molto semplice sia da guidare che da costruire, perchè se è semplice per un umano è semplice anche per un computer (spero), e perchè da (ri)costruire costa veramente poco (con 25€ di fogli depron ci fai almeno 4 o 5 modelli interi), si chiama blue baby ** Blu-Baby Primary Trainer ** Plans, Pics and Fun! - RC Groups, però ho usatoi le ali KFM3 invece che le originali (tutti i disegni sono nel link), sarebbe ottimo se riuscissimo a partire da un modello simile e poi, una volta che funziona, trovare queli parametri sono da modificare per rendere il codice adattabile.

il calcolo delle traiettorie teoriche non mi preoccupa (calcolo distanza e prua tra 2 coordinate, calcoli con gps e geometria sferica), ciò che mi preoccupa è il modificarle per poi ottenere il movimento a me necessario per tornare in rotta (in pratica in caso di vento cosa devo fare?) e per questo userei accelerometri e magnetometri.

ciò che ancora non capisco è come fa nel quaternione a registrare le rotazioni? nel senso che unità di misura usa? che assi di riferimento?
sono un tecnico non un matematico dai :sweat_smile:

lesto:
no, quella è la matrice identità, solo una particolarità che dice tornare utile dopo.

quello che vuoi usare è

mettendo al posto di i, j, k i versori dal giroscopio (credo i={x, 0, 0} j={0,y,0} k={0,0,z}), e I = {1,0,0} T, J={0,1,0} T , K = {0,0,1} T

ma come facciamo a "filtrare" i risultati del giroscopio nel tempo?

la DCM non filtra, ma alla fine quando integra usa un PID, o meglio solo la parte PI.

La sua vera forza sta nel correggere l'errore accumulato prorio fondendo assieme i dati dai sensori, infatti per funzionare DEVE avere una lettura giroscopi E alemno un'altra lettura. Tra l'altro tempo fa trovai un paper di uno che al posto del magnetometro usava il GPS: ma nulla vieta di usare sia magnetometro che gps! certo bisogna capire bene come fare per non incasinare tutto,ma credo che ogni sensore sia fondamentalmente indipendente.

allego l'algoritmo usato dalla freeimu, pulito dalle condizioni 6/9DOF

void  FreeIMU::AHRSupdate(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {

float recipNorm;
float q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
float halfex = 0.0f, halfey = 0.0f, halfez = 0.0f;
float qa, qb, qc;

// Auxiliary variables to avoid repeated arithmetic
q0q0 = q0 * q0;
q0q1 = q0 * q1;
q0q2 = q0 * q2;
q0q3 = q0 * q3;
q1q1 = q1 * q1;
q1q2 = q1 * q2;
q1q3 = q1 * q3;
q2q2 = q2 * q2;
q2q3 = q2 * q3;
q3q3 = q3 * q3;

// Use magnetometer measurement only when valid (avoids NaN in magnetometer normalisation)
if((mx != 0.0f) && (my != 0.0f) && (mz != 0.0f)) {
float hx, hy, bx, bz;
float halfwx, halfwy, halfwz;

// Normalise magnetometer measurement
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
mx *= recipNorm;
my *= recipNorm;
mz *= recipNorm;

// Reference direction of Earth's magnetic field
hx = 2.0f * (mx * (0.5f - q2q2 - q3q3) + my * (q1q2 - q0q3) + mz * (q1q3 + q0q2));
hy = 2.0f * (mx * (q1q2 + q0q3) + my * (0.5f - q1q1 - q3q3) + mz * (q2q3 - q0q1));
bx = sqrt(hx * hx + hy * hy);
bz = 2.0f * (mx * (q1q3 - q0q2) + my * (q2q3 + q0q1) + mz * (0.5f - q1q1 - q2q2));

// Estimated direction of magnetic field
halfwx = bx * (0.5f - q2q2 - q3q3) + bz * (q1q3 - q0q2);
halfwy = bx * (q1q2 - q0q3) + bz * (q0q1 + q2q3);
halfwz = bx * (q0q2 + q1q3) + bz * (0.5f - q1q1 - q2q2);

// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex = (my * halfwz - mz * halfwy);
halfey = (mz * halfwx - mx * halfwz);
halfez = (mx * halfwy - my * halfwx);

}

// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if((ax != 0.0f) && (ay != 0.0f) && (az != 0.0f)) {
float halfvx, halfvy, halfvz;

// Normalise accelerometer measurement
recipNorm = invSqrt(ax * ax + ay * ay + az * az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;

// Estimated direction of gravity
halfvx = q1q3 - q0q2;
halfvy = q0q1 + q2q3;
halfvz = q0q0 - 0.5f + q3q3;

// Error is sum of cross product between estimated direction and measured direction of field vectors
halfex += (ay * halfvz - az * halfvy);
halfey += (az * halfvx - ax * halfvz);
halfez += (ax * halfvy - ay * halfvx);

}

// Apply feedback only when valid data has been gathered from the accelerometer or magnetometer
if(halfex != 0.0f && halfey != 0.0f && halfez != 0.0f) {
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi * halfex * (1.0f / sampleFreq); // integral error scaled by Ki
integralFBy += twoKi * halfey * (1.0f / sampleFreq);
integralFBz += twoKi * halfez * (1.0f / sampleFreq);
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}

// Apply proportional feedback
gx += twoKp * halfex;
gy += twoKp * halfey;
gz += twoKp * halfez;

}

// Integrate rate of change of quaternion
gx *= (0.5f * (1.0f / sampleFreq)); // pre-multiply common factors
gy *= (0.5f * (1.0f / sampleFreq));
gz *= (0.5f * (1.0f / sampleFreq));
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb * gx - qc * gy - q3 * gz);
q1 += (qa * gx + qc * gz - q3 * gy);
q2 += (qa * gy - qb * gz + q3 * gx);
q3 += (qa * gz + qb * gy - qc * gx);

// Normalise quaternion
recipNorm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
}

Salve a tutti ragazzi.
Mi sto cimentando nella realizzazione di una IMU con und board a 9 DOF e arduino mini.
Siccome non ho trovato un qualcosa di "integrale" per capire bene come scrivere l algoritmo con i quaternioni volevo porvi una domanda.
Nel algoritmo che é postato poco sopra vengono prese le misure dirette dai sensori (RAW per intenderci) oppure vengono prima lette e ripulite da un offset (che si ottiene lasciando la IMU per qualche secondo dopo l accensione immobile).

Grazie mille
Ciao!

vanno pulite.

Come puoi notare quelle giroscopiche vanno pulite e poi convertite in radianti al secondo, mentre quelle di tutti gli altri sensori vanno "offsettate", linearizzate, termocompensate ed infine puoi lasciarle così; l'algoritmo poi si occupa di trasmormarle in un versore:

// Normalise magnetometer measurement
    recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;

ps. non sono bravo in matematica e queste formule non mi sono del tutto chiare, specialmentela parte

q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

lesto:
vanno pulite.

Come puoi notare quelle giroscopiche vanno pulite e poi convertite in radianti al secondo, mentre quelle di tutti gli altri sensori vanno "offsettate", linearizzate, termocompensate ed infine puoi lasciarle così; l'algoritmo poi si occupa di trasmormarle in un versore:

// Normalise magnetometer measurement

recipNorm = invSqrt(mx * mx + my * my + mz * mz);
    mx *= recipNorm;
    my *= recipNorm;
    mz *= recipNorm;




ps. non sono bravo in matematica e queste formule non mi sono del tutto chiare, specialmentela parte


q0 += (-qb * gx - qc * gy - q3 * gz);
  q1 += (qa * gx + qc * gz - q3 * gy);
  q2 += (qa * gy - qb * gz + q3 * gx);
  q3 += (qa * gz + qb * gy - qc * gx);

Grazie mille per la risposta.
Ok per le misure dal gyroscopio, ma se leggo il codice che mi hai inviato viene fatta la normalizzazione (componente di un vettore diviso la sua norma) del magnetometro. Per le accelerazioni invece non trovo traccia ne di normalizzazioni ne che viene fatto l offset. Hai per caso quel codice?

Posso chiederti se sei studente universitario?

Ciao

ahh ok, il codice che ho postato daper scontato che le correzzioni siano già state fatte.

per l'accelerometro, il codice che se ne occupa èilseocndo if:

// Normalise accelerometer measurement
    recipNorm = invSqrt(ax * ax + ay * ay + az * az);
    ax *= recipNorm;
    ay *= recipNorm;
    az *= recipNorm;

ps. non quotare tutto il messaggio, specialmente se èquello subito sopra, al massimo scegli delle frasi particolari