Kalman Filter and GPS D:

Using Arduino Project Guidance
#1

Hello :),

I’ve got a 10hz GPS module successfully logging out data to a file on a microsd and I would love to get some smoothing going. I’m having difficulty finding some kalman filter stuff for GPS within arduino specifically. I’m happy for you to correct me here and post a load of them :P.

Anyway, i discovered this yesterday GitHub - lacker/ikalman: An iPhone-friendly Kalman filter written in C. and after making a few changes to my preference such as wrapping the actual logic into an object and fixing up the for loops to work in or out of C99 mode (i forget which way round that is :P) i’ve got it successfully compiling onto an Uno. My changes can be found here: https://github.com/rfox90/ikalman its highly likely i may of made a silly mistake there I’m not exactly proficient at C++ and especially not code I don’t understand due to the maths behind it.

The problem appears to be that I run out of RAM on the Arduino as its restarting. This occurs the moment i call an update to the filter.

Am I heading in the right direction or just condemning myself to run out of RAM?

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

#include <PString.h>
#define RXPIN 2
#define TXPIN 3

//Defined in the specs for the module i am using
#define GPSBAUD 38400

#define error(s) sd.errorHalt_P(PSTR(s))
TinyGPS gps;


SoftwareSerial uart_gps(RXPIN, TXPIN);

GPSFilter f(2.5);

int freeRam();

char buff[50];
char pbuff[100];
PString line(pbuff,sizeof pbuff);

unsigned long lastUpdate;
unsigned long lastSentence;
float c;



void setup()
{
  Serial.begin(115200);
  Serial.println(F("...waiting for lock..."));
  uart_gps.begin(GPSBAUD);
  uart_gps.write("$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n");
  uart_gps.write("$PMTK220,100*2F\r\n");
  lastSentence = millis();
  lastUpdate = millis();
}


void loop()
{
  while(uart_gps.available()) {     // While there is data on RX 
    char c = uart_gps.read();
    if(c == '

){
   //  Serial.println();
   }
  // Serial.print(c);
   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;
     //seconds since last update
     f.update_velocity2d(ulat,ulong,c);
     lastSentence = millis();
     Serial.println(F(“Does not get here”);
     if((millis() - lastUpdate) >= 500 || lastUpdate == NULL){
     f.get_lat_long(&dlat,&dlong);

line.begin();
     line.print(“201,”);
     line.print(dlat,8);
     line.print(",");
     line.print(dlong,8);
     line.print(",");
     line.print(year);
     line.print("-");
     line.print(month);
     line.print("-");
     line.print(day);
     line.print(" “);
     line.print(hour);
     line.print(”:");
     line.print(minute);
     line.print(":");
     //Serial.println(second);
     line.print(second);
     line.print(".");
     line.print(hundredths);
     line.print(",");
     line.print(gps.f_altitude(),2);
     line.print(",");
     line.print(gps.f_course(),2);
     line.print(",");
     line.print(gps.f_speed_kmph(),2);
     Serial.println(line);
     lastUpdate = millis();
     }
   }
 }
}
int freeRam () {
 extern int __heap_start, *__brkval;
 int v;
 return (int) &v - (__brkval == 0 ? (int) &__heap_start : (int) __brkval);
}

#2

You can get rid of the 100 byte PString. You only use it in one place to write to the serial port, and you can simply write the separate parts of that message directly to the port.

Can you use F() and PROGMEM for the strings you're writing to the GPS?

If you are still tight on memory I'd find out how much memory the GPSFilter and TinyGPS classes are using. Nothing else you're doing looks as if it should be using a lot of RAM.

#3

The only reason I had PString was its nice ability to print floats out of the box. Ideally getting sprintf to accept floats would be awesome but I’m aware this is complex. How much program memory overhead does this cause?

Alternatively I’ve found

static void print_float(float val, float invalid, int len, int prec)
{
  char sz[32];
  if (val == invalid)
  {
    strcpy(sz, "*******");
    sz[len] = 0;
        if (len > 0) 
          sz[len-1] = ' ';
    for (int i=7; i<len; ++i)
        sz[i] = ' ';
    Serial.print(sz);
  }
  else
  {
    Serial.print(val, prec);
    int vi = abs((int)val);
    int flen = prec + (val < 0.0 ? 2 : 1);
    flen += vi >= 1000 ? 4 : vi >= 100 ? 3 : vi >= 10 ? 2 : 1;
    for (int i=flen; i<len; ++i)
      Serial.print(" ");
  }
  feedgps();
}

From the TinyGPS examples which will suffice With some tweaking. Unless there is a better way you know of.

I’ll report back with new memory figures if required :slight_smile:

Thanks for confirming I’m not insane.

#4

The only reason I had PString was its nice ability to print floats out of the box.

Serial is an instance of the HardwareSerial class, which (indirectly) derives from the Print class which knows how to print floats.

#5

In a larger sketch im using the similar pstring block to print to a file Which is not HardwareSerial related. I think that was the original reason :( EDIT: I'm a retard i switched to doubles for the gps coords. Should be fine i'll let you know :)

RAWWRR, still using floats for altitude course and speed D:

So i had multiple mistakes in my above sketch. Namely I used wrong variable types everywhere(Go look at my useage of "c").

Also its a lot easier to hardcode the seconds_since_last_timestamp in update_velocity2d(double lat, double lon, double seconds_since_last_timestep) to the module update rate. In my case 0.1.

I've now got sample output to Serial and i've abolished my use of PString. I'll rewrite my file usage to use the File class and It should all be fixed float-wise. Once it works ill update the repo i made with an example sketch.

I've yet to see if there's actually any benefit of using this filter :).

#6