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();
   }
 }
}