yes, here's the code:
// state transition matrix
Eigen::Matrix<float,7,7> Fsys = Eigen::Matrix<float,7,7>::Identity();
In the sketch, I'm using these values:
Filter.setAccelCovariance(0.025f); // also try 0.025f
Filter.setHeadingCovariance(0.125f); // also try 0.025f
Filter.setMagSrd(12);
// Set 41Hz DLPF bandwidth
//Imu.setDlpfBandwidth(MPU9250:
LPF_BANDWIDTH_41HZ); // Also try 20Hz
Imu.setDlpfBandwidth(MPU9250:
LPF_BANDWIDTH_20HZ); // Also try 20Hz
// setting SRD to 9 for a 100 Hz update rate
//Imu.setSrd(9);
Imu.setSrd(12);
I tried another experiment. I set my iPhone compass to "True North off" (i.e., magnetic compass not corrected to true North), and aligned the x-axis of MPU9250 to North on iPhone as best I could. It's reading Heading between 359 and 1 degree, so seems to be doing very very well at the moment.
I still can get the whole thing to crash (all "NaN's" to the serial port) once in a while if I "fly" the board around pretty wildly, but not unlike what a drone might do. So there's still robustness issues. And I'm not absolutely sure the Fsys set to Identity is correct, but it seems like it should be (from Phi = I + Fsys * dt), but would need to derive the dynamics to be sure.