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