NeoGPS Location_t affecting how the rest of the program works

[Mad_Monkey_Project]

I am using an Arduino Due, with a SparkFun BNO080 IMU, and a ported version of the Eigen library. My goal was to create a 2 dimensional Kalman Filter for the IMU compass for the purpose of creating a larger project that involved GPS. The program immediately converges with the raw compass data as expected.

My issue is that when I include NEOGPS::Location_t for the array of waypoints in a global scope, the filter takes several minutes to converge with raw compass data. If I comment out the namespace constructor the program again works as expected. Thoughts?

#include <Eigen.h>
#include <Eigen/Dense>
#include <NMEAGPS.h>
#include <GPSport.h>
#include <Wire.h>
#include "KalmanFilter.h"
#include "SparkFun_BNO080_Arduino_Library.h"
#include "Compass.h"

using namespace Eigen;

double filteredCompassData();

BNO080 IMU;
Compass compass;

/******************************
*       GPS information       *
******************************/
static NMEAGPS gps; // this parses the GPS characters
static gps_fix fix; // this holds the latest GPS fix

#define gpsPort Serial1
#define GPS_PORT_NAME "Serial1"

//NeoGPS::Location_t WAYPOINTS[] =
//{       /*   LAT           LON   */
//    {44429764L, -110584663L}
//};

unsigned long currentTime{};
unsigned long lastTime{};
unsigned long delayTime {100};

void setup() {
  Serial.begin(115200);
  Wire.begin();
  IMU.begin();
  
  Wire.setClock(400000); //Increase I2C data rate to 400kHz
  
  IMU.enableMagnetometer(50);  //Send data update every 50ms
  IMU.calibrateMagnetometer(); // enable dynamic calibration for magnetometer
}

void loop() {
  currentTime = millis();
  
  if (currentTime - lastTime >= delayTime) {
    lastTime = currentTime;
    filteredCompassData();
  }
}


double filteredCompassData() {
  uint8_t n = 4; // Number of states
  uint8_t m = 2; // Number of measurments

  double dt = 0.1; // time step 100 milliseconds

  MatrixXd A(n, n); // system dynamics matrix
  MatrixXd C(m, m); // Output matrix
  MatrixXd Q(n, n); // Process noise covariance
  MatrixXd R(n, n); // Measurement noise covariance
  MatrixXd P(n, n); // Estimate error covariance

  // Measuring position only
  A << 1, 0, dt, 0, 
       0, 1, 0, dt,
       0, 0, 1,  0,
       0, 0, 0,  1;
       
  C << 1, 0, 
       0, 1;

  // Reasonable covariance matrices
  Q << 0.05, 0, 0, 0, 
       0, 0.05, 0, 0,
       0, 0, 0.05, 0,
       0, 0, 0, 0.05;
       
  R << 150, 0, 0, 0,
       0, 150, 0, 0,
       0, 0, 150, 0,
       0, 0, 0, 150;
  
  P << 25, 0, 0, 0,
       0, 25, 0, 0,
       0, 0, 25, 0,
       0, 0, 0, 25;

  // Construct the filter
  KalmanFilter kf(dt, A, C, Q, R, P);

  // Look for reports from the IMU
  if (IMU.dataAvailable() == true) { 
    // magnetometer 
    float mX = IMU.getMagX();
    float mY = IMU.getMagY();

    // List of noisy position measurements from sensor
    VectorXd measurements(m);
    measurements << mX, mY;

    // Initialize filter
    kf.init();

    // Feed measurements into filter, output estimated states
    kf.update(measurements);
  
    Serial.print(compass.getCompassHeading(mX, mY));
    Serial.print(" ");
    Serial.println(compass.getCompassHeading(kf.state()[0], kf.state()[1]));
  }
  return 0;
}