Don, that sound like what I am seeing after movement. But I am still seeing drifting - maybe I will give a different sensor breakout board a try tomorrow. Not sure how you would do that without re-doing the sensor calibration.ppears to be slowly moving back towards zero now. Seems to me to imply that the filter needs to be opened up (Q increased) some, at least when there's movement.
Thanks Mike, will check those links out....
Here's something cool... I started up the 7-State, messed around ("flew") with the board for a bit, then went off for dinner for a couple hours. When I came back, heading is dead on (0.2 deg) and when I fly the board around it seems to stay good. Anyway, thought that was cool!
EDIT: Came back and REALLY messed with it and the heading jumped to 122 deg. Appears to be slowly moving back towards zero now. Seems to me to imply that the filter needs to be opened up (Q increased) some, at least when there's movement.
Also, I often have to build in safeguards where I do certain events if certain things happen. Hard to explain, but my last application I actually ran three different filters in parallel and kept one as default, but would switch to another autonomously if the default crashed or performed poorly.
So, anyway, a couple of thoughts. That's one reason I like to set the loop interval to some constant value (like 250Hz) and then tune from there. It makes things much easier as starting point. But I love the way we can make this thing scream with the use of interrupts!
Right now I let the filter run for over an hour both in steady state and after rapid movement (to see it goes back to its approximate starting point). In both cases the drift is still there, while small over time it continues to grow. I am not sure what's really driving it though. Remembering back to my Freeimu early days there were a couple of things I looked at. Think I have to do some experiments to see where it is at, but here are a couple of thoughts;When you're checking margins, is it similar to control laws (i.e. looking for something like a 6 dB gain margin)?
I did some timing tests using the interrupts at different SRDs and it really is rather consistent, a little different when you put prints in the loop though .Using the interrupt from the IMU should trigger the filter at a constant rate. And the nice aspect is that you know new data is ready when you read the IMU. I guess I don't see how it's different from timing the filter from the Teensy.
Brian,
What are your thoughts on trying the approach from the Watson thesis? For his h(x) equation, he does a transformation on the magnetometer measurements that supposedly isolates pitch/roll from yaw. Also, his h(x) is a 6x1, thus yielding an H of 6x7.
But it looks like he might be using ENU coordinates (from his MPU6050 diagram), so that might cause some pain in converting. I'll look at his approach some more...
Don
Hi Brian
Glad your making progress. Been doing some simple tests, besides writing a simple generic function to print Eigen Matrices. One of things I noticed with the 7-state was the drift corresponded to q0/q3 diverging while q1/q2 were steady. I tried to articifically increase the quat variances and it helped in steady state but not enough to really implement. So I did something simple. I went through a series of tests where I force gyroUpdated_, magUpdated_, accelUpdated_ to be true or a combination. The only case that seemed to make a difference was when I set gyroUpdated to true all the time. Quats started behaving and biases came in line with minimal variance and no offset. But still had the issue with jumping in yaw values by a lot. Drifting didn't start for about 40 minutes. After this I was going to start looking at the matrices. Was quite sure what the driver was but based on this was going to look at mag and accel. With gyro and mag set true drifting started after a couple minutes and noticed that the gyro biases were off for gy and gy. Also larger variances.
Mike
void setup() {
Serial.begin(9600);
while (!Serial && millis() < 4000 );
Serial.println("\n"__FILE__" "__DATE__" "__TIME__);
Serial2.begin(460800);
//Serial2.begin(9600);
}
void loop() {
char inChar;
if ( Serial.available() )
Serial2.print( inChar = Serial.read() );
if ( Serial2.available() )
Serial.print( inChar = Serial2.read() );
}
Title Count Age Current Minimum Maximum Average Deviation Unit Description
HDOP 28269 0 1.2 0.6 1.9 0.9 0.1 DOP Horizontal
VDOP 28635 0 1.3 0.8 2.8 1.2 0.2 DOP Vertical
Alt (HAE) 27853 0 42.500 -18.399 55.300 31.848 6.382 m Position LTP Altitude (above ellipsoid)
Alt (MSL) 27853 0 60.800 0.000 73.600 50.148 6.382 m Position LTP Altitude (above mean sea level)
In his thesis, he is keeping the magnetometer from affecting the pitch and roll channels. The issue is that the accelerometer is affecting the yaw channel.
Brian - Don has same M8N - similar to yours - not sure about Mike? What Messages are expected to be Configured? What other common settings?
They shouldn't be lost as long as the battery is working on the unit. Just remember to flash the settings from the CFG (Configuration) menu. Just saving after you change a setting won't do it. The only time it won't flash is if the battery doesn't or you have a knockoff, they have trouble saving I found.settings are lost with power - even with battery on the unit?
theta = asin(_accel(0,0));
theta = atan2(_accel(0,0), sqrt(_accel(1,0)*_accel(1,0) + _accel(2,0)*_accel(2,0)));
m11 = 2.*(q1.*q2 + q0.*q3);
m12 = q0.^2 + q1.^2 - q2.^2 - q3.^2;
m21 = -2.*(q1.*q3 - q0.*q2);
m31 = 2.*(q2.*q3 + q0.*q1);
m32 = q0.^2 - q1.^2 - q2.^2 + q3.^2;
bank = atan2(m31,m32);
pitch = asin(m21);
azimuth = atan2(m11,m12);
heading = atan2(2*qy*qw-2*qx*qz , 1 - 2*qy2 - 2*qz2)
attitude = asin(2*qx*qy + 2*qz*qw)
bank = atan2(2*qx*qw-2*qy*qz , 1 - 2*qx2 - 2*qz2)
except when qx*qy + qz*qw = 0.5 (north pole)
which gives:
heading = 2 * atan2(x,w)
bank = 0
and when qx*qy + qz*qw = -0.5 (south pole)
which gives:
heading = -2 * atan2(x,w)
bank = 0
/** assumes q1 is a normalised quaternion */
public void set(Quat4d q1) {
double test = q1.x*q1.y + q1.z*q1.w;
if (test > 0.499) { // singularity at north pole
heading = 2 * atan2(q1.x,q1.w);
attitude = Math.PI/2;
bank = 0;
return;
}
if (test < -0.499) { // singularity at south pole
heading = -2 * atan2(q1.x,q1.w);
attitude = - Math.PI/2;
bank = 0;
return;
}
double sqx = q1.x*q1.x;
double sqy = q1.y*q1.y;
double sqz = q1.z*q1.z;
heading = atan2(2*q1.y*q1.w-2*q1.x*q1.z , 1 - 2*sqy - 2*sqz);
attitude = asin(2*test);
bank = atan2(2*q1.x*q1.w-2*q1.y*q1.z , 1 - 2*sqx - 2*sqz)
}