uNav INS

Don,
Just loaded your v6 and running no problem with tonton81 new and improved SPI_MSTransfer library. Not officially published but wanted to give it a try just for sanity. Already tested v4 with all transfer options and they all work as well.

I also changed the SRD to 1 from 9, so that's 500hz updates for the IMU and no problems, but shouldn't be. Since you are updating at what appears to be 0.2 secs.

Mike
 
Right now for the EKF we're letting the propagation (time) stage refresh at whatever rate the accel and gyro readings become available, and then the update stage runs at the even dt = 0.2s GPS refresh rate. We could fix dt to 0.2s, but we'd lose the smaller propagation stage refreshes. So if I recall, onehorse was doing both the propagation and update stages at the same time, which is ok if you only want the EKF to refresh every 0.2s.

Leaving in dt as variable leaves the most options to the user, although I could see this EKF being used in perhaps three different "dt modes" perhaps:
1. Leave dt totally variable (as it is now)
2. Fix dt to 0.2s and refresh EKF prop and update stages together
3. Fix dt for propagation stage to say 20Hz, and set dt for update stage to 5Hz

Don
 
Mike,
Great, I meant to also try V6 also with different SRD values, so need to get to that today.

On the calibration side, I noticed something and was wondering if y'all are seeing this. I re-calibrated very carefully a couple of days ago and my unit then seemed to be very accurate with heading (I compared results to a compass). Now it's reading ~330 degrees when I point the IMU towards North. So I think my magnetometer drifts over several days, but not sure. 30 degrees off is quite a bit... and it's a consistent 30 degrees off. Will try re-calibrating again today and see what that does to heading.

Don
 
Morning Don,
Sorry for the delay - had to go make some coffee :)

Anyway to your points on update. I think going with option 3 would be better for a simple reason. 5Hz is geared towards updating is based on GPS updates (you can do 10hz GPS updates instead of 5) while the IMU (YPR) is updating to what ever we set at in. 20Hz for IMU would be could. I think you know a lot can happen to orientation in 1 sec time span and only 5 updates/sec would not be could for that. I hope this makes sense.

As to your last post, "magnetometer drifts over several days, but not sure" haven't really noticed that one been concentrating on yaw. But have you changed where you have put the IMU - its pretty sensitive to magnet variations. So if you calibrated in mid air then put it top of a table with a metal leg or on a metal desk it will pick it up. If you want to laugh it would even pick up the orientation of my desk chair if I wasn't careful :)
 
Option 3 is my fav too, but I think I remember Brian saying on a post that he likes something similar to Option 1 best.

The advantage to Option 3 is that any of the matrices with dt in them would then be constant, so much faster on the EKF propagation stage calculations. But then again, the T3.6 is doing the dt calculations pretty darn fast I think!

My IMU is on the same place on my wooden desk, altho there are metal brackets underneath the desk. Seems to have the same offset if I move it to other spots on the desk too. But I've got some longer USB cables so will hopefully do an outside test today once I recalibrate. Also want to see if altitude drifts less when I get the IMU out in a more open-sky area.
 
I really think option3 is the best way to go - but will have to do some tests I guess. When we get a nice stable version and what you might say is production code I will start back up on the magnetic interference code :)
 
Great. Then I'll make a first cut at an Option 3 version. For initial release I'll just get a dt = 20Hz version working, then we can brainstorm on fixing certain matrices constant and leveraging Tim's timing tricks/ideas too.

Off to early lunch... will look at this later this afternoon.

Don
 
Okey dokey Don. By the way I added Don's code into the sketch for a test and it works. gps is about 0.2 seconds on average
 
Current run from Yesterday of _Ver6 about to end at seconds :: 60703.203125 - so the GetSeconds() is working.

When I asked the GPS to emit 10 Hz updates instead of 5 Hz - every other update was unchanged? It didn't recalc at 10 Hz - it still did 5 Hz and just printed at 10 Hz with prior data. Maybe I didn't adjust another setting?

Coding the posted IMUdt as prior post suggested. It shows that dt is all but constant. That time would be 0.002 secs at IMU 500 Hz - what is shown only updates when the GPS data completes so it is on the 5 Hz schedule!:
Code: 
float FccPartSec( uint32_t CntPri1, uint32_t CntPri2 ) { // 6A18
  return ( (CntPri2 - CntPri1) / (float)cctAvg);
}
Above code is all that needs added to :: T_timeBase.ino, I'll post my current debugGPS() below showing usage for New & Old IMU times to get this.
NOTE: The same done for ccTimeNow_GPS is true time between GPS Tx. What is shown as "GPScc:218380" is similar (uint32_t # of millis) - but it shows the time from the start of the GPS Tx - to the time the receive was complete and it got to debugGPS()

If the same times for the current IMU dt were shown I suspect they would be even more variable "GPScc .vs. GPSdt" so using that as dt will add bias to the results - based on 'when the loop() called the function'.
This display of IMUdt is per 100 IMU updates as noted only printed/updated when the GPS is.

I agree Don's option #3 is best to reduce computational overhead and have all samples weighed the same without adding error from Math or 'perceived dt' - when the device is programmed to hit a fixed #. But that will be tied to SRD in use!

But - if best TRUE dt calculated per sample is desired - use the math as it is done here and EACH SAMPLE must be run with the dt seen between samples and it will auto adjust for current SRD in use.

Here debugGPS() showing
-----Loop#:486520 IMU#:501 >> 1Sec:100048200 >> IMUcc:95966
#1248 @16:48.42_342133 [fix:3 #:11 GPScc:289610 > cctAv:179999632 > GSecs:264.377991 > IMUdt:0.19977057 > GPSdt:0.19939037
#1249 @16:48.42_200342210 [fix:3 #:11 GPScc:218373 > cctAv:179999632 > GSecs:264.577759 > IMUdt:0.19777285 > GPSdt:0.19924930
#1250 @16:48.42_400342287 [fix:3 #:11 GPScc:304282 > cctAv:179999632 > GSecs:264.775543 > IMUdt:0.20176823 > GPSdt:0.20013787
#1251 @16:48.42_600342364 [fix:3 #:11 GPScc:245214 > cctAv:179999636 > GSecs:264.977295 > IMUdt:0.19977058 > GPSdt:0.20038478
#1252 @16:48.42_800342442 [fix:3 #:11 GPScc:218388 > cctAv:179999636 > GSecs:265.177063 > IMUdt:0.19777280 > GPSdt:0.19816259
-----Loop#:486218 IMU#:500 >> 1Sec:99887600 >> IMUcc:98291
#1253 @16:48.43_342519 [fix:3 #:11 GPScc:226353 > cctAv:179999636 > GSecs:265.374847 > IMUdt:0.20176826 > GPSdt:0.20157401
#1254 @16:48.43_200342596 [fix:3 #:11 GPScc:218307 > cctAv:179999636 > GSecs:265.576630 > IMUdt:0.19977054 > GPSdt:0.19989914
#1255 @16:48.43_400342673 [fix:3 #:10 GPScc:253412 > cctAv:179999636 > GSecs:265.776398 > IMUdt:0.19977057 > GPSdt:0.19936040
#1256 @16:48.43_600342750 [fix:3 #:10 GPScc:218322 > cctAv:179999638 > GSecs:265.976166 > IMUdt:0.19977057 > GPSdt:0.20083916
#1257 @16:48.43_800342827 [fix:3 #:11 GPScc:304823 > cctAv:179999638 > GSecs:266.175934 > IMUdt:0.20176829 > GPSdt:0.20016766
Click to expand...


Don - you added # to the Master output - not sure if it tell you anything?
-----Loop#:484758 IMU#:499 >> 1Sec:99638752 >> IMUcc:110028
#303633 @16:4.49_324895 [fix:3 #:14 GPScc:218384 >> cctAv:179999631 >> GSecs:60740.406250
60740.601562, 52.6771, 154.1658, -138.2148, 53.3616, 48.214401,-122.450371, 46.5161
#303634 @16:4.49_200324973 [fix:3 #:14 GPScc:218307 >> cctAv:179999631 >> GSecs:60740.601562
60740.804688, 52.6326, 154.3710, -137.9311, 53.6453, 48.214401,-122.450371, 46.4971
#303635 @16:4.49_400325051 [fix:3 #:14 GPScc:290632 >> cctAv:179999632 >> GSecs:60740.804688
60741.007812, 52.5658, 154.7678, -137.5087, 54.0677, 48.214401,-122.450371, 46.4760
#303636 @16:4.49_600325128 [fix:3 #:14 GPScc:277799 >> cctAv:179999632 >> GSecs:60741.007812
60741.207031, 52.5979, 154.4249, -137.5678, 54.0086, 48.214401,-122.450371, 46.4552
#303637 @16:4.49_800325206 [fix:3 #:14 GPScc:244432 >> cctAv:179999632 >> GSecs:60741.207031
60741.406250, 52.7022, 154.6446, -137.9012, 53.6752, 48.214401,-122.450371, 46.4335
Click to expand...

Updated debugGPS() to drop in and see above results::
Code: 
void debugGPS( )
{
  static uint32_t cctLast = 0;
  static uint32_t GPScnt = 0;
  static unsigned char lastT = 'a';
[COLOR="#FF0000"]  static uint32_t cctLast_IMU = 0;
  static uint32_t cctLast_GPS = 0;
[/COLOR]
  uint32_t ccNow = ARM_DWT_CYCCNT;

  qBlink();
  GPScnt++;
  if ( lastT != uBloxData.utcSec ) {
    coutD.print( "-----" );
    coutD.print( "Loop#:" );
    coutD.print( CntLoop );
    coutD.print( " IMU#:" );
    coutD.print( CntIMU );
    coutD.print( " >> 1Sec:" );
    coutD.print( ccPartSec( cctLast, ccNow ) );
    cctLast = ccNow;
    coutD.print( " >> IMUcc:" );
    coutD.print( ccPartSec( ccTimeNow_IMU, ccNow ) );
    coutD.println( );
    CntLoop = 0;
    CntIMU = 0;
  }
  lastT = uBloxData.utcSec;
  coutD.print( "    #" );
  coutD.print( GPScnt );
  coutD.print( "  @" );
  coutD.print( uBloxData.utcHour);
  coutD.print( ":" );
  coutD.print( uBloxData.utcMin);
  coutD.print( "." );
  coutD.print( uBloxData.utcSec);
  coutD.print( "_" );
  coutD.print( uBloxData.utcNano);
  coutD.print( " [fix:" );
  coutD.print( uBloxData.fixType);
  coutD.print( " #:" );
  coutD.print( uBloxData.numSV);
  coutD.print( " GPScc:" );
  coutD.print( ccPartSec( ccTimeNow_GPS, ccNow ) );
  coutD.print( " > cctAv:" );
  coutD.print( cctAvg );
  coutD.print( " > GSecs:" );
  char textTemp[30];  // won't match TViewer as it is done on GPS detect before serialPrint reset
  dtostrf(DrtcTime, 10, 6, textTemp);
  coutD.print( textTemp );
[COLOR="#FF0000"]  coutD.print( " > IMUdt:" );
  dtostrf( FccPartSec( cctLast_IMU, ccTimeNow_IMU ), 10, 8, textTemp);
  coutD.print( textTemp );
  coutD.print( " > GPSdt:" );
  dtostrf( FccPartSec( cctLast_GPS, ccTimeNow_GPS ), 10, 8, textTemp);
  coutD.print( textTemp );
  cctLast_IMU = ccTimeNow_IMU;  
  cctLast_GPS = ccTimeNow_GPS;  
[/COLOR]  coutD.println( );
}
 
mjs513 said:
Okey dokey Don. By the way I added Don's code into the sketch for a test and it works. gps is about 0.2 seconds on average
Click to expand...

My post shows the dt calc on GPS time between each dt? What code of Don's did you use to see similar?

GPSdt:0.20157401
GPSdt:0.19989914
GPSdt:0.19936040
GPSdt:0.20083916
GPSdt:0.20016766
Click to expand...
 
Last edited:
Well... I thought I had an idea on how to do 20Hz updates, but now I'm stumped in how to implement it. The challenge is that the 20hz (50ms) propagation steps have to be synch'ed to the start of the GPS 5 Hz samples. Here was the idea I had:

////////////////////////////////////////////////////////////////////////////////////////////////////////////////
read new GPS // as soon as it's ready
--nextCounter = micros() + 50000
--run Filter // both prop and update stages
--masterSerialPrint() // prints info from update stage to Master
--slaveSerialPrint() // prints info from prop/update stage to Slave

--for i = 1:9 // so this loop does the prop stage for 9 times
----while (nextCounter > micros()) {} // wait 50ms
----run Filter // runs only prop stage
----slaveSerialPrint() // prints info from prop stage to Slave
----nextCount = nextCount + 50000 // add 50ms to counter
--end for

end // end read new GPS
////////////////////////////////////////////////////////////////////////////////////////////////////////////////

So this approach triggers off of new GPS data arriving, but we're currently triggering off new IMU data.

What do y'all think? Is this a good approach? If so, how do we switch things around to be synch'ed to GPS? I suspect there's a clever way, but it's not coming to me yet.

Back to messing with IMU calibration for now...

EDIT: Darn, my code above is hard to read. I have spaces in my version, but this editor drops the spaces when I post it. the dashes are my spaces now...

Don
 
Last edited:
Wrap in the code brackets - the number sign. In the old version you had two calls one for propagate and another for update. The update stage would be run when gps data was available which was in the IMU loop when its interrupt hit - think it was the Euler-filter version.
 
Don - as Mike notes - use the CODE "#" Hashtag icon to wrap spaced text and it is preserved.

So EACH IMU update needs to run Propagate. It has to be pushed through while it is current for the dt - unless fixed based on SRD math counts to time?

And each time GPS data arrives you want to Update - and then 3 more updates before the next GPS - while each intermediate IMU is Propagated?

I'd do a counter and use " if ( !(counter % UP_VALUE) ) Update()"

At (SRD == 3) there are 250 IMU's/sec - which is 50 per GPS so UP_VALUE of "10" with Counter set ZERO on each GPS and incremented each IMU would fire update at :: 0, 10, 20, 30, 40

Then the GPS would set back to ZERO after #49 - and start the cycle over again?

Something like this?

Code: 
uint32_t UP_VALUE = 0;
uint32_t UP_CNT = 0;

setup()  {
// ...
  // This could be hardcoded - or done mathywise based on SRD - but here are two examples that work easily
  if ( SRD == 3 ) UP_VALUE = 10;
  if ( SRD == 1 ) UP_VALUE = 20;  // 500 Hz IMU - 100 per GPS : fire update at :: 0, 20 , 40 , 60 , 80
}

loop() {
if ( GPS_READY ) {
  UP_CNT = 0;
  Filter.Propagate() // done for GPS data?
  Filter.Update()
} 

if ( IMU_READY ) {
  UP_CNT++;
  Filter.Propagate()
  if ( !(UP_CNT % UP_VALUE) ) Filter.Update()
}
}

<EDIT>: gotta RUN - re-reading I see that is 25 Hz --- that's the concept ... no timers or tricky wasteful math ... just clockwork ...
 
Think the way you did it before would be using Tim's example:
Code: 
// Wait for new data from IMU to run filter algorithms
  if (IMU_READY ) {
     Filter.Propagate(); 

      if(GPS_READY)
      {
              Filter.Update();
      }
   .......

   insert code for printRate.......
}

Then IMU would update at the rate that is specified by SRD, and update at the GPS available that you set. Just a thought
 
Mike,
If we did it that way, I don't see how we'd get a constant dt because the IMU data would come whenever. So it would never be "in sync" with the incoming GPS data. That's why I was proposing running the IMU really fast, then wait for GPS. When GPS arrives, start 50ms timer, and do a prop followed by update. for the next 3 50ms intervals, grab an IMU reading and do the prop step only. Then wait for next GPS meas and start over. (Tim, you are right, it's 3 IMU readings not 9 like I had said).

To answer Tim's questions:
1. The Prop step requires accel and gyro readings
2. The Update step requires a GPS posn, GPS vel, and mag readings

So a tough challenge seems to be grabbing the "best" (closest) IMU readings given the dt we select. So if we select 20Hz for dt, we want accel and gyro readings at 50ms intervals, and GPS and mag readings at 200ms.

It looks like there's a uBlox bool command called upDated which goes true if the GPS is data is new. don't know how it resets, maybe it resets after a GPS read command.

Don
 
That's why I was proposing running the IMU really fast, then wait for GPS.
Click to expand...
Am still a little concerned that because it sounds like when you loose the INS you loose the orientation. From your explanation you are pull accel/gyro data during every IMU update and then only pulling mag data when GPS is available? For stable yaw and heading you are going to need mag data to correct any drift from accel/gryo's.

Or am I off the wall here.

Mike
 
Mike,
It's still using the mag data to correct yaw/heading drift, but it just does so at the GPS rate (the way this particular INS code is set up). So one way to look at it is that in-between the "big-step" GPS/mag measurement updates, the accel and gyro info is used as control inputs to tweak the position and attitude in "small-steps."

This actually seems like a pretty cool way to do it, in that the mag IMU readings are often available at a much slower rate than the accel / gyro readings anyway, so this could let the accel / gyro data be leveraged at a faster rate.

Don
 
Running some tests outside today. Want to see if outside calibration seems to work any better, and if better GPS visibility tightens up the altitude wander somewhat....

Don
 
Thanks Don. Think still need to address can we get orientation data output even if GPS is not available?

Good Luck with your outside tests today.

Oh BTW. Did a modification to the sketch this morning so now I can select one of two messages to send from the master. Message 55 is the full 77 variables and message 56 is reduced to 11 (ypr, lat, long, vels and a couple of others). Works off of your serialEvent code - 5 will send message 55 and 6 will send message 56 :)
 
Speaking of the serialEvent code I finally added to adjust Params and alter behavior.

When I put that in I left in 'p' for Pause - it does a delay(500). Typically that causes NaN Assert fail on most if not every try. Wondering why that is - perhaps the 'dt' math calculation? Somehow it breaks the filter math?

Good use of Alternate messages - that is why I showed adding a new message in the sample - was dismayed when the old '55' 12 var version was overwritten as that message alternative was what I envisioned - so it could work with old, alternate or evolving Slave code for varying messages and testing.

As for processing incoming data - it seems any reported IMU - interrupt presented at SRD rate - data should be filter.Prop() as it arrives. After the INT happens the read is FAST without wait for data calc on SRD schedule. I don't see how a 50 ms delay/wait process of any sort would be helpful? My post #438 scheme was to always run IMU .prop() and when a GPS data came .Prop it - them IMU - then do .Update { as below with MAG data refresh? }

As for MAG data it comes at some lesser multiple by nature. If not immediately propagated - it should at least be 'read' and averaged? Maybe pushed into a CB for every new reading - then pulled as CB.AVG when time to Propagate>Update it : perhaps at the time GPS data is received? - and then that AVG CB should be cleared to EMPTY?

I see this is hardcoded - not a #define:: Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_41HZ);
> That suggests about 8 sets of mag data for each of 5 GPS data sets? But that goes up to DLPF_BANDWIDTH_184HZ. So 3 MAG_CB's or a MAG_CA[3] for x,y,z of length 64 would allow reading that data - pushing it until GPS ( or as desired? ) then grab .average()'s and then do .clear()'s? Assuming a simple average over 0.2 secs would be the best way to introduce MAG data? Or since the GPS can do 0.1 sec updates ( Thanks Mike ) that might change.


Don - Can you bump your hardware "#define GPS_BAUD 115200"? Mike and I are at 460800, and it seems we could double or triple that safely based on Chris O.
 
By the way was testing Chris's code and the GPS works fine at 921600 baud if you are interested. At some point I going to switch over to use Chris's library. Will allow you to make some GPS setting changes directly from the sketch.
 
Tim,
I tried both 460800 and 230400 for GPS_BAUD, and mine only seems to work if I set it to 115200. Wonder if I need to change some other uBlox setting?

Here's another weird thing. I'm trying to run some tests outside. Normally I power my uBlox M8N with a USB cable from a USB port on either my laptop or iMac. To move outside, I thought I'd power it with a USB power supply (phone charger type). When I plug the uBlox into the USB power supply, the light comes on, but the filter stops working (i.e., stops sending data to Master terminal). When I plug it back into the computer USB, everything works again. Aaarrrrgggg. Guessing this is some sort of USB interface issue rather than power issue....
 
Outside testing update... So far I seem to be getting better Heading reporting, we'll see if it drifts. I pointed +X axis towards magnetic North using my iPhone (with Compass mode set to Mag not True North), and the filter is reading 356 deg, so that's probably not bad. Let's see if it holds there for a while...
 
for higher speeds UART, when I worked on porting ESP8266 core methods to teensy 3.6, I was able to achieve 3Megabaud easily without CTS/RTS. The loop alone can’t do this, you NEED to use SerialEvent(), this solved all issues with high speed uart communication

it was as simple as putting ESP.events() in both main loop() and SerialEvent function, no other code required
 
Mike,
Let me know how testing with Chris' GPS library goes. Sounds interesting.

Heading still holding at 357 or so, decent I think....

Don
 
You must log in or register to reply here.
Back
Top