uNav AHRS

Status
Not open for further replies.
Interesting. I'm not seeing drift, but I also haven't tried rapid movement yet. Any suggestions for tuning to help constrain it?
 
Brian

Still playing with some settings but I noticed when I changed sensors (onehorse I2c to the emsensr-9250 spi) a issue cropped up with the gryo biases. Gbz has a small variance compared to gby and gbx. gby/gbx also still have a dc offset from zero that seems to contribute to drifting issue in my new tests. Want to play with DLPF a bit more. Here is a screen shot to illustrate:
Capture.PNG
 
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.
 
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.
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.
 
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.

That makes sense to me. I'm modeling the process noise as only gyro covariance. In reality, there are some unmodeled dynamics and other effects (possibly even gyro rate saturation if you really move the sensor hard). Increasing Q should effectively increase the correction when measurement updates occur. I've also been thinking of increasing the initial value of the state covariance, P, to hopefully improve the initial convergence time.
 
Brian,
Some rambling on tuning... This is a bit of a different beast, especially with the use of quaternions, but here's some thoughts...

I've had a relatively simple approach to tuning KF applications over the years. First, R represents measurement noise, so usually setting this is relatively straight-forward and makes sense, in that we know the real noise levels associated with gyros, accels, mags, GPS posn, GPS vel. Second, Q represents the uncertainty in our ability to model the system dynamics. Whoa. OK, so set Q to be realistic, based on informed knowledge, but a bit on the high side.

Now, monitor the filter innovations (y-Hx, or y-h(x) if nonlinear), and place the system (vehicle) under a full range of dynamics. Look at the statistics on the innovations. Is there a bias? What's the standard deviation? Now use Q as a "tuning knob" to attempt to reduce innovation bias and reduce innovation standard deviation. But not too small as the system gets too constrained and locks. Leave some margin in.

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!

Don
 
Hi Don

With your approach aren't you now getting into the realm of an adaptive kalman filter. I've seen some papers/thesis that use a very similar approach as you are describing.

Mike
 
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!

Thanks for the thoughts on tuning! I'll have to investigate that more. I'll also be interested to hear what you and Mike find as you play with the filter. When you're checking margins, is it similar to control laws (i.e. looking for something like a 6 dB gain margin)?

For switching between filters, do you have a good way to do that without introducing a transient? With a nonlinear filter, it seems like you can't initialize it to be transient free during the switch. It's interesting to me because during flight testing, we've occasionally encountered bad GPS data, which corrupted our 15 state EKF and the pilot had a "fun" time getting the aircraft back. It would have been great to revert back to an AHRS or dead-reckoning filter in those cases.

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.
 
Hi Brian
When you're checking margins, is it similar to control laws (i.e. looking for something like a 6 dB gain margin)?
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;
1. After rapid movement with Madgwick the gyros zero was off and I saw a lot of drift. I sent a command to rezero the gyro;s and it settled right down.
2. Initially when I ran the filters with just gyro/accel I got good results but putting in the magnetometer I got some more drift until I put a butterworth filter on the accel and magnetometer and drift went to almost zero long term hours.
3. Want to really measure what we are talking about with drift when there is mo motion. If it is 1 deg over a few hours not sure your going to do much better than that. That would leave what to do after rapid motion. Maybe do a reset of the gryo's after zero motion is detected (level flight would be included I guess.
4. Still not 100% sure about how to measure dt. When the predict/update loop is fast the next update is quite a bit later if you do other things in the loop. A possible test would be to put the filter proper in a thread and do prints periodically/other stuff in the main loop.
5. And yes I just started reading about adaptive filters again.

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.
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 :).

Its late and I am rambling.

Mike
 
Mike,
Yes, it is sort of getting into the realm of adaptive filter. But I really refer to it as building in "smarts," verses making it truly adaptive. But the more smarts built in, the more adaptive it becomes. I've never implemented a filter, even simple one, that didn't have to have some level of smarts needed.

For example, one trick to get a filter to react to dynamic change is to perform a "Q-bump." So if the residuals start increasing above a threshold (or some other trigger is reached, like an accel detected), then a one-time insertion of a larger Q is done that increases P and allows the filter to "open up." This is a great trick that works very well in the right situation.


Brian,
Depending upon how you transition, yes, there will be a transient. But in the example I gave, I'm not starting the secondary (or tertiary) filter from scratch, it's already running. So the transient delta is the bad solution of the first filter jumping to the good (or better) solution of the second. There is a jump, but it's better than loss of lock.

I'll be glad to help dive into trying to get the 7-state (and later 15-state) tuned. Fascinating stuff! I'm going to go back and re-look at your new 7-state code so I can get a better feel for the approach, and then come back and start trying some things.

Don
 
Last edited:
Some interesting observations so far:

1. I can reproduce the drift that Don and Mike are seeing if I "fly" the IMU by hand. I'm using a book clamped to a work table to set the initial alignment with and I can compare the yaw angle by bringing the IMU back to that starting orientation. The "flying" doesn't need to be rough and I am watching a printout to make sure I'm not going above 50 degrees in pitch or roll.
2. If I comment out the measurement update, my pitch and roll angles suffer a little (single digit degrees), but the yaw angle is closer to the starting orientation than when using measurement updates.
3. If I just do an accelerometer measurement update, it corrupts my yaw angle significantly.
4. If I look at the results of M*R*M.transpose(), the heading channel covariance is much much higher than the accelerometer covariance. I think this is from the M matrix that I derived to take magnetometer covariance to heading covariance. If I estimate heading covariance during the two minute initialization, I get 0.001985 whereas M*R*M.transpose() is yeilding around 3.2731180191. The accel covariance is around 0.000349.
5. The root issue seems to be that the accelerometer measurement update is corrupting the heading. The heading covariance is currently way too high so it takes a long time to bring the solution back to the measured heading. Instead of M*R*M.transpose() I tried setting the covariance directly to the three accelerometer covariance values and the heading covariance value. Behavior is much improved but not perfect. The heading is still getting corrupted by the accelerometer measurement update before getting pulled back to the correct value. I can improve performance further by putting a limit on the norm of the accelerometer input (i.e. if the accel norm is above 9.9 don't do a measurement update), but may mean never getting a measurement update in real world conditions. Quaternions are fun, I feel like it would be trivially easy to separate accel for pitch / roll update and magnetometer for heading update with an Euler based filter.
 
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
 
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

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.
 
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

The one thing to watch out for is that the version of the 7 state EKF on github only updates the magetometer in combination with an accelerometer update.
 
I got a $20 M8N uBlox on eBay - free ship from N.J. - got here fast but sold out now. Seems to work, Put a wire on + end of tiny PPS LED so I have that to play with. Has 75 Hz limited i2c prior gen Honeywell HMC5883L MAG onboard - hoping it supports same commands as the IST8310 on Don's 'mRo' unit.

Brian - Don has same M8N - similar to yours - not sure about Mike? What Messages are expected to be Configured? What other common settings?

Using u-connect I set my output to 460800 baud from initial 9600. It supports doubling once more, T_3.6 Serial 1 or 2 can easily do that. Just changed rate to 10 Hz/100 ms and u-connect is keeping up. Faster baud will minimize sync wait for transfer, my start bit detect interrupt should allow knowing when reading is coming, that reading from measurements some uS prior to calc and transmit.

My generic unit doesn't have USB - mounted on T_3.6 and wrote this PROXY sketch to allow using u-connect:
Code:
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() );
}

It shows 9600 on Serial2 for use at default baud - then after u-connect change I recompile for faster for now. This gives me 'man-in-the-middle' ability to store u-connect transmissions - Plan to dump them out another serial port to a second Teensy and display them on a second SerMon for storage and replay as needed to bypass u-connect as the settings are lost with power - even with battery on the unit?

I made the note about my altitude - this unit is showing Alt from 40 to 60 meters MSL and 20 to 40 HAE. The MSL is close as I see sea water out my window - but still +/- 10m - so on average my house is 50 m.

Code:
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)

Sitting in my window sill my GPS readings show moving 3 to 12 inches per second - AVG Speed over Ground is ~5.5 inches/sec.
 
Brian. You talking about doing something like in this paper:
 

Attachments

  • 569.PDF
    346 KB · Views: 250
settings are lost with power - even with battery on the unit?
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.
 
Hi Brian,

A little confused and am probably missing something but at about line 277 you have
Code:
theta = asin(_accel(0,0));
. If theta is pitch, should it be
Code:
theta = atan2(_accel(0,0), sqrt(_accel(1,0)*_accel(1,0) + _accel(2,0)*_accel(2,0)));

EDIT: Reference is freescales AN3461
 
Last edited:
Thanks Brian/Mike. I RTFM'd and found write config files - survived an unplug at 10Hz and 460800 baud. I had done save but not understood that menu option and check box for Flash commit.
Only uBlox sample I tried was from Don and it failed so I'll work with those samples and info.
 
@defragster. One other recommendation. If you use Brian's library turn off all messages except for the PVT message. Don't forget to save the configuration after you do that. Can't really think of anything else.
 
Hi Brian,
Sorry for an added post on going through the code questions. But a policy change on the forum presents you from editing past posts. From what I read they are looking at a potential solution to allow non-spammers to edit past posts. Anyway, I am looking at your quat2Euler function and was wondering where you got this formulation. I have seen it a couple of other ways, for instance in Matlab functions quat2eul it is defined as:
Code:
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);

Which is slightly different than this formalization but close to yours, I think ref: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/:
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

There code looks like this:
Code:
/** 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)
}

This site has another way that I just came acoss: https://math.stackexchange.com/ques...t-bryan-angles-from-quaternion-representation

Not sure who's is right but just throwing it out there. Differences are probably in axis rotations, my guess.
 
Status
Not open for further replies.
Back
Top