uNav AHRS

Status
Not open for further replies.
@defragster. One other recommendation. If you use Brian's library turn off all messages except for the PVT message. ...

Thanks Mike- that highlights Brian's p#170 answer and link to MSG: UBX NAV-PVT on the github readme. Explains why the sample would not run before as it does now. I see 5 Hz MSG - even though 100 ms requested - set to 200 ms and get the same 5 Hz.

After soldering to the PPS LED on my unit - scared not seeing BLUE Tx LED across the room - that is because 460,800 Tx is so much quicker than 9,600 with a single 100 byte message in about 2.17 ms. With that working I'll get back to tracking CPU cycles on events to see if it can help down the road.

If everyone has a second Teensy - Proxy code like above would allow debug prints to display out one port [ and allow run time input to code ] while TViewer (or u-console) connected on a second port [ USB, Serial# not using GPS ], then using TyCommander both Teensy's could be programmed from GUI and seen on its serial monitor.
 
Hi All,

Finally got to create a matrixPrint that sort of mimics Eigen's IOFormat function. I am attaching the .h file and an example sketch in case you are interested.

Cheers
Mike
 

Attachments

  • matrixPrint.zip
    2.2 KB · Views: 148
For my own edification, I created an Euler based EKF AHRS. It's a two stage filter, the first stage filter works on the pitch and roll angles (2 state EKF) and the second stage filter works on the yaw angle (1 state EKF). Of course, it's blazing fast on the Teensy 3.6 (around 40 us), and I'm no longer getting drift on the yaw channel. The usual Euler angle caveats apply.
 

Attachments

  • uNavEulAHRS.zip
    7.1 KB · Views: 199
Thanks Brian. You are amazing. I was about to give up.

Was trying a bunch of different scenarios since last night until I had to go by kitchen cabinets :( have to install them now. No matter what I tried I couldn't get drift under control while at the same time avoiding jumps in yaw/heading. If yaw was stable gbz was drifting horribly. When it jumped or was drifting gbz was stable around zero, etc. Found 1 paper that was kind of interesting but haven't read it yet.

Can't wait to give it a try.
 
Hi Brian
Did a couple of tests with the new library and am attaching a pdf file for your review. Its a good news bad news situation. Some discontinuities are appearing where there should not be. Not sure it is me, my breakout board or something else.

Mike

PS. If you want the data files I can mail them to you or post them.
 

Attachments

  • uNavEulAHRS Test.pdf
    1.1 MB · Views: 122
Was wondering about CPU free time - figured dropping CPU speed would be easier to call FAIR and get right and it showed value.

24 MHz T_3.6 is enough to do an all in one test Just catching the I/O reading & parsing:
> onehorse MPU9250 on under pins [ i2c Wire.setSCL(47); Wire.setSDA(48); ]
> Interrupts catching IMU at 1KHz :: Performs IMU.readSensor() in the isr() - pin #50
> uBlox M8N 5Hz @460,800 baud
> Parse 5 'UBX NAV-PVT' GPS emissions per second
> Triggering isr() on Serial2 Start bit to time start of the coming GPS data.
> PPS at 1Hz to get a time base [ RTC works as well with more slop ]
> ADDED counter of passes per second through loop()'s for feel of 'free time'

Results: 24 Hz everything is caught and kept up - except intermittent failure to fully parse GPS, unless default yield() is replaced!
>> LTO adds seconds to build time but seems to free up runtime for loop()

Code:
@_24MHz:: loop()'s _20 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, misses a few+ GPS $'s per 10sec

@_24MHz:: loop()'s _44 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s << Fastest, void yield()
@_24MHz:: loop()'s 125 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s << w/ LTO, void yield()

@_48MHz:: loop()'s _63 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s
@_48MHz:: loop()'s 165 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s << w/ LTO
@_48MHz:: loop()'s 402 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s << w/ LTO, void yield()

@180MHz:: [U]loop()'s 1.47 MHz[/U] : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s << w/ LTO, void yield()

@240MHz:: loop()'s 309 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s  << FAST
@240MHz:: loop()'s 608 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s  << FAST, void yield()
@240MHz:: loop()'s 926 KHz : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s  << Fastest,  void yield() :: F_BUS120 MHz
[U]@240MHz:: [B]loop()'s 2.40 MHz[/B] : 1001 IMU int()/sec, and 5 GPS Rx start, All GPS $'s  << Fastest w/LTO, void yield() :: F_BUS120 MHz[/U]
Note: 24 and 48 MHz compile and run FASTEST, but 240 MHz IMU errors - noted above compiled FAST
>> Error -5 - bad WhoAmI(): Doesn't happen 180 MHz w/LTO. Added delay() in .begin did not find a fix.
FIX: EDIT TD Sources ~line 765 in T:\arduino_b1.9_B34_td141\hardware\teensy\avr\cores\teensy3\kinetis.h
Code:
#if (F_CPU == 240000000)
 #define F_PLL 240000000
 #ifndef F_BUS
[COLOR="#FF0000"] //#define F_BUS 60000000[/COLOR] // COMMENT THIS LINE
 //#define F_BUS 80000000   // uncomment these to try peripheral overclocking
[B] #define F_BUS 120000000  // all the usual overclocking caveats apply...[/B]  [COLOR="#FF0000"]// UNCOMMENT THIS LINE[/COLOR]

** :: void yield() {}; // Void yield() to bypass default PJRC yield overhead - Add this to user sketch .INO
>> PJRC yield() just includes a walk across all (6) UARTS for calls to serialEvent() - this overhead breaks 24 MHz GPS parsing

Sample output 1 second at 240 MHz and Fastest w/LTO, void yield() :: w/F_BUS 120 MHz shows:
> count of IMU isr()'s, running PPS ticks, GPS Rx start bits caught and GPS $'s parsed, loops cycled
> "X" is rounded count of 100 IMU isr() readSensor()'s, interwoven with parsed GPS string output
IMU Read #1001 PPS Ticks 783 GPS Rx= 5 Parsed= 5 loops= 2405940
X 1/22 @ 7:58:16 5 36.55
XX 1/22 @ 7:58:16 5 36.78
XX 1/22 @ 7:58:16 5 36.79
XX 1/22 @ 7:58:16 5 36.85
XX 1/22 @ 7:58:16 5 36.90
XX

@Brian - I see uBlox sets the Teensy baud rate - but doesn't call out to the device which is where I was at. [ it can't without prior baud and of course saving to FLASH fixes this ]

<edit>: Code not included - Was just smashed together to test to see how low it could go, and code includes PPS that nobody else likely has. Has no added value except minor operational details perhaps. I will #ifdef it to default to RTC PPS. Also note IMU similarly fails to run at 216 MHz - but I did not bother looking to solve that as 240 MHz is stable on all my units when F_BUS is bumped to 120 MHz. Also - I really made debug spew sparse and I thought that was killing 24 MHz before I thought to replace the default 'weak' yield() - with the GPS active that 6 UART scan was a killer to find minimal needed speed. I could try at 16 MHz - but then I'd have to work without USB, which would be a good excuse for second PROXY debug teensy.

I placed on this thread (though the findings really hits the UBlox and MPU9250) because 'everyone' is here - and this thread/code can use all the spare horsepower it can get.

Also: 24 MHz FAST without LTO only parses about 10 of 50 GPS messages in 10 seconds, even FAST with LTO misses many of those 50 - even with a high 98 KHz loop count. So 24 MHz is on the edge without proper optimization selected. I'm not sure why uBlox fails parsing - the code looked nice and clean, but something can kill it some seconds while running loop() that fast means it should be able to catch all the chars since it only get 500/sec? Hopefully this won't show later with more intensive filters active.
 
Last edited:
Update: GPS Parse works at 24 MHz ( fix below) - without LTO as FAST or SMALLEST code - even with a delayMicroseconds(50); in loop() dropping loop cycles down to 4100 Hz - though IMU reads drop to 996.

Problem was default 64 byte Serial Rx buffer overflowing at 460,800 baud with 100 byte message. For Serial# in use an edit like "#define SERIAL2_RX_BUFFER_SIZE 124" in 'T:\arduino_b1.9_B34_td141\hardware\teensy\avr\cores\teensy3\serial2.c' worked here on Serial2 even with delayMicroseconds(150); slowing loop() to 1400 Hz - and dropping. The isr() on the Serial Rx start bit will still have a proper timestamp.

Problem was found as was to be expected from above >> "Interrupts catching IMU at 1KHz :: Performs IMU.readSensor() in the isr() - pin #50"
Do not do this! (known repeat of note to self) i.e. call a complex function from an isr() - even for ( perhaps especially ) when doing a quick test for a baseline.
A flag was being set in the isr() IF readSensor() was no error - now just sets flag [ records timestamp ] and returns for the loop to perform the read.

With LTO and readSensor in loop() 998 to 1000 IMU data reads are recorded in any second at F_CPU=24 MHz, without LTO it was 996-997.

At 240 MHz With the read in loop() IMU reading full speed ( and larger Serial2 Rx buffer) the loop() cycles a bit faster than 2.4MHz showing 2,522,503, all GPS parsed and 1001 IMU reads.
 
defragster - WOW. That's all I can say. By the way have you tried using TeensyThreads. Don't know what impact this would be?

EDIT: You can send commands to the m8n. I've used something like this in the past - mySerial_GPS.println(F("$PUBX,40,RMC,0,0,0,0*47")). You just need to know the commands from the Ublox manual and always remember a couple of things: 1) reset the Serial baud to new ublox baud. These are good if the flash does not work.

Mike
 
Last edited:
Brian,
Got your Euler EKF up and running this morn. VERY nice work! It seems very stable so far, even when I "fly" around the board. I see one hiccup, probably an "Eulerism" as you mentioned. Roll jumped to 360+ instead of back to 0, so suspect a simple fix with a "mod360" when roll > 360. In other words, a roll of 10 deg is now reading 370 deg.

Looking very nice, impressive!! I'll dig into your cpp file a bit so I can see the approach you're using for the dynamics. Nice!

I had another "duh" moment this morning. I had been moving your cpp and h files into library each time you sent them, and then using notepad++ to go in an edit them. This time I just kept the cpp, h, and ino file in the same folder (without creating a library), and when the ino compiled, the cpp and h files appear as tabs. Duh. I use tabs a lot, but did not know I could add the cpp and h files as tabs. Makes trying things out in the cpp files much easier. Only been doing arduino for a year or so, but hadn't discovered this feature yet...

Don
 
Flew it around much more aggressively, and got it confused. suspect it's more euler singularities crossed. had to restart...

Brian, got an idea to throw out... since this version seems stable at modest "flight", what if we ported this to Matlab (or Python) to use as a benchmark? Then we could compare the quaternion version to the euler version over a single filter iteration or two, with the exact same sensor inputs, and see if we get the exact same response. Anyway, just a thought...
 
Brian,
In your propagation step for yaw, you're using the already-propagated-by-dt values of pitch and roll to propagate yaw by dt. Maybe it's such a small difference that it doesn't matter, but it seems like the pre-propagated values of pitch and roll should be used in the yaw propagation. I guess I'd need to try it both ways to see if it matters...

Don
 
Don't have much time today, but I noticed an issue with my yaw measurement update. The attached code fixes it. The issue had to do with handling the -180 to +180 switchover. Now I unwrap the measured psi and use the unwrapped measurements in the code before wrapping them for output.

Mike, I collected a bunch of stationary and rotation data of the EKF vs the raw measurement so we can talk about the jumps that you're seeing. I'll post it when I get a chance.

Don, a matlab or python port sounds nice. I need to think about the state propagation more, the effect should be small.
 

Attachments

  • uNavEulAHRS.zip
    5.3 KB · Views: 178
I just put in some counters to see how fast the various stages of the Euler EKF were running. The propagation stage kicks off with:

if (gyroUpdated_)

Likewise the accel update stage kicks off with:

if (accelUpdated)

and the mag update stage kicks off with:

if (magUpdated_)

What I'm seeing is that all three are occurring at almost exactly 100 Hz (often with gyro and accel at 101, mag at something like 97). So does this seem right? I suppose I need to go back and look at the MPU0250 settings, but I guess I was expecting the gyro and accel readings to be available much faster (like 1-2 KHz?), with maybe the mag at a much slower rate (maybe 50-200Hz?). So in other words, looks like on my Teensy3.6 we're running the filter at an overall 100Hz rate unless I'm miscalculating something.
 
defragster - WOW. That's all I can say. By the way have you tried using TeensyThreads. Don't know what impact this would be?
...
Mike

It was comforting to see the T_3.6 at 24 MHz was able to handle all the interrupts and data processing needed to feed the Filtering Engines. At 240 MHz that leaves ~216 MHz for USB data/feedback and working with the data. Indeed TeensyThreads could be a good way to assure the time is properly and efficiently shared to prevent starving/breaking the time critical data acquisition.

I've seen TeensyThreads and ran a couple of the samples that seemed to present efficient switching. Indeed it might help - or it might break stuff. The stuff being communications if the interrupted libraries are caught unprepared. Though having the right balance the calculations could just run/rerun 'continuously' on queued IMU/GPS data as desired - if the threading timing pulls off for data transfers as needed and other tasks. Seemed like getting it stable/functional first because ( like my quick test ) it can easily produce mysteries on the edges.


Don - my notes above set the MPU9250 at what I saw as max interrupt rate of 1 KHz. The onehorse code would run continuously - AFAIK pulling new data when present and just re-run an iteration on old data when not but never stop and wait for data that was not ready. It seems like there must be something blocking the loop getting/waiting for data? My above test had all the data in a RAM struct as it arrived - but then did no processing and moved on at 1000 Hz.
 
Tim, my test was very simple. I commented out the Serial.print lines in the ino file, added a simple counter each for the accel, gyro, and mag sections of the code, and then just printed those counters out at 1Hz. So I tried to eliminate/minimize anything like Serial.prints that would slow things down. It seems weird that it's running at (almost) exactly 100Hz, almost like it was set to do so.
 
Don. Did you remember to change the SRD, i.e., Imu.setSRD(9) = 100hz, Imu.setSRD(1) = 500 hz, I think. I have been doing most of my tests at an SRD of 4.
 
Thanks Mike, that's it. For anyone interested, here's what I'm typically seeing with the SRD settings on the Teensy3.6

SRD=9, 101Hz for gyro and accel, 97Hz for mag

SRD=4, 196Hz for gyro and accel, 93Hz for mag

SRD=1, 445Hz for gyro and accel, 88Hz for mag

Don
 
Last edited:
Mike, I collected a bunch of stationary and rotation data of the EKF vs the raw measurement so we can talk about the jumps that you're seeing. I'll post it when I get a chance.

Thanks Brian. I didn't get a chance to dig deeper into it but I know from previous tests the raw data looked - it behaved the way it was suppose to. Getting a little behind today - busy with kitchen updates - rather do this. I'll play around with the new code when I get a chance - just taking a break now.
 
Here's a version of Brian's code that pulls out 18 parameters to serial port for viewing at 5Hz in TelemetryViewer. I also included the layout for TelemtryViewer, along with a screen shot.

Here's a summary of what I'm displaying:
- PRYH
- sqrt of the covariance for PRH
- sqrt of Q for PRH
- sqrt of R for accel XYZ and mag
- innovations (y-h) for PRYH

I commented the places in the cpp or h files where I made a modification.

Also, I noticed that the yaw (not heading though) is bouncing around more with this latest version. Easy to see in TelemetryViewer.
 

Attachments

  • uNavAHRS_MPU9250_EulerV2.zip
    630.9 KB · Views: 195
  • Screenshot (1).jpg
    Screenshot (1).jpg
    151.7 KB · Views: 129
Here's a couple of (very!) early observations from looking at the TelemetryViewer plots:

- Two of the Q values (LQL') jump up by a factor of two with pitch movements. Q stays pretty much steady with roll and yaw movements.
- The mag innovation (y-h) was steady at -2 deg mean, with the other innovations (accel XYZ) at near 0. I applied some pitch, the Q jumped, and the mag innovation re-settled back to zero mean with the other three.
- The sigma on Yaw (~2deg sigma?) is much larger than Heading
- The four R's are showing 4 distinct levels. But the accel innovations are showing zero mean with very low sigma, and the mag innovation is showing zero mean with maybe 10x sigma vs accel. Makes me think we might try setting the accel R's to similar low values, with mag R maybe much higher.

Anyway, is neat seeing the stats print to screen real-time!
 
Don. Very Cool! Can't wait to give it a try. Been Tviewer for most of my testing. Comes in real hand to see impact of various parameters. Did you happen to notice that with Brian's new code yaw and heading are the values. Think I know where to look but I am taking a quick break.
 
Worked for me! including the Tviewer config.

Needed to allow for these edits to my hardware:
Code:
#define IMU_ADDR 0x69 // 0x68
#define IMU_SCL 47 // 0x255
#define IMU_SDA 48 // 0x255
#define IMU_INT_PIN 50 // 1

and in parts:
Code:
MPU9250 Imu(Wire,IMU_ADDR);

setup() {
// ...
  if ( 255 != IMU_SCL ) Wire.setSCL(IMU_SCL);
  if ( 255 != IMU_SDA ) Wire.setSDA(IMU_SDA);
  status = Imu.begin();
// ...
  // attaching the interrupt to microcontroller pin IMU_INT_PIN
  pinMode(IMU_INT_PIN,INPUT);
  attachInterrupt(IMU_INT_PIN,runFilter,RISING);

Note: using pin 1 for IMU int prevents use of Serial1 (on 0,1). Freeing that up would allow debug output through a proxy Teensy independent of Tviewer on USB Serial, and GPS 2nd Serial when integrated.
 
Just noticed that in Brian's latest version of the code, he commented out the "-_inertialEuler(2,0)" part of the yaw calculation. So in that version yaw (yh_(0,0)) is really reading out the raw (or noisier) heading. So if you want yaw read out, need to subtract inertialEuler(2,0) from yh_(0,0) before passing it back to be sent to serial port.

In other words, the Kalman is estimating the heading as xh_(0,0). yh_ is the raw heading as calculated from the mag readings. So if you really want the yaw (i.e., the initial heading direction when MPU was initialized), you need to subtract _inertialEuler(2,0).

Mike, great point on Pin1, I need to get that moved somewhere else when I get a chance!
 
Just noticed that in Brian's latest version of the code, he commented out the "-_inertialEuler(2,0)" part of the yaw calculation. So in that version yaw (yh_(0,0)) is really reading out the raw (or noisier) heading. So if you want yaw read out, need to subtract inertialEuler(2,0) from yh_(0,0) before passing it back to be sent to serial port.

In other words, the Kalman is estimating the heading as xh_(0,0). yh_ is the raw heading as calculated from the mag readings. So if you really want the yaw (i.e., the initial heading direction when MPU was initialized), you need to subtract _inertialEuler(2,0).

Mike, great point on Pin1, I need to get that moved somewhere else when I get a chance!

Yeah, oops! In my haste yesterday, I accidentally left some of my test code in. I was using this to see how well the EKF was tracking the raw yaw measurements. The commented out portion of code actually computes the yaw angle from the EKF.

Attached, you'll find a couple plots of stationary data. You'll see that the EKF is generally just filtering the noisy raw measurements as expected.

EKF test1.jpg

EKF test 2.jpg

And here you'll see some tests with movement where the EKF is tracking the changing raw value.

EKF Test Rotation.jpg

Rotation.jpg
 
Status
Not open for further replies.
Back
Top