Gps Kalman Filter con Arduino Uno

Partendo da questo link volevo provare a testare il filtro Kalman sui dati di un GPS ma privo della contributo di una IMU (come fatto qui)

La libreria Arduino utilizzata (questo il link) e' una conversione di un codice in C originario

Mettendo a confronto la linea blu dei dati non filtrati con quella rossa dei dati filtrati e lasciando ferma l'antenna GPS si osserva che il filtro funziona ma non limita le tipiche fluttuazioni (anche molto significative) del segnale GPS

Questo test e' stato effettuato con due antenne a basso costo: un primo modulo senza marca di derivazione cinese ed una antenna con predisposizione per PixHawk denominata gygpsv5 con a bordo un Ublox NEO6M-V2

Utilizzando il codice scaricato da Internet senza particolari modifiche tutto funzionava con il modulo cinese ma non con Ublox nonostante i messaggi NMEA fossero correttamente ricevuti da Arduino.

Salvando i dati si capisce il motivo

Neo 6P
$GLGSV,3,3,09,88,20,181,*56
$GNGLL,4347.03855,N,01113.80955,E,132645.00,A,A*7C
$GNRMC,132646.00,A,4347.03855,N,01113.80955,E,0.030,,270319,,,A*6B
$GNVTG,,T,,M,0.030,N,0.055,K,A*3E
$GNGGA,132646.00,4347.03855,N,01113.80955,E,1,08,1.35,42.1,M,45.5,M,,*76
$GNGSA,A,3,12,24,25,32,,,,,,,,,2.61,1.35,2.23*1E
$GNGSA,A,3,68,86,77,87,,,,,,,,,2.61,1.35,2.23*12

GPS
$GPGSV,2,2,08,32,33,290,40,14,26,308,23,31,04,310,,34,,,*47
$GPRMC,133332.000,A,4347.0429,N,01113.8140,E,0.21,175.07,270319,,,A*60
$GPGGA,133333.000,4347.0428,N,01113.8140,E,1,5,1.55,36.1,M,47.6,M,,*6E
$GPGSA,A,3,12,25,24,29,32,,,,,,,,1.83,1.55,0.98*00
$GPGSV,2,133,37,25,58,293,33,24,46,141,33,29,37,207,25*75
$GPGSV,2,2,08,32,33,290,39,14,26,308,22,31,04,310,,34,,,*48
$GPRMC,133333.000,A,4347.0428,N,01113.8140,E,0.35,175.07,270319,,,A*65
$GPGGA,133334.000,4347.0427,N,01113.8141,E,1,5,1.55,36.1,M,47.6,M,,*67
$GPGSA,A,3,12,25,24,29,32,,,,,,,,1.83,1.55,0.98*00


Il modulo cinese usa delle frasi NMEA che iniziano per GP***** mentre Ublox usa frasi che usaon un prefisso GN****. La libreria TinyGPS riconosce solo il primo prefisso e quindi non sa come trattare i dati dell'Ublox ...e' stato sufficiente editare la libreria sostituendo le stringhe per rendere compatibile TinyGPS con UBlox.....ma come mai questa differenza

GN e' un prefisso generico che si usa per qualunque sorgenti date (Gps americano, Beidu, Glonass etc) mentre il prefisso GP e' relativo solo al GPS americano. (Glonass per esempio ha il prefisso GL).

-----------------------------------------------------------------
#include <SoftwareSerial.h>
#include <TinyGPS.h>
#include <GPSFilter.h>

#define RXPIN 2
#define TXPIN 3

#define GPSBAUD 9600

TinyGPS gps;
SoftwareSerial uart_gps(RXPIN, TXPIN);

GPSFilter f(2.5);

unsigned long lastUpdate;
unsigned long lastSentence;
float c;

void setup()
{
 Serial.begin(9600);
 uart_gps.begin(GPSBAUD);
 lastSentence = millis();
 lastUpdate = millis();
}


void loop()
{
 while(uart_gps.available()) {    
   char c = uart_gps.read();
    
   if(gps.encode(c)) {
     int year;
     byte month, day, hour, minute, second, hundredths;
     unsigned long age;
     gps.crack_datetime(&year,&month,&day,&hour,&minute,&second,&hundredths,&age); //Get the date from the GPS
     unsigned long date; 
     gps.get_datetime(&date,0,0);
     if(date == TinyGPS::GPS_INVALID_DATE){
        Serial.println(F("Invalid Age"));
         continue;
     }
     if(age == TinyGPS::GPS_INVALID_AGE) {
         Serial.println(F("Waiting for more valid data"));
         continue;
     }
     float latitude, longitude;
     double dlat,dlong;
     gps.f_get_position(&latitude, &longitude);

     double ulat,ulong;
     ulat = (double)latitude;
     ulong = (double)longitude;
     c = (millis() - lastSentence)/1000;
     f.update_velocity2d(ulat,ulong,c);
     lastSentence = millis();
  
     if((millis() - lastUpdate) >= 500 || lastUpdate == NULL){
          f.get_lat_long(&dlat,&dlong);
         Serial.print(latitude,8);
         Serial.print(";");
         Serial.print(longitude,8);
         Serial.print(";");
         Serial.print(dlat,8);
         Serial.print(";");
         Serial.println(dlong,8);     
        
         }
          lastUpdate = millis();
   }
 }
}

Filtro Madgwick con MPU6050 ed Arduino

Un test del filtro di Madgwick , un possibile sostituto per il filtro Kalman su processori meno potenti

La libreria per Arduino si trova a questo link

Da notare prima di tutte che :

1) la finestra di campionamento del filtro in Madgwick.begin aumenta o diminuisce la velocita' di convergenza dei dati

2) i dati devono essere inseriti nel filtro con le corrette unita' fisiche (m/sec2 e deg/sec) e non con i dati raw in uscita dal sensore

3) la libreria permette di inserire anche i dati di un sensore magnetico (che non e' pero' presente nella 6050)

Una prova veloce con il filtro Madgwick e MPU statica

Nr campioni : 73071
Media : 9.82
Range : 0.56
Std : 0.04
Varianza : 0.001
Skewness : -0.01

Nelle stesse condizioni ma con filtro Kalman

Nr campioni : 16568
Media : 9.84
Range : 0.44
Std : 0.04
Varianza : 0.001
Skewness : 0.05

Aggiornamento : per calcolare il valore della Temperatura in gradi Celsius si usa la formula

T = (lettura/340.0)+36.53

--------------------------------------------------------------------------------------------
#include  <MadgwickAHRS.h> 
#include <Wire.h>

int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, temperature;


Madgwick  MadgwickFilter;

void setup() {
Serial.begin(115200);
  //Inizio configurazione clock interno 
  
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();
  
  // Inizializzazione Accelerometro intervallo di 8g Fattore di scala 4096
  Wire.beginTransmission(0x68);
  Wire.write(0x1C);
  Wire.write(0x10);
  Wire.endTransmission();
  
  // Inizializzazione Gyro Intervallo di 500 deg/sec LSB (fattore di scala) 65.5 deg/s
  Wire.beginTransmission(0x68);
  Wire.write(0x1B);
  Wire.write(0x08);
  Wire.endTransmission();

  // Inizializzazione LPF (Filtro passa basso) 43 Hz
  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x03);
  Wire.endTransmission();
  
  MadgwickFilter.begin(100);  // 100 Hz

}

void loop() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission();
  Wire.requestFrom(0x68, 14);
  while (Wire.available() < 14);
  axRaw = Wire.read() << 8 | Wire.read();
  ayRaw = Wire.read() << 8 | Wire.read();
  azRaw = Wire.read() << 8 | Wire.read();
  temperature = Wire.read() << 8 | Wire.read();
  gxRaw = Wire.read() << 8 | Wire.read();
  gyRaw = Wire.read() << 8 | Wire.read();
  gzRaw = Wire.read() << 8 | Wire.read();

  /*Serial.print(axRaw/4096.0); Serial.print(",");
  Serial.print(ayRaw/4096.0); Serial.print(",");
  Serial.print(azRaw/4096.0); Serial.print(",");
  Serial.print(gxRaw/65.5); Serial.print(",");
  Serial.print(gyRaw/65.5); Serial.print(",");
  Serial.println(gzRaw/65.5);*/
  MadgwickFilter.updateIMU(gxRaw/65.5,gyRaw/65.5,gzRaw/65.5,axRaw/4096.0,ayRaw/4096.0,azRaw/4096.0) ;
  Serial.println(MadgwickFilter.getRoll());
  //ROLL  = MadgwickFilter.getRoll();
  //PITCH = MadgwickFilter.getPitch();
  //YAW   = MadgwickFilter.getYaw();
  delay(2);
}

Filtro Kalman e tempo di campionamento

Stavo continuando a provare questa libreria del filtro Kalman per MPU quanto, per risparmiare spazio su disco, ho provato a modificare il tempo di campionamento da 20 ms ad 1 secondo ed ' accaduto qualcosa che non mi aspettavo

Tempo di campionamento 1 sec

Nr campioni : 5085
Media : -0.35
Range : 7.44
Std : 0.21
Varianza : 0.044
Skewness : -14.8

Tempo di campionamento 0.02 sec

Nr campioni : 67889
Media : -0.35
Range : 0.33
Std : 0.0.37
Varianza : 0.0014
Skewness : -0.011

Come si vede il valore medio e' comparabile ma la standard deviation e' decisamente peggiore con il tempo di campioanmento di 1 secondo

MPU 6050 con filtro Kalman ed Arduino Uno

Ho trovato questa comoda libreria per interfacciare MPU6050 (un accelerometro-giroscopio triassiale) con Arduino e processare i dati con il filtro di Kalman in tempo reale

(Per un articolo di data fusion sui dati di accelerometro e giroscopio si puo' andare a questo link applicato ai filtri Kalman)

La connesione tra MPU 6050 (anche chiamato GY 521) avviene sul canale I2C. E' quindi collegare Vcc a 3,3V, GND e SDA su pin Arduino A4 ed SCL su pin Arduino A5

Ho acquisito un po' di misure e le ho poi processate con Octave. Si vede chiaramente che il valore medio dell'angolo di roll e' comparabile tra le misure con e senza filtro di Kalman...il dato che varia in modo sensibile e' la standard deviation (le misure sono state effettuate a sensore fermo e spengendo e riaccendendo il sensore per evitare un eventuale drift del giroscopio)

Le misure sono angoli di roll espressi in gradi decimali

Set senza filtro Kalman
Nr misure : 7572
Media : -6.76
Range : 2.15
Stdev : 0.2
Varianza : 0.041
Skewness : 0.054



Set 1 con fil tro Kalman
Nr misure : 28013
Media : -6.71
Range : 0.46
Stdev : 0.05
Varianza : 0.003
Skewness : -0.051


Set 2 con fil tro Kalman
Nr misure  : 11844
Media : -6.71
Range : 0.46
Stdev : 0.05
Varianza : 0.003
Skewness : -0.12



Set 3 con fil tro Kalman
Nr misure : 5545
Media : -6.76
Range : 0.35
Stdev : 0.05
Varianza : 0.0025
Skewness : 0.14

Filtro Kalman su dati accelerometro Android

Una implementazione del filtro kalman su dati sui dati dell'accelerometro in Android (in realta' i dati sono pitch e roll ma derivano dall'accelerometro)

Per il calcolo e' stata utilizzata la libreria JKalman.  Dal pacchetto zip si estrae il file .jar e dalla cartella dist e lo si copia nella cartella /libs del progetto Android. A questo punto si clicca destro sul file .jar in Android Studio e si clicca "Add as Library"

Si aggiunge la classe ripresa da qui

------------------------------------------------------------------

package innocenti.luca.com.camerakit;

import jkalman.JKalman;
import jama.Matrix;

public class KalmanFilter {

    private int variables;
    private JKalman kalman;
    private Matrix s; // state [x, y, dx, dy, dxy]    private Matrix c; // corrected state [x, y, dx, dy, dxy]    private Matrix m; // measurement [x]
    /*     * Inicializa el filtro kalman con 2 variables     */    public void initialize2() throws Exception{
        double dx, dy;

        if(variables != 0){
            throw new RuntimeException();
        }
        variables = 2;
        kalman = new JKalman(4, 2);

        // constant velocity        dx = 0.2;
        dy = 0.2;

        s = new Matrix(4, 1); // state [x, y, dx, dy, dxy]        c = new Matrix(4, 1); // corrected state [x, y, dx, dy, dxy]
        m = new Matrix(2, 1); // measurement [x]        m.set(0, 0, 0);
        m.set(1, 0, 0);

        // transitions for x, y, dx, dy        double[][] tr = { {1, 0, dx, 0},
                {0, 1, 0, dy},
                {0, 0, 1, 0},
                {0, 0, 0, 1} };
        kalman.setTransition_matrix(new Matrix(tr));

        // 1s somewhere?        kalman.setError_cov_post(kalman.getError_cov_post().identity());

    }

    /*     * Aplica Filtro a variables     */    public void push(double x,double y) throws Exception{
        m.set(0, 0, x);
        m.set(1, 0, y);

        c = kalman.Correct(m);
        s = kalman.Predict();
    }

    /*     * obtiene arreglo con datos filtrados.     */    public double[] getKalmanPoint2() throws Exception{
        double[] point = new double[2];
        point[0] = c.get(0,0);
        point[1] = c.get(1,0);
        return point;
    }

    /*     * obtiene arreglo con prediccion de punto.     */    public double[] getPredict2() throws Exception{
        double[] point = new double[2];
        point[0] = s.get(0,0);
        point[1] = s.get(1,0);
        return point;
    }

    /*     * obtiene cantidad de variables del objeto     */    public int getNVariables() throws Exception{
        return this.variables;
    }

}

------------------------------------------------------------------

a questo punto e' sufficiente creare il filtro in OnCreate

kf = new KalmanFilter();

try {
    kf.initialize2();
} catch (Exception e) {
    e.printStackTrace();
}

si popola poi l'array dei dati e si ottiene il valore filtrato

try {
    kf.push(pi,ro);
} catch (Exception e) {
    e.printStackTrace();
}

try {
    double[] test = kf.getKalmanPoint2();
    Log.d("Kalman", Double.toString(Math.toDegrees(test[0]))+ " "+ 

 Double.toString(Math.toDegrees(test[1])) + " " +Math.toDegrees(pi)+ " "+

 Math.toDegrees(ro));
} catch (Exception e) {
    e.printStackTrace();
}