uNav AHRS

Status
Not open for further replies.
The loop() rate is use specific I suppose - and without drag would be 100K or a million+ - since the GPS message takes 2.2 ms - 1 KHz assures the buffer won't over flow and it will all be gotten in three passes - though 4 wouldn't hurt - just more lag. Twice that would be good - but the Filter code I was seeing about 700 uS at 240 MHz.
 
Maybe in loop() the : if(sincePrint > 25){
Could be : if ( (0 == GPS_newData ) && (sincePrint > 25)){ // ... }

That would just delay the serialDataOut() and all the math that goes with it for another 1-2 ms. And as soon as the GPS finishes it will hit it the next pass.
 
Watching TV here in the woodstove room - where my computer is so I tried some of the above!

This is an abominable FAIL - try it and laugh along - I did then commented it out - cout.send_now(); is evil::
if (1==GPS_newData) { readGPS(); cout.send_now(); } readGPS();

The other ideas noted after clean it up much better with another change or two as included in the attached?
View attachment MPU9250_FiltersGPS_V10c (5).zip

My T_3.6 is running at 96 MHz? and I see this debug output for the GPS data - the extra read #6 slips in just because of how the elapsedMillis I put in needs better logic for grouping - anything >= 1000 should advance the second count because it starts at zero - you can see the UTC times are accurate.:
2906.00==2906 |tS=0.04 | @78847.0000000000 ms198==1 ||
2907.00==2907 |tS=0.04 | @78847.2031250000 ms399==2 ||
2908.00==2908 |tS=0.04 | @78847.3984375000 ms599==3 ||
2909.00==2909 |tS=0.04 | @78847.6015625000 ms796==4 ||
2910.00==2910 |tS=0.04 | @78847.7968750000 ms999==5 ||
2911.00==2911 |tS=0.04 | @78848.0000000000 ms1200==6 || --------

2912.00==2912 |tS=0.04 | @78848.2031250000 ms200==1 ||
2913.00==2913 |tS=0.04 | @78848.3984375000 ms399==2 ||
2914.00==2914 |tS=0.04 | @78848.6015625000 ms600==3 ||
2915.00==2915 |tS=0.04 | @78848.7968750000 ms800==4 ||
2916.00==2916 |tS=0.04 | @78849.0000000000 ms1000==5 || --------

2917.00==2917 |tS=0.04 | @78849.2031250000 ms199==1 ||
2918.00==2918 |tS=0.04 | @78849.3984375000 ms398==2 ||
2919.00==2919 |tS=0.04 | @78849.6015625000 ms600==3 ||
2920.00==2920 |tS=0.04 | @78849.7968750000 ms800==4 ||
2921.00==2921 |tS=0.04 | @78850.0000000000 ms998==5 ||
2922.00==2922 |tS=0.04 | @78850.2031250000 ms1197==6 || --------
Click to expand...
 
Just downloaded CodeCompare - looks promising for a trial!

I was lazy when I did the second timing - wanting the "-------" separator after the print. Putting the code below in place of prior Zip'd serialDataOut() is better reading more logical output like this - and again this is running at 96 MHz:
--------
86.00==86 |tS=0.04 | @80513.7968750000 ms0==1 ||
87.00==87 |tS=0.04 | @80514.0000000000 ms200==2 ||
88.00==88 |tS=0.04 | @80514.2031250000 ms397==3 ||
89.00==89 |tS=0.04 | @80514.3984375000 ms600==4 ||
90.00==90 |tS=0.04 | @80514.6015625000 ms800==5 ||
--------
91.00==91 |tS=0.04 | @80514.7968750000 ms0==1 ||
92.00==92 |tS=0.04 | @80515.0000000000 ms200==2 ||
93.00==93 |tS=0.04 | @80515.2031250000 ms402==3 ||
94.00==94 |tS=0.04 | @80515.3984375000 ms602==4 ||
95.00==95 |tS=0.04 | @80515.6015625000 ms803==5 ||
--------
96.00==96 |tS=0.04 | @80515.7968750000 ms0==1 ||
97.00==97 |tS=0.04 | @80516.0000000000 ms201==2 ||
98.00==98 |tS=0.04 | @80516.2031250000 ms401==3 ||
99.00==99 |tS=0.04 | @80516.3984375000 ms600==4 ||
100.00==100 |tS=0.04 | @80516.6015625000 ms800==5 ||
--------
Click to expand...

Code: 
void serialDataOut()
{
  // Create single text parameter and print it
    if(rtk.upDated){
      GPSHz++;
      if ( GPSMon >= 950 ) {
        coutD.print( "--------\n" );
        GPSHz = 1;
        GPSMon =0;
      }
      if ( (rtk.heading +10) < GPSrxs ) {
        GPSrxs = rtk.heading;
        PairMiss++;
      }
      if(timeStamp*0.001 > 235.0 && timeStamp*0.001< 275) 
            incMissed++;
      coutD.print( rtk.heading );
      coutD.print( "==" );
      coutD.print( GPSrxs ); 
      coutD.print( "\t |tS="); 
      coutD.print( timeStamp*.001 ); 
      if ( rtk.heading != GPSrxs ) {
        coutD.print( "\t - " );
        coutD.print( GPSrxs - rtk.heading );
        coutD.print( " - " );
        coutD.print( PairMiss );
        coutD.print( " - " );
      }
      else
        coutD.print( " | " );
      if ( PairMiss ) {
        coutD.print( " == "); 
        coutD.print(PairMiss);
        coutD.print( " == "); 
        coutD.print(incMissed); coutD.println();
      }
      coutD.print( "\t @" );
            char utcText[30];
      dtostrf(utcTime, 10, 10, utcText);
      coutD.print( utcText );

      coutD.print( "\t ms" );
      coutD.print( GPSMon );
      coutD.print( "==" );
      coutD.print( GPSHz ); 
      coutD.print( " || " );
  #ifdef coutD
    coutD.print("\n");
  #endif
          rtk.upDated = false;
    } 
  cout.printf("%10.6f, %10.6f, %10.6f, %8.3f, %d",
           timeStamp*0.001, rtk.utcTime, IMU_RX_time*0.000001, 
           _dt, rtk.fixType);
  cout.send_now();
  cout.printf("%12.8lf,%12.8lf,%10.4f", rtk.lat1, rtk.lon1, rtk.hMSL);
  cout.send_now();
  cout.printf("%10.4f,%10.4f,%10.4f", rtk.velN, rtk.velE, rtk.velD);
  cout.send_now();
  cout.printf("%10.4f,%10.4f,%10.4f", ypr[1], ypr[2], ypr[0]);
  cout.send_now();
  cout.printf("%10.4f,%10.4f,%10.4f\n",aNEDx1, aNEDy1, aNEDz1);
}
 
:) yes it is. I was compiled and let it run and just looked and started laughing. You beat me too it. Woodstove - man that must be nice.

Anyway gave your latest version a try and it looks like you broke the code. I had tried something like your "if( (0 == GPS_newData ) && (sincePrint > 25))" except using rtk.upDated but still had all the misses. I saw you added a rtk.upDated = false in the serialportout function. I am really thinking that's what fixed it.
 
Yes, like GPS_newData not being reset at all - rtk.upDated = false; was being reset too late to be right. That helped the debug stats look right - other loop() changes make it flow well too.

Woodstove is nice indeed - I have an office downstairs I haven't used in years because the woodstove keeps me here fall to spring to keep it fed and for it to keep me warm - and once I get on one side of the stairs I stay too long and the front door and the world is upstairs in our house - and the woodstove gives upstairs the win - even in summer since I'm settled here.
 
Nice. Almost makes me want to move. Here my whole life so little change I will be moving soon. Although I've small wood burning stoves that I might be tempted at some point.

With the changes haven't seen a missed GPS so looks like that issue is solved on to the next that comes up. I picked up a new book that you might be interested in: C++ Concurrency in Action - Practical Multithreading by Anthony Williams. Not a bad book. Clearly written with lots of examples. Makes me less afraid of using threads.
 
Nice to have it running cleanly. The rtk.upDated and GPS_newData may need attention when the GPS data gets incorporated - they are overlapping. Would be a good time to remove some of the spare debug code and hacks that may be redundant or ugly - and so Brian and Don can have a clean start if this is the new baseline. Since you are more on the core purpose of the code I'll leave that to you for the next proper zip posting?

We miss 'back east'. But more moderate summer&winter have kept us. With a couple acres of trees that die in turn - woodstove is a natural - just lots of work for free heat.
 
I'll take care of the cleaning up the code and posting an updated working version (11). I'll work on that tomorrow. Eyes are half closed right now. Probably when Don gets his filter working we should just focus on the one filter instead of one size fits all.

The older I get the more I appreciate the milder temperatures. Oh well, nice to daydream.
 
I updated the Filters sketch to version 11 doing some code cleanup and incorporating the change to the debug routine in post 479. I did leave this piece in until with the changes to that Don is working on.

Cheers
Mike
 

Attachments

  • MPU9250_FiltersGPS_V11.zip
    32.1 KB · Views: 64
Pulled down and compiled V11 no problem - looks good Mike. It is running at 48 MHz on T_3.6 and seems fine as uploaded! GPS Debug log coming in like clockwork.

Walked through with CodeCompare and nothing looked odd in quick scan except: "timestamp = millis();" No timestamp is recorded in isr runFilter() ?

The code relating to 'GPSshows' could probably be removed - the debug count per second will show when there is trouble, and it won't likely be from the tuned up isr() missing anymore seeing it run at 48 MHz. Same for any use of 'GPSrxs'?

Compiled at 240 MHz on new install of TD_1.42b1 and it failed again because that copy did not have the F_BUS=120Mhz edit. There are some features missing not using i2c_t3 library - IIRC GetClock() returns actual clock speed on the bus, and I wonder if it would work with F_BUS 60 at 240 MHz showing a bug in the wire.h library?
 
Updated to Version 11

I'm leaving in timestamp in for now, using it as part of the serial prints as a time stamp on IMU data from when GPS was updated - check in SerialDataOut.

Just deleted GPSshows, since everything is tuned up no real need now.

You know, we could still use i2c_t3 library, we can just use an ifdef on TEENSYDUINO for i2_t3 else Wire for other boards.
 
Last edited:
Took a look at changing to i2c_t3 but looks like would also have to change the MPU9250 library as well. Oh well.
 
Just arrived back in Houston, ready to re-engage. Was on family trip, so pretty much only looked at code on the plane. Got kfInit debugged, have kfProp and kfUpdate to go. Then I need to catch up on what you two have been up to, sounds like some cool stuff over the last 4-5 days!

Once I get the kf routines debugged, I'll need to think about best ways to smooth out the accel data that comes out of the AHRS. I'm guessing we'll be able to get accel (body frame) from the AHRS at a fairly good clip, and then am hoping we can smooth it down to some slower rate and do the conversion to accel (NED-inertial). This (accel NED-inertial) is then what goes into the kfProp step, hopefully not so noisy. Then the kfUpdate step blends in the GPS position and velocity measurements. kfProp would be nice at something like 20Hz, whereas kfUpdate would run at the GPS rate (like 5Hz). Anyway, these are my thoughts so far.

Brian, you were running the propagate and update steps at same rate of the GPS receiver (5Hz) I believe in your implementation. What is your current thinking? For something like high accuracy drones applications, I'm hearing that folks are working towards wanting the INS/GPS to output on the order of 20Hz, so that's why I'm thinking that running the kfProp at 20 Hz (or faster, depending on the quality of accel control inputs) and the kfUpdate at 5Hz might be cool.
 
mjs513 said:
Took a look at changing to i2c_t3 but looks like would also have to change the MPU9250 library as well. Oh well.
Click to expand...

There was discussion about wire .vs. i2c_t3 some page or two back ... indeed it takes edits to MPU9250.

There should be a timestamp recorded on the IMU_isr() code - as that is the point it was known to be fresh - like the GPS.

Perhaps I'll get back to the RTC based CycleCounter based clock to get that as a uniform, less overhead time base.
 
Hi Tim. I made a few minor revisions to Version 11. I put back in the IMU timestamp :) (code is actually still there for the GPS timestamp if we need it, I changed the SRD to 4 for the 9250 which is 200 hz, and I added in a define for the printRate (milliseconds) for the sincePrint in the config tab. I set mine at every 10 milliseconds and everything is still working fine.
 

Attachments

  • MPU9250_FiltersGPS_V11a.zip
    32.1 KB · Views: 62
Got it. I was just adding a time monitor on the cout.printf() in serialDataOut(). On T_3.6 at 240 MHz.

It takes right at 14 ms to do those 5 printf()'s! And it shows it is doing 39 prints per second with printRate=25. I timed the send_now() and it was not adding much there 40 uS.

When I set printRate=10 the GPS_newData limiter shows it running at 70-72 Hz. And the printf()'s now show they are taking 25 ms - up from 14, because the USB is staying more filled.

You can monitor it if it looks useful replacing serialDataOut() in serialport.ino with adds as below to keep an eye on how fast it is running IMU prints:
Code: 
elapsedMicros TVt;
elapsedMillis TVsec;
uint32_t AC1=0, AC2=0, AC3=1000001; 
void serialDataOut()
{
  // Create single text parameter and print it
#ifdef coutD
    debugGPS();
    if ( TVsec >= 1000 ) {
      coutD.print( "\n | AC1 tos=");
      coutD.print( AC1 );
      coutD.print( "\t | AC2 tpf=");
      coutD.print( AC2 );
      coutD.print( "\t | AC3 Hz=");
      coutD.println( AC3 );
      if ( AC3 > 1000000 )
        TVsec = 0;
      else
        TVsec -= 1000;
      AC1 = AC2 = AC3 = 0;
    }
    AC3++;
    
    TVt = 0;
#endif

  rtk.upDated = false;
  cout.printf("%10.6f, %10.6f, %10.6f, %8.6f, %d",
           timeStamp*0.001, rtk.utcTime, IMU_RX_time*0.000001f,
           _dt, rtk.fixType);
  cout.printf("%12.8lf,%12.8lf,%10.4f", rtk.lat1, rtk.lon1, rtk.hMSL);
  cout.printf("%10.4f,%10.4f,%10.4f", rtk.velN, rtk.velE, rtk.velD);
  cout.printf("%10.4f,%10.4f,%10.4f", ypr[1], ypr[2], ypr[0]);
  cout.printf("%10.4f,%10.4f,%10.4f\n",aNEDx1, aNEDy1, aNEDz1);
  AC1 += TVt;
  
  TVt = 0;
  //cout.send_now();
  AC2 += TVt;
}
 
Interesting. I incorporated your print timings and saw almost no difference between printRates of 25 and 10 milliseconds. The only real difference I saw was between timing between serialDataOut and serialPortOut:
Code: 
SerialPrintOut:
| AC1 tos=5660	 | AC2 tpf=13	 | AC3 Hz=55  //printRate 25
| AC1 tos=5753	 | AC2 tpf=14	 | AC3 Hz=56  //printRate 10

SerialDataOut:
| AC1 tos=44348	 | AC2 tpf=12	 | AC3 Hz=55  //printRate 25
| AC1 tos=45291	 | AC2 tpf=18	 | AC3 Hz=55  //printRate 10

Out of curiosity I oc'd to 168Mhz for the SerialPortOut case with a printRate of 10:
| AC1 tos=4353	 | AC2 tpf=8 (actually runs between 4-14	 | AC3 Hz=53

I am assuming tos is the time in micros, tpf is lines per and Hz well that obvious. The Hz should be different unless there is something different between the way T3.5 and T3.6 handles prints?

Sorry for the delay in getting back to you on this. I've been under the weather all day.
 
It only directly times the serialDataOut as posted. All time values are in micros based on the elapsedMicros timer.

I'm in the middle of testing a version of elapsedCycles that emulates elapsedMicros but relies on the CycleCounter since the CycleCounter is a simple memory read.

I first designed and put it in the serialPortOut to time the dtostrf to buffers and the snprintf - but then moved it to the serialDataOut that was in use when updated V11a was posted. IIRC > 'AC1 tos' when that was Time Ouput to Strings when it was in serialPortOut

AC2 giving tpf was originally Time Print Flush - after the commented out send_now(), so it is now just some random value with send_now commented out.


I figured times would be different based on speed - but limited by USB throughput. Just set my T_3.6 to 120 and seeing the same AC3 70-72 Hz, though AC1 is now 46 ms - about the same as shown above.

To complete the compare I'm wondering if the PC is limiting your USB transfer? I'm on an i7 with TyCommander for SerMon, though I get same 71 Hz using IDE SerMon for EITHER one output window, so maybe my i7 using USB3 ports is faster?:
_48 MHz T_3.6:: AC1 tos=70213 | ... | AC3 Hz=45 [Note: Cycles between 44 and 72 Hz - but 'AC1 tos' hits 110675 ???]
120 MHz T_3.6:: AC1 tos=46750 | ... | AC3 Hz=71
168 MHz T_3.6:: AC1 tos=34187 | ... | AC3 Hz=71
240 MHz T_3.6 :: AC1 tos=24729 | ... | AC3 Hz=71
Click to expand...

I noted the set of SRD was showing failing inside the MPU9250 code - I wonder if that would start working if the i2c_t3 code was used instead of WIRE.h?

I just changed my i2c Wire.setClock() speed and it made no difference in Hz displayed over the 400K value.
 
Just remembered the times shown are cumulative for the second, not divided by Hz.

I put the above code with adds of : uint32_t AC0=0, AHz=0;
into code below and at 168 MHz I see this from 70 to 73 Hz::
AC0 tos=31187 | AC1 tsp=4249 | AC2 tpl=10 | AC3 tso=82 | AHz =71
Click to expand...

The slowest part is the Value to float string at over 400 uS each pass totaling the 31 ms used per second as the new AC0.

Code: 
void serialPortOut(){  
  // Set textLength to the number of parameters to print * 31
  int  textLength = 16 * 31;
  char text[textLength];
  
  //Temporary text parameters
  char acc_xText[30];
  char acc_yText[30];
  char acc_zText[30];
  char accNED_xText[30];
  char accNED_yText[30];
  char accNED_zText[30];
  char dtText[30];
  
  char utcText[30];
  char fixTypeText[30];
  char latText[30];
  char lonText[30];
  char altText[30];
  char velNText[30];
  char velEText[30];
  char velDText[30];
  
  char pitchText[30];
  char rollText[30];
  char yawText[30];
  char tS_Text[30];
  
#ifdef coutD
  if ( TVsec >= 1000 ) {
    coutD.print( " AC0 tos=");
    coutD.print( AC0 );
    coutD.print( "\t | AC1 tsp=");
    coutD.print( AC1 );
    coutD.print( "\t | AC2 tpl=");
    coutD.print( AC2 );
    coutD.print( "\t | AC3 tso=");
    coutD.print( AC3 );
    coutD.print( "\t | AHz =");
    coutD.println( AHz );
    AC0 = AC1 = AC2 = AC3 = AHz = 0;
    TVsec = 0;
  }
  AHz++;

  TVt = 0;
#endif
  dtostrf(timeStamp*0.001,10,6, tS_Text);          
  dtostrf(utcTime, 10, 10, utcText);
  dtostrf(_dt, 10, 10, dtText);
  
  dtostrf(uBloxData.fixType, 10, 7, fixTypeText);
  dtostrf(uBloxData.lat, 10, 7, latText);
  dtostrf(uBloxData.lon, 10, 7, lonText);
  dtostrf(uBloxData.height, 10, 7, altText);
  dtostrf(uBloxData.velN, 10, 4, velNText);
  dtostrf(uBloxData.velE, 10, 4, velEText);
  dtostrf(uBloxData.velD, 10, 4, velDText);

  dtostrf(ypr[1], 10, 4, pitchText);
  dtostrf(ypr[2], 10, 4, rollText);
  dtostrf(ypr[0], 10, 4, yawText);

  dtostrf(aNEDx1, 10, 4, accNED_xText);
  dtostrf(aNEDy1, 10, 4, accNED_yText);
  dtostrf(aNEDz1, 10, 4, accNED_zText);
  AC0 += TVt;
  TVt = 0;

  rtk.upDated = false;

  // Create single text parameter and print it
  snprintf(text, textLength, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
           tS_Text, utcText, dtText, fixTypeText,
           latText, lonText, altText,
           velNText, velEText, velDText,
           pitchText, rollText, yawText,
           accNED_xText, accNED_yText, accNED_zText);
  cout.println(text); 
  AC1 += TVt;
  TVt = 0;
  AC2 += TVt;
  TVt = 0;
  cout.send_now();
  AC3 += TVt;
}
 
I updated the code based on your changes. Seeing close to what I saw before in terms of Hz at 168Mhz oc'd. Our tsp are very similar and am seeing a about 40ms in tos vs. your 31ms. I had run a test with and without the send_now's and it seem to make much difference

AC0 tos=40500 | AC1 tsp=4430 | AC2 tpl=9 | AC3 tso=58 | AHz =54
Click to expand...

I am using the sermon for the debug prints, and am on Tosihba Laptop, i5. I expected differences from your results but what I was not expecting was the consistency in the Hz between the clock speeds.

EDIT. I am using IDE1.85 with TD1.40. Don't know if that makes a difference.
 
It finally registered in my foggy head that the times are really per second, DOH!!!!! Have to stop looking at code before my second cup of coffee So the data makes sense to me now.

I made a slight modification to your serialPrintOut code, AC1 is timing the snprintf time and AC2 is measuring the print time

Code: 
void serialPortOut(){  
  // Set textLength to the number of parameters to print * 31
  int  textLength = 16 * 31;
  char text[textLength];
  
  //Temporary text parameters
  char acc_xText[30];
  char acc_yText[30];
  char acc_zText[30];
  char accNED_xText[30];
  char accNED_yText[30];
  char accNED_zText[30];
  char dtText[30];
  
  char utcText[30];
  char fixTypeText[30];
  char latText[30];
  char lonText[30];
  char altText[30];
  char velNText[30];
  char velEText[30];
  char velDText[30];
  
  char pitchText[30];
  char rollText[30];
  char yawText[30];
  char tS_Text[30];

#ifdef coutD
  if ( TVsec >= 1000 ) {
    coutD.print( " AC0 tos=");
    coutD.print( AC0 );
    coutD.print( "\t | AC1 tsp=");
    coutD.print( AC1 );
    coutD.print( "\t | AC2 tpl=");
    coutD.print( AC2 );
    coutD.print( "\t | AC3 tso=");
    coutD.print( AC3 );
    coutD.print( "\t | AHz =");
    coutD.println( AHz );
    AC0 = AC1 = AC2 = AC3 = AHz = 0;
    TVsec = 0;
  }
  AHz++;

  TVt = 0;
#endif

  dtostrf(timeStamp*0.001,10,6, tS_Text);          
  dtostrf(utcTime, 10, 10, utcText);
  dtostrf(_dt, 10, 10, dtText);
  
  dtostrf(uBloxData.fixType, 10, 7, fixTypeText);
  dtostrf(uBloxData.lat, 10, 7, latText);
  dtostrf(uBloxData.lon, 10, 7, lonText);
  dtostrf(uBloxData.height, 10, 7, altText);
  dtostrf(uBloxData.velN, 10, 4, velNText);
  dtostrf(uBloxData.velE, 10, 4, velEText);
  dtostrf(uBloxData.velD, 10, 4, velDText);

  dtostrf(ypr[1], 10, 4, pitchText);
  dtostrf(ypr[2], 10, 4, rollText);
  dtostrf(ypr[0], 10, 4, yawText);

  dtostrf(aNEDx1, 10, 4, accNED_xText);
  dtostrf(aNEDy1, 10, 4, accNED_yText);
  dtostrf(aNEDz1, 10, 4, accNED_zText);

  AC0 += TVt;  // double to string time
  TVt = 0;

  rtk.upDated = false;

  // Create single text parameter and print it
  snprintf(text, textLength, "%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s,%s",
           tS_Text, utcText, dtText, fixTypeText,
           latText, lonText, altText,
           velNText, velEText, velDText,
           pitchText, rollText, yawText,
           accNED_xText, accNED_yText, accNED_zText);

  //snprintf(text, textLength, "%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f,%10.6f",
  //         timeStamp*0.001, rtk.utcTime,
  //         _dt, rtk.fixType,rtk.lat1, rtk.lon1, rtk.hMSL,
  //         rtk.velN, rtk.velE, rtk.velD,
  //         ypr[1], ypr[2], ypr[0],
  //         aNEDx1, aNEDy1, aNEDz1);
  AC1 += TVt;  // time to convert to text string
  TVt = 0;
  cout.println(text);  
  AC2 += TVt;  //time to print to port
  TVt = 0;
  cout.send_now();
  AC3 += TVt;  //send_now flush time
}

Now for the interesting part. I ran two cases at the same clock speed, 120Mhz but different printRates:

printRate = 25, 120Mhz : AC0 tos=25613 | AC1 tsp=2891 | AC2 tpl=212 | AC3 tso=40 | AHz =30
printRate = 10, 120Mhz : AC0 tos=53283 | AC1 tsp=5396 | AC2 tpl=388 | AC3 tso=69 | AHz =56
Click to expand...


Changing the clock to 168Mhz, and printRate=25:
AC0 tos=20334 | AC1 tsp=2301 | AC2 tpl=156 | AC3 tso=32 | AHz =30
Click to expand...
Times went down as expected but there was not enough so no change in the in the overall print rate, Ahz. I am using USB 3.0 port for the sermon (IDE);
 
View attachment MPU9250_INS6State_V1.zip

Here's my first cut at releasing the INS/GPS (simple) 6-state Kalman. When you run it, it prints 9 parameters to the serial terminal (LLA, XYZ covariance, XYZ innovations).

This release is VERY preliminary, with several limitations. I haven't extensively tested it, but it's iterating and output looks reasonable at first glance. But...

- It's running only at the GPS output speed of 5Hz. To really prove it's working, the kfProp step would need to be running faster (like 20Hz) than the kfUpdate (5Hz) step. And we'd need to verify that the accelNED control inputs are aiding (not hurting) the filter in kfProp.

- I suspect for it to really work well we will need smoothed values of accelNED, which we can get a number of ways (optimize MPU9250 settings, do a moving window smoother on MPU accel values, etc)

- timing for the accelNED values will need to be synch'ed with GPS time (I'm just grabbing the current value after reading GPS)

- adding 3 more states (acceleration) to the Kalman might really help. Then Q might have only values relating to acceleration.

Anyway, fun to see the KF at least cycling!!

Note: This version uses the "pre-threading version" from a week or two back. So none of the new things you two have been researching is incorporated.

Don
 
Last edited:
A couple of more notes on the Kalman I just posted...

1. I'm running the Kalman filter states in the ECEF (earth centered earth fixed) coordinate frame. This is very common for a lot of tracking systems, GPS, avionics, and satellite systems.

2. The LLA position and the NED velocity from the GPS is converted to ECEF before it's read by the filter as measurements for kfUpdate.

3. Raw accel (XYZ body) readings from the IMU are converted to accelNED and then to accelECEF, and then it is treated as a control input in kfProp.

4. Q is fixed right now. It really needs to be a function of dt, as in Brian's LQL' implementation.
 
Status
Not open for further replies.
Back
Top