salve a tutti, ho da poco ricevuto questa IMU che ho trovato ad un prezzo molto conveniente... Il problema è che non riesco a trovare la posizione angolare partendo dalla velocità angolare, o meglio analiticamente so come fare ma non riesco a scrivere il programma... Partendo da una velocità in gradi al secondo dovrei fare l'integrale di questa sommando quindi i valori campionati in base alla frequenza di lettura dell'imu... A spiegare potrebbe essere facile, ma dal punto di vista del programma come devo fare? Ovviamente essendo l'errore ridondante dovrò poi applicare un filtro (di kalman o complementare) per avere la posizione corretta...
Grazie a tutti!!!
Devi discretizzare il tuo algoritmo, l'integrale diventa una sommatoria, a questo punto non dovrebbe essere difficile scrivere il programma..
Ciao
Cosa sai realmente a livello analisi matematica ?
Se ti dico che ti serve un Kalman o una DCM, meglio se li usi tutti e due, che mi rispondi ?
astrobeed:
Cosa sai realmente a livello analisi matematica ?
Considera che sono in 5 superiore e per adesso sono arrivato agli integrali...
Kalman l'ho sempre letto qui sul forum ma non l'ho mai studiato, DCM non l'ho mai sentito...
Su questo sito trovi ottimo materiale
Ciao
flz47655:
Su questo sito trovi ottimo materiale
Gioblu.com is for sale | HugeDomainsCiao
Volevo usare proprio quella guida ma non riesco a scaricare l'allegato in quanto mi dice che ho superato la banda giornaliera, anche se non ho mai scaricato da la... Se qualcuno potesse scaricarlo ed allegarlo qui sarebbe molto buono! 
Come volevasi dimostrare, ti mancano gli strumenti matematici, ma anche la fisica, per una cosa di questo genere, tieni presente che ottenere l'asseto 3D, anche se parliamo solamente dell'inclinazione, è una cosa che richiede tanta matematica di livello superiore.
Ti consiglio di "rovistare" dentro MultiWii, software per quadricotteri, che supporta i sensori della tua IMU ed estrapolare le sue routine per integrarle nel tuo software, non sono un gran che visto che implementano solo la parte generica dei vari algoritmi, ovvero non sono ottimizzate, però funzionano decentemente e per il tuo uso dovrebbero bastare-
Stamattina ho preso carta e penna e ho cominciato a scrivere qualche calcolo e ne è risultato fuori questo programma:
#include <Wire.h>
#include <ITG3200.h>
#define address 0x40
ITG3200 gyro = ITG3200();
float gyroX, gyroY, gyroZ;
float accX, accY, accZ;
float angoloAssoluto=0;
long previousMicros = 0;
unsigned long currentMicros;
void setup(void) {
Serial.begin(9600);
Wire.begin();
initBMA180();
delay(1000);
gyro.init(ITG3200_ADDR_AD0_LOW);
Serial.print("zeroCalibrating...");
gyro.zeroCalibrate(2500, 2);
Serial.println("done.");
previousMicros = micros();
}
void loop(void) {
while (gyro.isRawDataReady()) {
currentMicros = micros();
gyro.readGyro(&gyroX, &gyroY, &gyroZ);
Serial.print(gyroX);Serial.print("\t");
accX = readAccel3(0x03, 0x02);
accY = readAccel3(0x05, 0x04);
accZ = readAccel3(0x07, 0x06);
float intervallo = currentMicros-previousMicros; //trovo l'intervallo trascorso fra una lettura e l'altra
angoloAssoluto+=gyroX*(intervallo/1000000); //moltiplico l'accelerazione angolare (°/sec) per il tempo in secondi (dividendo quindi il valore in microsecondi per 1000000). Incremento quindi l'angolo assoluto.
Serial.print(angoloAssoluto);Serial.print("\t");
Serial.print(intervallo);Serial.print("\n");
previousMicros = currentMicros;
}
}
effettivamente ruotando di 90° il socket su cui c'è montata l'imu sul monitor seriale leggo un valore di circa 90 (che poi molto lentamente si incrementa perchè la velocità angolare non è mai 0). Questa cosa però mi suona un po' strana perchè l'accelerazione il la ottengo in rad/sec, e non effettuo nessuna conversione e come è possibile che il risultato è in gradi?
ho detto una bella cazzata, il valore l'ottengo in gradi al secondo... a questo punto dovrebbe essere corretto, non mi resta che applicare il filtro di kalman...
AGGIORNAMENTO: se quando faccio il calcolo dell'angolo assoluto il valore della velocità angolare lo passo come intero noto che il valore finale non viene incrementato col tempo, mantenendo una certa sensibilità; resta ovviamente il problema del drift....
AGGIORNAMENTO 2: Applicando la soluzione di sopra aumenta l'errore in movimento, quindi conviene lasciare il codice scritto in alto...
Non ho capito bene alcune cose riguardanti il filtro di kalman...
mettiamo che io voglia usare questo codice:
// KasBot V1 - Kalman filter module
float Q_angle = 0.01; //0.001
float Q_gyro = 0.0003; //0.003
float R_angle = 0.01; //0.03
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
// newAngle = angle measured with atan2 using the accelerometer
// newRate = angle measured using the gyro
// looptime = loop time in millis()
float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;
y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;
x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;
return x_angle;
}
quando richiamo la funzione kalmanCalculate devo passare il primo valore che come dice il commento è l'arctan2 della x e della y dell'accelerometro, ma usando queste funzioni non so che unità di misura hanno i valori restituiti e soprattutto non so se posso usarli cosi come sono per la funziona kalmanCalculate.
Per quanto riguarda newRate non è specificato se devo passare la velocità angolare (non credo) o l'angolo assoluto e se cosi fosse 'unità di misura dovrebbe essere in gradi o in radianti?
Il looptime cosa sarebbe? Devo passare il valore di "intervallo" del codice scritto da me?
Grazie ancora...
Non riesco a calcolare l'angolo di inclinazione partendo da 2 assi dell'accelerometro.
Facendo
atan2(accX, accY);
ottengo valori totalmente sbagliati che non centrano niente con l'angolo... Come potrei fare?
Hai calibrato l'accelerometro ?
La formula che usi è totalmente sbagliata, l'angolo altro non è che il seno, o il coseno a seconda del verso, dell'accelerazione previa sua normalizzazione in un valore che varia tra 0 e 1 (0 = 0G = 0°, 1 = 1G = 90°)
E poi i computer non calcolano gli angoli in gradi ma in radianti (360Gradi sono 2Pi).
Ciao Uwe
astrobeed:
[quote author=Dario Gogliandolo link=topic=138083.msg1039064#msg1039064 date=1356006218]
ottengo valori totalmente sbagliati che non centrano niente con l'angolo... Come potrei fare?
Hai calibrato l'accelerometro ?
La formula che usi è totalmente sbagliata, l'angolo altro non è che il seno, o il coseno a seconda del verso, dell'accelerazione previa sua normalizzazione in un valore che varia tra 0 e 1 (0 = 0G = 0°, 1 = 1G = 90°)
[/quote]
Si, l'accelerometro è calibrato.
Io vedo che il valore minimo che mi può restituire è -8191 che equivale a -90° e il valore massimo è 8191 che equivale a 90°. Come faccio a far rientrare questi valori in un range compreso fra -1 e 1?
io ho provato facendo cosi:
float beccheggio()
{
float angVal=0;
if (axis[Y]>0)
{
angVal=mapFloat(axis[Y], 0,8191, 0, 1);
}
else
{
angVal=mapFloat(axis[Y], -8191,0, -1, 0);
}
//float angVal = mapFloat(axis[Y], -8191,8191, -1, 1);//((float)axis[Y]/8191.0*bma180.getGSense())/2; //mapFloat(axis[Y], -8191,8191, -90, 90);
return (angVal);
}
float mapFloat(int x, int in_min, int in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
ma effettivamente utilizzando questo codice quando il valore avis[Y] è superiore a 6800 circa il valore di angVal super l'1 e questo mi sembra molto strano perché quando richiamo la funzione mapFloat come massimo imposto 1 tanto è vero che facendo gli stessi calcoli con carta e penna tutto quadra perfettamente...
Non capisco come mai questi comportamenti cosi strani...
a me va... non supera mai i limiti impostati...
float mapFloat(float x, float in_min, float in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void loop() {
// set the cursor to column 0, line 1
// (note: line 1 is the second row, since counting begins with 0):
lcd.setCursor(0, 1);
// print the number of seconds since reset:
int i ;
for (i = -8191; i < 8191; i++){
float angVal = mapFloat(i, -8191,8191, -1, 1);
lcd.clear();
lcd.print("i:" );
lcd.print (i);
lcd.print (" ->");
lcd.print (angVal);
delay(10);
}
}
pero non capisco nel tuo programma che fine ha poi fatto l'arcoseno...
Io vedo che continui a non capire, ma come puoi pensare di convertire direttamente l'accelerazione in gradi ?
Ti ho detto chiaramente che l'accelerazione rappresenta il seno, o coseno, dell'angolo non che è l'angolo.
qsecofr:
pero non capisco nel tuo programma che fine ha poi fatto l'arcoseno...
ahhh si aggiungo anche una cosa: forse già la sai ma le operazioni sui double a livello di tempi di calcolo sono veleno... spesso in tal senso si creano degli array di "arcoseni" nel tuo caso già calcolati a partire magari dai gradi che puoi ipotizzare interi e che quindi ti fanno da "indice" dell'array.
Comunque casomai vedrai a tempo debito.
astrobeed:
[quote author=Dario Gogliandolo link=topic=138083.msg1039539#msg1039539 date=1356030426]
Io vedo che il valore minimo che mi può restituire è -8191 che equivale a -90° e il valore massimo è 8191 che equivale a 90°.
Io vedo che continui a non capire, ma come puoi pensare di convertire direttamente l'accelerazione in gradi ?
Ti ho detto chiaramente che l'accelerazione rappresenta il seno, o coseno, dell'angolo non che è l'angolo.
[/quote]
Scusa astrobeed ma non è che non ho capito, al massimo mi sono spiegato male. 8191 effettivamente corrisponde a 90° ma non ho detto che faccio la proporzione fra i 2 valori. io la proporzione la faccio fra -8191/8191 e -1/1 e successivamente faccio l' arcsen (o l'arccos) del valore. Infatti nel codice che ho postato
if (axis[Y]>0)
{
angVal=mapFloat(axis[Y], 0,8191, 0, 1);
}
else
{
angVal=mapFloat(axis[Y], -8191,0, -1, 0);
}
il valore massimo in uscita della funziona mapFloat è 1.
Non per dire ma un briciolo di trigonometria me la ricordo! 
AGGIORNAMENTI:
Sono riuscito a trovare l'angolo assoluto sia di gyroscopio che di accelerometro ed ho provato ad usare sia il filtro di kalman che quello complementare: quello di kalman mi da come risultato lo stesso valore identico dell'angolo dell'accelerometro, mentre quello complementare (di 1° livello) mi da in uscita un valore accettabile. Il codice che ho usato é il seguente:
#include <Servo.h>
#include <Wire.h>
#include <ITG3200.h>
#include <bma180.h>
#include <math.h>
#define address 0x40#define RAD_TO_DEG 57.295779513082320876798154814105
ITG3200 gyro = ITG3200();
BMA180 bma180 = BMA180();
Servo myservo;
int accAxis[3];
float gyroAxis[3];
float angleGyro[3];
const int X=0, Y=1, Z=2;
float accErr[3] = {0.00, 1.50, 0.00};
long previousMicros = 0;
unsigned long currentMicros;//###### COMPLEMENTARY #######
float tau=0.01;
float a=0.0;
float x_angleC=0;
//##### END COMPLEMENTARY ####//######## KALMAN ##########
float Q_angle = 0.001;//0.01; //0.001
float Q_gyro = 0.003;//0.0003; //0.003
float R_angle = 0.03;//0.01; //0.03
float x_angle = 0;
float x_bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float y, S;
float K_0, K_1;
//###### END KALMAN ########long intervallo2, currentMillis, previousMillis;
void setup(void) {
Serial.begin(115200);
Wire.begin();
Serial.println("Inizializzazione accelerometro...");
bma180.setAddress(BMA180_ADDRESS_SDO_LOW);
bma180.SoftReset();
bma180.enableWrite();
bma180.SetFilter(bma180.F10HZ);
bma180.setGSensitivty(bma180.G1);
bma180.SetSMPSkip();
bma180.SetISRMode();
bma180.disableWrite();
Serial.println("Completato.");
myservo.attach(9);
gyro.init(ITG3200_ADDR_AD0_LOW);Serial.print("Calibrazione zero gyro...");
gyro.zeroCalibrate(2500, 2);
Serial.println("Completato.");
delay(100);
previousMicros = micros();
}float gyroY=0;
boolean stato=false;void loop(void) {
currentMillis=millis();while (gyro.isRawDataReady()) {
currentMicros = micros();
float intervallo = currentMicros-previousMicros;
currentMillis=millis();
intervallo2=currentMillis-previousMillis;
if (intervallo2>500 && stato==false)
{
myservo.write(120);
stato=true;
previousMillis=currentMillis;
}
else if (intervallo2>500 && stato==true)
{
myservo.write(60);
stato=false;
previousMillis=currentMillis;
}bma180.readAccel(accAxis);
gyro.readGyro(&gyroAxis[X], &gyroAxis[Y], &gyroAxis[Z]);
//gyroY+=gyroAxis[Y]*(intervallo/1000000); //FUNZIONA
float angolo = Complementary(getAngleAcc(Y),gyroY, intervallo);/*Serial.print(getAngleAcc(Y));
Serial.print("\t");Serial.print((float)((int)(-gyroY10))/10);
//Serial.print(getAngleGyro(Y, intervallo));
Serial.print("\t");/
Serial.println((float)(int)((angolo)*10)/10);
previousMicros = currentMicros;}
}float mapFloat(int x, int in_min, int in_max, float out_min, float out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}float getAngleGyro(int asse, float tempo)
{
angleGyro[Y]+=gyroAxis[Y](tempo/1000000);
return angleGyro[Y];
//angleGyro[asse]+=gyroAxis[asse](tempo/1000000);
//return angleGyro[asse];
}float getAngleAcc(int asse)
{
float angVal=0;
angVal = mapFloat(accAxis[asse], -8191,8191, -1, 1);
return acos(angVal)*RAD_TO_DEG-90-accErr[Y];//(float)(int)((acos(angVal)*RAD_TO_DEG-90-accErr[Y])*10)/10;
}float kalmanCalculate(float newAngle, float newRate,int looptime)
{
float dt = float(looptime)/1000;
x_angle += dt * (newRate - x_bias);
P_00 += - dt * (P_10 + P_01) + Q_angle * dt;
P_01 += - dt * P_11;
P_10 += - dt * P_11;
P_11 += + Q_gyro * dt;y = newAngle - x_angle;
S = P_00 + R_angle;
K_0 = P_00 / S;
K_1 = P_10 / S;x_angle += K_0 * y;
x_bias += K_1 * y;
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01;return x_angle;
}float Complementary(float newAngle, float newRate,int looptime) {
float dtC = float(looptime)/1000.0;
a=tau/(tau+dtC);
x_angleC= a* (x_angleC + newRate * dtC) + (1-a) * (newAngle);
return x_angleC;
}
A questo punto ho montato tutta l'imu su un servo facendola girare di 60° (da 60 a 120) ed ho ottenuto il grafico in allegato. Secondo voi come è il risultato del filtro?
Ma se usassi 2 assi dell'accelerometro e 1 del giroscopio la misura migliora? Se si come posso fare ad usare 2 assi dell'accelerometro?