uNav AHRS

Status
Not open for further replies.
Hi Don, nice work. Just ran it and saw things changing. Unfortunately I ran v1 and then only updated the kfINS6State tab. Everything is working but I did make a couple of changes. I changed the lat/long to 10,6 in the serialprintout tab (only saw changes if I did that). Noticed the covariance stays constant at 1.87... but all other values changing. I did make one other change and made a v3 version. Tim and I went to using a common tab for config defines so when we go from version to version all we have to change 1 define statement instead of searching the code for changes when they are hard coded. I added one for you define DK. Check the A_ConfigDefines.h tab to make sure that is what you have for your config. Can we agree to the v3 version as your baseline version?

There are a whole lot of other changes we did also to interrupts etc to get the timing right. Don't know how it will impact what you have but as the day progresses I will go through it.

Mike
 

Attachments

  • MPU9250_INS6State_V3.zip
    35.2 KB · Views: 82
Sounds great, V3 is baseline! I'll take a look at it in a couple hours.

The covariance will stay the same once it settles. that's (1.87 m) is what I get too. Once we add more sophistication (use a dt-based Q vs. my temporary fixed-value Q, for example) it will move around a bit.

Looking forward to hearing about the interrupt changes you all have been discussing!

Don
 
Hi Don-Tim

Attached is version 4 to your ins6state sketch. I merged version 11 with your version 3 so they should work. Please take a look and let me know if its working ok. Just a quick question, for the update function. Right you have:
Code:
      // KF propagate, update, and reset
      kfProp();
      kfUpdate();
      kfReset();
If we wanted to put the kfUpdate into the GPS loop it would be something like
Code:
      // KF propagate, update, and reset
      kfProp();
      if(GPS UPDATE)  kfUpdate();
      kfReset();

Thanks
Mike
 

Attachments

  • MPU9250_INS6State_V4.zip
    37.8 KB · Views: 90
Mike,
Yes that's right. The kfUpdate would only execute if new GPS measurements (LLA, vel NED) are available. kfProp could update with every new accel reading from the IMU or (better yet) at some slower rate (like 20Hz) that a smoothed accel reading is available.

Downloading V4 now, thx!

Don
 
Mike, downloaded and am running V4 now. I see you added a readGPS tab added, and that there are some time parameters added to the main routine.

The serial data looks like it's streaming by pretty fast. Is V4 is still grabbing GPS reads at 5Hz? Do you think it's possible to read GPS at the regular 5 Hz rate, but then concurrently get IMU (AHRS) data faster?
 
Running here.

Don - enjoy the feature of : ...\MPU9250_INS6State_V4\A_ConfigDefines.h

The GPS and IMU data were independent and running at their own rates before? Don't have time to look more now to see if anything changed.
 
in my V3 I put in what I thought was what Brian had in his, which was to read GPS, and if 5+ sats were found, read IMU. But sounds like we need something like

read IMU
if newIMUData, then run kfProp

read GPS
if newGPSData, then run kfUpdate // This should happen at 5 Hz

Think this is what Mike was saying in his last post. I'm headed to happy hour meeting, so maybe I can try it afterwards.
 
Been working through the running. GPS is,nope getIMU was run anytime data was ready. Was working moving the all imu stuff of the print loop and just leave the serialportprints in. Also trying to resolve the final logic for the update loop. Feeling like garbage, head is in a fog so its taking longer to see it, if you know what I mean :(
 
Not sure thread has much added info since we already 'began'- but this post on UBlox-GPS-Module-Primer-for-beginners has a good note about the PPS association with UTC that goes with my notes - without the tie of true PPS is still a floating point in space - and it would take PPS signal to couple the GPS reading to the IMU readings.

Get well soon Mike - I know the fog feeling but have so far been bypassed this flu/cold season.
 
Thanks guys - still working the issues but at least its something to do. I just order a GPS with the PPS signal broken out from EBay - coming from China so probably won't be here for a month. Right now there is an timing issue that we saw before between GPS and IMU. With the added complexity of the KF code not sure its just not better to let it free run and do the timings.

EDIT: HERE IS THE LINK IN CASE YOU ARE INTERESTED, https://www.ebay.com/itm/173037794812
 
Last edited:
I'm thinking Tim is right... really would take PPS (or something similar if it exists) to sync the IMU to get high accuracy timing. Wondering if there are any inexpensive RTC boards that might provide GPS PPS other than using a uBlox m8n. I have several Adafruit Ultimate GPS boards, wonder if I could pull PPS from that... will check...

I can see the PPS pin on my uBlox m8n board, so I may consider trying to solder to it. It's awfully tiny pin though!!
 
Mike: Nice that one only $15 with PPS broken out - as it should be - as TelephoneBill noted in his post - having a PPS signal is very handy for various compute'y type things.

That one must be 'the' NEW variant - I see it from Hong Kong for $13. It does not have onboard MAG unit like the one I got - but that would just be another i2c device to connect and track - not sure if twin MAG's gives a better impression? Mine shipped from NJ,USA and arrived in good time - but they are all gone - and getting PPS is a gamble hitting the end of that resistor without taking it off. I don't see other versions of that in a quick eBay scan.

Don: I'm thinking you are right ... to be thinking I am right :) I had been moving toward Teensy RTC since only I had PPS - but will go back to basing off PPS which is much more stable and locked to GPS data reports - and tied to CPU cycles with PPS_isr() it gives a stable reference point for all local measurements. The CPU Crystal cycle rate varies over time/temp - but looked to track very well with my 8 point moving average that is pretty trivial to update once per second to track any trend. The IMU has it's own clock for making 100 updates per second ( or whatever selected rate ) - but when the INT_isr() fires it provides a delta relative to the reported GPS PPS and it's UTC time used for it's reports.

Don - if you have 30 gauge Wire Wrap wire soldering to uBlox should be do-able. Then be sure to Kapton tape or otherwise (Conformal Coating as glue) it secure it to the PCB to get the signal to the Teensy. I managed mine to that LED with a strand of 24 gauge cat 5 wire, and it isn't pretty - I should have used the smaller gauge.

It would be COOL if some body made a uBlox to TEENSY PCB for us to use. Caught up with onehorse and he has new hardware variations with GPS ( alternate imu too ) - but it uses an embedded Cortex M0+ aimed for lowest power and it has a Teensy_LC class CPU if I read right - he linked me to his Hack-a-Day page. that detailed his latest that may be on Tindie.
 
Tim:

"It does not have onboard MAG unit like the one I got - but that would just be another i2c device to connect and track - not sure if twin MAG's gives a better impression?" - all mine GPS's have a mag on board but never use it. Think its there so you can mount it away from the electronics/motors on the quad/rover. So its not a big deal.

"if you have 30 gauge Wire Wrap wire soldering to uBlox should be do-able" - can you also hot-air it to the resistor instead of soldering? Not sure if you would wind up melting the plastic on the wire unless you kapton it down first.

Oh, by the way, driving me nuts on the non-threaded version so I decided to put it on the side and see if I could get it run on the threaded version. Sure enough it ran - not sure its right though but gave me some insight into what has to get done on the non-threaded version - I think. But I think I will work on that tomorrow. If you have nothing better to do here is what I have:
 

Attachments

  • MPU9250_FiltersGPS_V5_threaded.zip
    38.4 KB · Views: 93
Mike - what issues are you seeing that is driving you nuts? Slowdowns from output again? Or lag from the filters running too long?

Is there any way the filters could be made interruptible? Where they could process a finite part or portion exit and return where they left off? That is what TThreads provides.

Will look at V5_Threaded ...

Indeed 30 ga heats in solder fast and can burn quickly depending on coating - but tape or the coating can keep it safe.

Seemed Brian was looking for uBlox to have MAG data with it - was surprised it was separate i2c bus and not integrated when I got mine. Wasn't sure if it added value and would get used.
 
Brian...

Curious to hear your thoughts on the "PPS for timing" topic. Do you have any ideas on how to get accurate IMU timing other than syncing to PPS?
 
Tim,
Looks like my Adafruit Ultimate GPS has a PPS pinout, so a bit of a pain, but I can add the Ultimate to my board (i.e., run 2 GPSs) and use the PPS from the Ultimate for timing. Ultimately (ha-ha) I'll need to switch to a m8n or m8p that has PPS onboard tho.
Don
 
Morning Tim. As for your questions here goes:
"what issues are you seeing that is driving you nuts? Slowdowns from output again? Or lag from the filters running too long? " - yes to all the above but I did some more tweaking and running a little better but still same slowdowns and skipping GPS updates.

"Is there any way the filters could be made interruptible? Where they could process a finite part or portion exit and return where they left off? That is what TThreads provides." - not that I can think of. What I did was break it down into logical code blocks using functions where a break could occur if we wanted to do something else with GPS reads. T

Did some timing tests for each segment of calls. This is what I came up with in microseconds:
=readGPS= 52
=rtkUpdat= 58
=getIMU= 737
=getBT= 234
=KFP= 670
=KFU= 2356
=KFR= 6
=refLLA= 507

All adds up about 4600 microseconds, we already know how long it takes to print. I did add a test for when GPS is updated to update the kfUpdate function. That reduced the 2356 down to about 2 or 3 microseconds. But still getting the skips. I am attaching the version I am working with maybe you can see something I am missing.
 

Attachments

  • MPU9250_INS6State_V4.zip
    38.5 KB · Views: 71
Tim, Mike,

I'm planning to make two mods to the KF tab today
1) Add 3 acceleration states to the Kalman (will help with noisy accel inputs, but will add a small bit to processing time)
2) Adjust calculation of Q matrix to put most/all noise in the accel states and to allow for variable dt

If I see an improvement, then perhaps we'll bring the new KF tab into the current version.

Also, here's a nice reference on Kalman filters that I like. The author also has a Python KF library that he provides as well:

https://drive.google.com/file/d/0By_SW19c1BfhSVFzNHc0SjduNzg/view

I'm planning to release a complete PDF version of the Kalman filtering short-course I used to teach, maybe on LinkedIn, so will let y'all know when I get around to it...
 
Mike,

I've got the 9State done and the qMatrix model modified. I edited globals.h, modified kfINS6State into kfINS9State, and added a new tab called kfProcessNoise. Seems to be working well.

Should I release another version (V5) or are you working on a V5? If you are, I'll wait for you to post then add these new routines in.

Don
 
Hi Don.

No go ahead and release it as version 5. I'm working on a new version right now. Ran out of ideas on how to skipped GPS updates. Seems to work ok with threaded since the GPS updates work at its own speed. Was hoping Tim could do his magic and have something under his hat.

Mike
 
View attachment MPU9250_INS9State_V5.zip

Here's the 9-state. A quick summary of new things for Ver5
- added three accel states to KF
- added three models for calculating Q, so user can select which model
- added utcTime to serial port printout
- did some minor adjustments to R and Q noise levels so output looks more reasonable. These would need to be tuned later on for any "final" operating environment

I just added utcTime to the serial port printing, and noticed that it's printing 5 times for each 5Hz time-stamp. I need to look and see why, but calling it a night for now.

Don
 
Pulled V5 - will see what I can see. Didn't look at Thread_V ...

Getting 5 Hz GPS mostly - some 3's and 4's just now . . .

I see from the V5 start:
>> GPS updates are being missed - missing one may corrupt the next one - didn't look at how bad it was before hacking . . .
>> " // Imu.readSensor(); //BUGBUG DUPLICATE" called before 'getIMU();' that calls that too.
>> readGPS() was back to and else case of :: rtk.upDated = false; that would hide GPS update
>> no void yield() - may make yield() call readGPS() if that helps run any other task as well.

Looking at:
>> calling the debugGPS() from readGPS() - so it shows at any time debug is on no matter the SerialPort output type.
>> Need to call readGPS() more often - some of these code blocks are buried way too long
>> Making a Dtime() that will start and show 1 of 10 'static' debug timers - so adding two lines - and no local variable garbage - will allow tracking displaying micros() between points

Anyone up to make some PCB's : Bare OSH PCB OSH_uBlox.png

onehorse has a GPS Add-On - has a UBLOX's CAM M8Q - similar to Neo 8- but has a temp compensated Crystal - and won't do logging. Comes with a passive antenna he gets good results with. It is $50 and I think that is without the MPU950 mounted on the board for another $35?
 
My T_3.6 is running at 168 MHz from last test. I put the Dtime() around groups of code and it shows the hot spots - below in uS. I'll play a bit more and post before signing off.

This is with a few readGPS(); added between these to pick up more often and still getting misses. Just bumping to 240 MHz to look for change.

Here is a group of output showing BULK times on the first use of Dtime() throughout loop() when GPS updates were missed:
Code:
-----		 @6:32.6 >> 1==	 ms_0
getIMU uS=738
getIMU uS=734
E°_YPR uS=29
b2i uS=91
kfPUR uS=5262
ecef2lla uS=285
getIMU uS=742
getIMU uS=735
getIMU uS=732
E°_YPR uS=31
b2i uS=91
kfPUR uS=5263
ecef2lla uS=285
getIMU uS=738
getIMU uS=734
getIMU uS=733
E°_YPR uS=30
b2i uS=91
kfPUR uS=5263
ecef2lla uS=287
getIMU uS=740
getIMU uS=735
getIMU uS=732
E°_YPR uS=30
b2i uS=91
kfPUR uS=5262
ecef2lla uS=285
getIMU uS=741
getIMU uS=734
getIMU uS=735
E°_YPR uS=30
b2i uS=91
kfPUR uS=5264
ecef2lla uS=287
getIMU uS=742
getIMU uS=734
getIMU uS=733
E°_YPR uS=30
b2i uS=91
kfPUR uS=5265
ecef2lla uS=284
getIMU uS=739
getIMU uS=734
getIMU uS=733
E°_YPR uS=30
b2i uS=90
kfPUR uS=5297
ecef2lla uS=284
getIMU uS=742
getIMU uS=736
getIMU uS=733
getIMU uS=733
getIMU uS=735
getIMU uS=733
getIMU uS=732
getIMU uS=734
getIMU uS=733
getIMU uS=733
getIMU uS=733
getIMU uS=732
getIMU uS=733
getIMU uS=732
getIMU uS=732
getIMU uS=732
getIMU uS=733
getIMU uS=733
getIMU uS=734
getIMU uS=734
getIMU uS=734
getIMU uS=734
getIMU uS=732
getIMU uS=732
getIMU uS=733
getIMU uS=733
getIMU uS=734
getIMU uS=733
getIMU uS=734
getIMU uS=734
getIMU uS=733
getIMU uS=732
getIMU uS=733
getIMU uS=732
getIMU uS=733
getIMU uS=733
getIMU uS=733
getIMU uS=732
getIMU uS=732
getIMU uS=733
		 @6:32.6 >> 2==	 ms_598

I added a case to Dtime() to ignore times under 700 uS - and split up the timer for kfPUR uS=5297 into P and U and R - the P and U are still long! - when the GPS fails the IMU times show uninterrupted.

This shows a series of GPS working sequentially and then when it reports late with some misses:
Code:
		 @7:5.51 >> 5==	 ms_799
getIMU uS=738
kfP__ uS=2768
kf_U_ uS=2495
getIMU uS=741
getIMU uS=736
getIMU uS=735
kfP__ uS=2768
kf_U_ uS=2492
getIMU uS=740
getIMU uS=734
getIMU uS=732
kfP__ uS=2766
kf_U_ uS=2493
getIMU uS=741
getIMU uS=736
getIMU uS=733
kfP__ uS=2766
kf_U_ uS=2493
getIMU uS=741
getIMU uS=736
getIMU uS=733
kfP__ uS=2764
kf_U_ uS=2493
getIMU uS=740
getIMU uS=735
getIMU uS=732
kfP__ uS=2764
kf_U_ uS=2495
getIMU uS=741
getIMU uS=734
getIMU uS=731
kfP__ uS=2764
kf_U_ uS=2492

-----		 @7:5.52 >> 1==	 ms_0
getIMU uS=741
getIMU uS=736
getIMU uS=732
kfP__ uS=2766
kf_U_ uS=2493
getIMU uS=741
getIMU uS=735
getIMU uS=733
kfP__ uS=2767
kf_U_ uS=2493
getIMU uS=742
getIMU uS=735
getIMU uS=734
kfP__ uS=2765
kf_U_ uS=2493
getIMU uS=741
getIMU uS=735
getIMU uS=733
kfP__ uS=2766
kf_U_ uS=2494
getIMU uS=743
getIMU uS=735
getIMU uS=733
kfP__ uS=2764
kf_U_ uS=2492
getIMU uS=740
getIMU uS=737
getIMU uS=734
kfP__ uS=2765
kf_U_ uS=2493
getIMU uS=742
getIMU uS=735
getIMU uS=736
		 @7:5.52 >> 2==	 ms_201
getIMU uS=737
kfP__ uS=2772
kf_U_ uS=2493
getIMU uS=739
getIMU uS=735
getIMU uS=735
kfP__ uS=2765
kf_U_ uS=2497
getIMU uS=741
getIMU uS=734
getIMU uS=735
kfP__ uS=2766
kf_U_ uS=2495
getIMU uS=742
getIMU uS=734
getIMU uS=733
kfP__ uS=2764
kf_U_ uS=2499
getIMU uS=740
getIMU uS=736
getIMU uS=733
kfP__ uS=2764
kf_U_ uS=2492
getIMU uS=740
getIMU uS=735
getIMU uS=732
kfP__ uS=2764
kf_U_ uS=2492
getIMU uS=741
getIMU uS=735
getIMU uS=734
kfP__ uS=2764
kf_U_ uS=2495
getIMU uS=743
		 @7:5.52 >> 3==	 ms_402
getIMU uS=739
getIMU uS=734
kfP__ uS=2764
kf_U_ uS=2495
getIMU uS=741
getIMU uS=735
getIMU uS=732
kfP__ uS=2765
kf_U_ uS=2498
getIMU uS=741
getIMU uS=736
getIMU uS=734
kfP__ uS=2765
kf_U_ uS=2493
getIMU uS=741
getIMU uS=735
getIMU uS=732
kfP__ uS=2765
kf_U_ uS=2497
getIMU uS=741
getIMU uS=736
getIMU uS=734
kfP__ uS=2764
kf_U_ uS=2496
getIMU uS=741
getIMU uS=734
getIMU uS=733
kfP__ uS=2765
kf_U_ uS=2497
getIMU uS=740
getIMU uS=736
getIMU uS=737
		 @7:5.52 >> 4==	 ms_602
getIMU uS=737
kfP__ uS=2771
kf_U_ uS=2494
getIMU uS=740
getIMU uS=735
getIMU uS=735
kfP__ uS=2768
kf_U_ uS=2496
getIMU uS=741
getIMU uS=735
getIMU uS=735
kfP__ uS=2764
kf_U_ uS=2495
getIMU uS=738
getIMU uS=735
getIMU uS=733
kfP__ uS=2766
kf_U_ uS=2493
getIMU uS=740
getIMU uS=734
getIMU uS=732
kfP__ uS=2774
kf_U_ uS=2494
getIMU uS=741
getIMU uS=737
getIMU uS=733
kfP__ uS=2766
kf_U_ uS=2495
getIMU uS=741
getIMU uS=734
getIMU uS=734
kfP__ uS=2767
kf_U_ uS=2493
getIMU uS=739
		 @7:5.52 >> 5==	 ms_802
getIMU uS=735
getIMU uS=734
kfP__ uS=2760
kf_U_ uS=2493
getIMU uS=741
getIMU uS=734
getIMU uS=733
kfP__ uS=2768
kf_U_ uS=2491
getIMU uS=740
getIMU uS=734
getIMU uS=733
kfP__ uS=2767
kf_U_ uS=2492
getIMU uS=740
getIMU uS=734
getIMU uS=733
kfP__ uS=2767
kf_U_ uS=2492
getIMU uS=740
getIMU uS=736
getIMU uS=733
kfP__ uS=2769
kf_U_ uS=2493
getIMU uS=742
getIMU uS=736
getIMU uS=733
kfP__ uS=2761
kf_U_ uS=2497
getIMU uS=741
getIMU uS=734
getIMU uS=738

-----		 @7:5.53 >> 1==	 ms_0
getIMU uS=736
kfP__ uS=2773
kf_U_ uS=2492
getIMU uS=741
getIMU uS=735
getIMU uS=734
kfP__ uS=2768
kf_U_ uS=2493
getIMU uS=741
getIMU uS=736
getIMU uS=732
kfP__ uS=2766
kf_U_ uS=2495
getIMU uS=740
getIMU uS=736
getIMU uS=734
kfP__ uS=2765
kf_U_ uS=2493
getIMU uS=741
getIMU uS=734
getIMU uS=734
kfP__ uS=2771
kf_U_ uS=2497
getIMU uS=741
getIMU uS=735
getIMU uS=733
kfP__ uS=2766
kf_U_ uS=2496
getIMU uS=741
getIMU uS=735
getIMU uS=731
kfP__ uS=2765
kf_U_ uS=2495
getIMU uS=740
		 @7:5.53 >> 2==	 ms_199
getIMU uS=738
getIMU uS=734
kfP__ uS=2764
kf_U_ uS=2494
getIMU uS=740
getIMU uS=734
getIMU uS=734
kfP__ uS=2765
kf_U_ uS=2495
getIMU uS=741
getIMU uS=735
getIMU uS=733
kfP__ uS=2768
kf_U_ uS=2499
getIMU uS=741
getIMU uS=737
getIMU uS=732
kfP__ uS=2756
kf_U_ uS=2495
getIMU uS=741
getIMU uS=734
getIMU uS=733
kfP__ uS=2764
kf_U_ uS=2497
getIMU uS=740
getIMU uS=735
getIMU uS=734
kfP__ uS=2768
kf_U_ uS=2495
getIMU uS=740
getIMU uS=736
getIMU uS=732
kfP__ uS=2795
kf_U_ uS=2493
getIMU uS=739
getIMU uS=736
getIMU uS=734
getIMU uS=733
getIMU uS=732
getIMU uS=734
getIMU uS=732
getIMU uS=733
getIMU uS=732
getIMU uS=735
getIMU uS=733
getIMU uS=732
getIMU uS=734
getIMU uS=733
getIMU uS=732
getIMU uS=732
getIMU uS=732
getIMU uS=732
getIMU uS=734
getIMU uS=733
getIMU uS=735
getIMU uS=733
getIMU uS=733
getIMU uS=732
getIMU uS=733
getIMU uS=732
getIMU uS=735
getIMU uS=733
getIMU uS=732
getIMU uS=731
getIMU uS=734
getIMU uS=732
getIMU uS=734
getIMU uS=733
getIMU uS=734
getIMU uS=733
getIMU uS=732
getIMU uS=732
getIMU uS=734
getIMU uS=736
		 @7:5.53 >> 3==	 ms_798
 
Status
Not open for further replies.
Back
Top