uNav INS

Don Kelly · Apr 2, 2018

tonton81,
I wouldn't recommend naming the intermediate steps. So maybe just something like

cb.average() and cb.stdev()

Sorry about my short replies, am off to meetings this morning then will re-engage this afternoon. Tough to keep up with you guys!!

P.S. I've ordered one of Brian's Marmot (hardwired MPU9250 and T36) boards. Looking forward to loading this all on it. No more loose wires issues with it I hope! and the SPI connections will be via cable with plus too I think...

Don

tonton81 · Apr 2, 2018

ill be home in 1 hr or so to add the queue features for Don, this is gonna be an awesome addition

indeed i wont make steps, i just figured now ill just keep average() and deviation(), and maybe if you want the variance() as well,

lemme know im en route

mjs513 · Apr 2, 2018

tonton81 = might as well add variance. Easier to add now than later. This way it will be complete

Don: nice board that Brian has - wish he had a version without the beagle connectors soldered on

tonton81 · Apr 2, 2018

ok! ill add deviation and variance

bbl

tonton81 · Apr 2, 2018

According to: http://www.calculator.net/standard-deviation-calculator.html
Thats where I'm using it's calculator to test if the variance is correct in my library, I paste this on the website:

Code:

3.14159,12.3456,78.91234

Website says: Variance (Population Standard), σ2 1139.6723588745

However, serial monitor reports:

Code:

1139.67236328125

am i doing something wrong or thats just how the floats work on microcontrollers vs computers?

This is the current method:

Code:

template<typename T, uint16_t _size, uint16_t multi>
T Circular_Buffer<T,_size,multi>::variance() {
  T _mean = average();
  T value = 0;
  for ( uint16_t i = 0; i < _available; i++ ) {
    value += ((_cbuf[(head+i)&(_size-1)] - _mean) * (_cbuf[(head+i)&(_size-1)] - _mean));
  }
  value /= _available;
  Serial.println(_mean,11); // [COLOR="#006400"]debug[/COLOR]
  Serial.println(value,11); // [COLOR="#006400"]debug[/COLOR]
  return value;
}

Don Kelly · Apr 2, 2018

tonton81,

you want the general form for standard deviation, the "population" version, where you divide by n,

not the "sample" form for standard deviation where they calculate it a bit different and divide by n-1

The link I sent on my post from last night has a nice example with real numbers that we can test with, using a population size of 20 samples. for his example of 20 samples, mean comes out to 7, and standard deviation comes out to 2.983.

Don

tonton81 · Apr 2, 2018

Yeah im just need to fix the variation to make sure its okay before i use it for deviation calculation afterwards, i just wanna make sure the numbers match

the calculater from the site you linked showed this:

Code:

3419.017076
(Add up the Squared Differences)
Variance:
[COLOR="#FF8C00"]1139.672359[/COLOR]
(Sum of Differences2 / Count)

and teensy got this:
1139.67236328125

Don Kelly · Apr 2, 2018

tonton81,
this will be really nice to have. This will allow us to dig into the inner workings of the EKF and attempt to optimize the tuning parameters (Q and R). Brian has some code in his uNavAHRS that uses a particular method for calculating the mean and variance of the gyro, accel, and mag when you kick off the IMU, but your circular buffer is much better suited (as best I can tell) for continually getting an updated mean and deviation in real time. Very slick.

Time for me to download Mike's latest version of uNavINS and give it a whirl...

Don

tonton81 · Apr 2, 2018

Code:

  Serial.println(myFloats.deviation(),8); // print last 8 decimal places

Output:

Code:

33.75903320

Lastly, take the square root of the Variance:
Standard Deviation:
33.75903374
(The square root of the Variance)

Thats pretty close! But yeah, ready for action

deviation function was easier:

Code:

template<typename T, uint16_t _size, uint16_t multi>
T Circular_Buffer<T,_size,multi>::deviation() {
  if ( multi || !_available ) return 0;
  return sqrt(variance());
}

I also added mean() method, its same as average()

Circular_Buffer will have a commit soon, with these new features

Code:

        T variance();
        T deviation();
        T average();
        T mean() { return average(); }

EDIT: commit complete: https://github.com/tonton81/Circular_Buffer/commit/a438494278cfe0899972a68bb84d4ea95d5ff68f

Don Kelly · Apr 2, 2018

tonton81,
Cool! I'll try it out in a few hours!

Mike,
Am running your latest uNavINS version, so far so good. My pitch and roll values are starting high (like 8-10 deg), so I will re-calibrate my IMU later today to see if that helps. Pretty cool.

Once I make sure angles are looking steady again, my thought is to do a little test routine to make sure I see what tonton81 is doing with his mean/deviation functions. then i'll pick a particular EKF state (like "pitch" maybe) and then develop the analysis plots via SPI and TViewer...

Don

mjs513 · Apr 2, 2018

tonton81:

Don: Glad its finally working the way its suppose to. The only value starting high for me was roll at 25 degrees. Same here on recalibration eventually. Good luck.

defragster · Apr 2, 2018

tonton81 - may not be useful here - but since you are crunching stats - MIN and MAX might be nice to have?

mjs513 said:
Having coffee now so in an hour or so i will be awake again.
I am now running the Master at 168Mhz and the Slave at 144Mhz. Yeah. I didn't update to the latest yet - probably will this morning now that this is working.
Glad you found that Assert too. Have a few more compiler things I can add to my list - wish there was one spot that had what you could use.
For the test ran it for about 5000secs no issues.
Now guess Don can continue the KF debug effort and expand on it.
Not sure what to do next - slave I2C or play circular buffer. Oh well....

Didn't make it to 5,000 here ... I came to find a failed assert for NaN found at 4281.916992!
Master:

Code:

-----Loop#:281139 IMU#:502
	 #21403  @11:46.6_479937 [fix:3 #:16
	 #21404  @11:46.6_200480011 [fix:3 #:15
	 #21405  @11:46.6_400480086 [fix:3 #:16
	 #21406  @11:46.6_600480160 [fix:3 #:16
	 #21407  @11:46.6_800480235 [fix:3 #:16
-----Loop#:281385 IMU#:501
	 #21408  @11:46.7_480310 [fix:3 #:16
	 #21409  @11:46.7_200480384 [fix:3 #:16
	 #21410  @11:46.7_400480459 [fix:3 #:16
ASSERT: NaN Fail index = 26
 ___ ASSERT FAILED ___ FILE, LINE#, Expression 
T:\tCode\_GPSimuDK\1Apr18\uNavINS_MPU9250_I2C_MAGV3\C_serialPrint.ino
64
!std::isnan( MST_PrintVals[jj] )
 ___ ASSERT FAILED ___ STOPPING !!!!

Slave - At END of TyComm buffer:

Code:

4281.838867, 51.198551,154.976212,-148.012558, 63.657646, 48.214287,-122.450485, 24.315233,  0.000311, -0.000629,  0.018962, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.560522,  0.560557,  0.721608,  0.541436,  0.542839,  0.531820,  0.050322,  0.031611,  0.043818,  0.089853,  0.093999,  0.095019,  0.006747,  0.005976,  0.006443,  0.388454,  0.698663,  0.543849, -0.255346,
4281.841309, 51.198593,154.976074,-148.012619, 63.657585, 48.214287,-122.450485, 24.315195,  0.000327, -0.000660,  0.018958, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.560747,  0.560783,  0.721781,  0.543380,  0.544798,  0.533661,  0.050326,  0.031615,  0.043822,  0.089854,  0.094000,  0.095019,  0.006747,  0.005976,  0.006443,  0.388455,  0.698663,  0.543849, -0.255346,
4281.843262, 51.198643,154.975998,-148.012650, 63.657551, 48.214287,-122.450485, 24.315159,  0.000336, -0.000712,  0.018936, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.560973,  0.561010,  0.721955,  0.545319,  0.546752,  0.535495,  0.050330,  0.031618,  0.043825,  0.089854,  0.094000,  0.095019,  0.006747,  0.005976,  0.006444,  0.388456,  0.698663,  0.543849, -0.255346,
4281.845215, 51.198753,154.975906,-148.012726, 63.657475, 48.214287,-122.450485, 24.315113,  0.000353, -0.000755,  0.018896, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.561259,  0.561298,  0.722176,  0.547734,  0.549186,  0.537779,  0.050335,  0.031623,  0.043830,  0.089855,  0.094000,  0.095020,  0.006748,  0.005977,  0.006444,  0.388456,  0.698662,  0.543848, -0.255346,
4281.847168, 51.198830,154.975754,-148.012863, 63.657337, 48.214287,-122.450485, 24.315084,  0.000365, -0.000771,  0.018871, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.561432,  0.561471,  0.722309,  0.549179,  0.550642,  0.539145,  0.050338,  0.031625,  0.043832,  0.089855,  0.094000,  0.095020,  0.006748,  0.005977,  0.006444,  0.388457,  0.698662,  0.543848, -0.255347,
4281.849121, 51.198837,154.975494,-148.013138, 63.657066, 48.214287,-122.450485, 24.315048,  0.000375, -0.000794,  0.018822, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.561665,  0.561705,  0.722488,  0.551101,  0.552579,  0.540961,  0.050342,  0.031629,  0.043836,  0.089855,  0.094001,  0.095020,  0.006748,  0.005978,  0.006445,  0.388458,  0.698662,  0.543847, -0.255346,
4281.851074, 51.198818,154.975113,-148.013428, 63.656773, 48.214287,-122.450485, 24.315002,  0.000379, -0.000817,  0.018793, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.561958,  0.561999,  0.722715,  0.553495,  0.554991,  0.543223,  0.050347,  0.031633,  0.043841,  0.089856,  0.094001,  0.095020,  0.006749,  0.005978,  0.006445,  0.388460,  0.698663,  0.543846, -0.255345,
4281.853027, 51.198895,154.974915,-148.013596, 63.656609, 48.214287,-122.450485, 24.314966,  0.000376, -0.000829,  0.018760, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.562195,  0.562237,  0.722897,  0.555404,  0.556915,  0.545025,  0.050351,  0.031637,  0.043844,  0.089856,  0.094001,  0.095020,  0.006749,  0.005979,  0.006445,  0.388461,  0.698662,  0.543845, -0.255345,
4281.854980, 51.198975,154.974823,-148.013657, 63.656540, 48.214287,-122.450485, 24.314928,  0.000390, -0.000835,  0.018737, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.562435,  0.562477,  0.723081,  0.557307,  0.558834,  0.546822,  0.050355,  0.031641,  0.043848,  0.089857,  0.094001,  0.095020,  0.006749,  0.005979,  0.006446,  0.388462,  0.698662,  0.543845, -0.255346,
4281.857422, 51.199085,154.974777,-148.013763, 63.656445, 48.214287,-122.450485, 24.314892,  0.000404, -0.000833,  0.018719, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.562676,  0.562719,  0.723267,  0.559205,  0.560747,  0.548612,  0.050359,  0.031644,  0.043851,  0.089857,  0.094002,  0.095021,  0.006750,  0.005979,  0.006446,  0.388462,  0.698662,  0.543844, -0.255347,
4281.858887, 51.199203,154.974823,-148.013840, 63.656361, 48.214287,-122.450485, 24.314856,  0.000414, -0.000822,  0.018719, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.562919,  0.562964,  0.723454,  0.561098,  0.562654,  0.550397,  0.050363,  0.031647,  0.043855,  0.089858,  0.094002,  0.095021,  0.006750,  0.005980,  0.006447,  0.388462,  0.698662,  0.543844, -0.255348,
4281.869141, 51.199051,154.974365,-148.014343, 63.655849, 48.214287,-122.450485, 24.314672,  0.000464, -0.000840,  0.018762, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.564166,  0.564216,  0.724413,  0.570480,  0.572113,  0.559238,  0.050382,  0.031665,  0.043874,  0.089860,  0.094003,  0.095022,  0.006752,  0.005982,  0.006448,  0.388463,  0.698664,  0.543842, -0.255347,
4281.871094, 51.199028,154.974274,-148.014435, 63.655762, 48.214287,-122.450485, 24.314627,  0.000472, -0.000867,  0.018760, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.564486,  0.564537,  0.724659,  0.572806,  0.574458,  0.561426,  0.050387,  0.031670,  0.043878,  0.089860,  0.094003,  0.095022,  0.006752,  0.005982,  0.006449,  0.388463,  0.698664,  0.543841, -0.255346,
4281.873047, 51.199051,154.974274,-148.014465, 63.655735, 48.214287,-122.450485, 24.314590,  0.000479, -0.000897,  0.018754, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.564744,  0.564797,  0.724858,  0.574661,  0.576328,  0.563171,  0.050391,  0.031673,  0.043882,  0.089861,  0.094004,  0.095022,  0.006752,  0.005983,  0.006449,  0.388463,  0.698664,  0.543841, -0.255347,
4281.875000, 51.199059,154.974274,-148.014481, 63.655712, 48.214287,-122.450485, 24.314554,  0.000497, -0.000943,  0.018739, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.565005,  0.565059,  0.725058,  0.576510,  0.578194,  0.564910,  0.050395,  0.031677,  0.043886,  0.089861,  0.094004,  0.095022,  0.006753,  0.005983,  0.006449,  0.388463,  0.698664,  0.543841, -0.255347,
4281.877441, 51.199001,154.974258,-148.014542, 63.655651, 48.214287,-122.450485, 24.314518,  0.000509, -0.000991,  0.018710, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.565268,  0.565323,  0.725260,  0.578355,  0.580054,  0.566644,  0.050399,  0.031680,  0.043889,  0.089861,  0.094004,  0.095023,  0.006753,  0.005984,  0.006450,  0.388463,  0.698664,  0.543841, -0.255346,
4281.878906, 51.198986,154.974197,-148.014648, 63.655556, 48.214287,-122.450485, 24.314480,  0.000500, -0.001049,  0.018677, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.565533,  0.565589,  0.725463,  0.580195,  0.581910,  0.568373,  0.050403,  0.031684,  0.043893,  0.089862,  0.094004,  0.095023,  0.006753,  0.005984,  0.006450,  0.388463,  0.698665,  0.543840, -0.255346,
4281.880859, 51.199074,154.974121,-148.014725, 63.655476, 48.214287,-122.450485, 24.314444,  0.000492, -0.001100,  0.018650, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.565800,  0.565857,  0.725668,  0.582031,  0.583761,  0.570097,  0.050407,  0.031687,  0.043897,  0.089862,  0.094005,  0.095023,  0.006754,  0.005985,  0.006450,  0.388464,  0.698664,  0.543840, -0.255347,
4281.883301, 51.199150,154.973999,-148.014893, 63.655319, 48.214287,-122.450485, 24.314407,  0.000492, -0.001152,  0.018625, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.566069,  0.566128,  0.725875,  0.583861,  0.585607,  0.571815,  0.050411,  0.031691,  0.043900,  0.089863,  0.094005,  0.095023,  0.006754,  0.005985,  0.006451,  0.388465,  0.698664,  0.543839, -0.255347,
4281.885254, 51.199158,154.973892,-148.014938, 63.655270, 48.214287,-122.450485, 24.314371,  0.000501, -0.001197,  0.018608, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.566341,  0.566400,  0.726084,  0.585687,  0.587449,  0.573529,  0.050415,  0.031694,  0.043904,  0.089863,  0.094005,  0.095023,  0.006754,  0.005985,  0.006451,  0.388465,  0.698664,  0.543839, -0.255347,
4281.887207, 51.199169,154.973816,-148.014832, 63.655357, 48.214287,-122.450485, 24.314335,  0.000505, -0.001225,  0.018614, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.566615,  0.566675,  0.726294,  0.587508,  0.589285,  0.575237,  0.050419,  0.031698,  0.043908,  0.089864,  0.094005,  0.095024,  0.006755,  0.005986,  0.006452,  0.388466,  0.698664,  0.543840, -0.255347,
4281.889160, 51.199177,154.973648,-148.014908, 63.655296, 48.214287,-122.450485, 24.314299,  0.000505, -0.001278,  0.018624, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.566891,  0.566953,  0.726505,  0.589324,  0.591118,  0.576941,  0.050423,  0.031701,  0.043911,  0.089864,  0.094006,  0.095024,  0.006755,  0.005986,  0.006452,  0.388467,  0.698663,  0.543840, -0.255346,
4281.891113, 51.199162,154.973572,-148.015015, 63.655186, 48.214287,-122.450485, 24.314262,  0.000509, -0.001341,  0.018616, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.567169,  0.567232,  0.726719,  0.591135,  0.592945,  0.578639,  0.050427,  0.031705,  0.043915,  0.089864,  0.094006,  0.095024,  0.006755,  0.005987,  0.006452,  0.388467,  0.698664,  0.543839, -0.255346,
4281.893066, 51.199223,154.973557,-148.014999, 63.655209, 48.214287,-122.450485, 24.314217,  0.000531, -0.001399,  0.018598, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.567520,  0.567585,  0.726988,  0.593393,  0.595223,  0.580755,  0.050432,  0.031709,  0.043920,  0.089865,  0.094006,  0.095024,  0.006756,  0.005987,  0.006453,  0.388467,  0.698663,  0.543840, -0.255346,
4281.895020, 51.199322,154.973541,-148.014938, 63.655254, 48.214287,-122.450485, 24.314180,  0.000554, -0.001444,  0.018598, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.567803,  0.567870,  0.727206,  0.595195,  0.597041,  0.582442,  0.050436,  0.031713,  0.043923,  0.089865,  0.094006,  0.095024,  0.006756,  0.005988,  0.006453,  0.388468,  0.698663,  0.543840, -0.255347,
4281.896973, 51.199348,154.973511,-148.014908, 63.655296, 48.214287,-122.450485, 24.314154,  0.000558, -0.001476,  0.018595, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.568017,  0.568085,  0.727370,  0.596543,  0.598401,  0.583705,  0.050439,  0.031715,  0.043926,  0.089866,  0.094006,  0.095025,  0.006756,  0.005988,  0.006453,  0.388468,  0.698662,  0.543840, -0.255347,
4281.898926, 51.199276,154.973480,-148.014801, 63.655399, 48.214287,-122.450485, 24.314108,  0.000561, -0.001527,  0.018605, -0.000355, -0.000007, -0.000172, -1.642543,  2.729327,  1.959170,  0.568377,  0.568446,  0.727645,  0.598784,  0.600663,  0.585803,  0.050444,  0.031720,  0.043931,  0.089866,  0.094007,  0.095025,  0.006757,  0.005988,  0.006454,  0.388468,  0.698662,  0.543841, -0.255346,
4281.900879, 51.244141,154.969254,-148.046585, 63.623619, 48.214287,-122.450485, 24.315016, -0.034192, -0.049254,  0.025625, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.548783,  0.548788,  0.712479,  0.384042,  0.384560,  0.380700,  0.050034,  0.031371,  0.043547,  0.089824,  0.093982,  0.095005,  0.006724,  0.005946,  0.006417,  0.388574,  0.698618,  0.543634, -0.255745,
4281.903809, 51.243999,154.969025,-148.046707, 63.623497, 48.214287,-122.450485, 24.314939, -0.034184, -0.049337,  0.025641, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.548963,  0.548969,  0.712621,  0.387989,  0.388522,  0.384537,  0.050040,  0.031376,  0.043553,  0.089825,  0.093983,  0.095006,  0.006724,  0.005947,  0.006418,  0.388574,  0.698618,  0.543634, -0.255744,
4281.905273, 51.243984,154.968933,-148.046768, 63.623428, 48.214287,-122.450485, 24.314915, -0.034181, -0.049354,  0.025631, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549024,  0.549029,  0.712668,  0.389296,  0.389835,  0.385807,  0.050042,  0.031378,  0.043555,  0.089825,  0.093983,  0.095006,  0.006725,  0.005947,  0.006418,  0.388575,  0.698618,  0.543634, -0.255743,
4281.907227, 51.243965,154.968689,-148.046951, 63.623249, 48.214287,-122.450485, 24.314865, -0.034172, -0.049382,  0.025603, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549146,  0.549151,  0.712764,  0.391899,  0.392449,  0.388336,  0.050046,  0.031381,  0.043558,  0.089825,  0.093983,  0.095006,  0.006725,  0.005947,  0.006418,  0.388576,  0.698618,  0.543633, -0.255743,
4281.909180, 51.243935,154.968430,-148.047134, 63.623074, 48.214287,-122.450485, 24.314816, -0.034185, -0.049426,  0.025579, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549269,  0.549275,  0.712861,  0.394487,  0.395047,  0.390848,  0.050049,  0.031385,  0.043562,  0.089826,  0.093983,  0.095006,  0.006725,  0.005948,  0.006419,  0.388577,  0.698618,  0.543633, -0.255742,
4281.911133, 51.243912,154.968185,-148.047318, 63.622879, 48.214287,-122.450485, 24.314764, -0.034200, -0.049448,  0.025571, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549393,  0.549399,  0.712958,  0.397058,  0.397630,  0.393344,  0.050053,  0.031389,  0.043566,  0.089826,  0.093983,  0.095006,  0.006726,  0.005948,  0.006419,  0.388578,  0.698618,  0.543632, -0.255741,
4281.913086, 51.243900,154.968033,-148.047516, 63.622696, 48.214287,-122.450485, 24.314714, -0.034222, -0.049463,  0.025553, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549519,  0.549525,  0.713057,  0.399615,  0.400197,  0.395825,  0.050057,  0.031392,  0.043569,  0.089826,  0.093984,  0.095006,  0.006726,  0.005949,  0.006420,  0.388578,  0.698619,  0.543632, -0.255741,
4281.915039, 51.243893,154.967743,-148.047867, 63.622326, 48.214287,-122.450485, 24.314653, -0.034260, -0.049480,  0.025528, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549677,  0.549683,  0.713181,  0.402790,  0.403386,  0.398904,  0.050062,  0.031397,  0.043574,  0.089827,  0.093984,  0.095007,  0.006726,  0.005949,  0.006420,  0.388579,  0.698620,  0.543630, -0.255741,
4281.916992, 51.243881,154.967484,-148.048172, 63.622028, 48.214287,-122.450485, 24.314602, -0.034281, -0.049503,  0.025505, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549804,  0.549811,  0.713281,  0.405313,  0.405920,  0.401350,  0.050066,  0.031400,  0.043577,  0.089827,  0.093984,  0.095007,  0.006727,  0.005950,  0.006420,  0.388580,  0.698620,  0.543629, -0.255741,
4281.911133, 51.243912,154.968185,-148.047318, 63.622879, 48.214287,-122.450485, 24.314764, -0.034200, -0.049448,  0.025571, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549393,  0.549399,  0.712958,  0.397058,  0.397630,  0.393344,  0.050053,  0.031389,  0.043566,  0.089826,  0.093983,  0.095006,  0.006726,  0.005948,  0.006419,  0.388578,  0.698618,  0.543632, -0.255741,
4281.913086, 51.243900,154.968033,-148.047516, 63.622696, 48.214287,-122.450485, 24.314714, -0.034222, -0.049463,  0.025553, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549519,  0.549525,  0.713057,  0.399615,  0.400197,  0.395825,  0.050057,  0.031392,  0.043569,  0.089826,  0.093984,  0.095006,  0.006726,  0.005949,  0.006420,  0.388578,  0.698619,  0.543632, -0.255741,
4281.915039, 51.243893,154.967743,-148.047867, 63.622326, 48.214287,-122.450485, 24.314653, -0.034260, -0.049480,  0.025528, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549677,  0.549683,  0.713181,  0.402790,  0.403386,  0.398904,  0.050062,  0.031397,  0.043574,  0.089827,  0.093984,  0.095007,  0.006726,  0.005949,  0.006420,  0.388579,  0.698620,  0.543630, -0.255741,
4281.916992, 51.243881,154.967484,-148.048172, 63.622028, 48.214287,-122.450485, 24.314602, -0.034281, -0.049503,  0.025505, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549804,  0.549811,  0.713281,  0.405313,  0.405920,  0.401350,  0.050066,  0.031400,  0.043577,  0.089827,  0.093984,  0.095007,  0.006727,  0.005950,  0.006420,  0.388580,  0.698620,  0.543629, -0.255741,
4281.911133, 51.243912,154.968185,-148.047318, 63.622879, 48.214287,-122.450485, 24.314764, -0.034200, -0.049448,  0.025571, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549393,  0.549399,  0.712958,  0.397058,  0.397630,  0.393344,  0.050053,  0.031389,  0.043566,  0.089826,  0.093983,  0.095006,  0.006726,  0.005948,  0.006419,  0.388578,  0.698618,  0.543632, -0.255741,
4281.913086, 51.243900,154.968033,-148.047516, 63.622696, 48.214287,-122.450485, 24.314714, -0.034222, -0.049463,  0.025553, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549519,  0.549525,  0.713057,  0.399615,  0.400197,  0.395825,  0.050057,  0.031392,  0.043569,  0.089826,  0.093984,  0.095006,  0.006726,  0.005949,  0.006420,  0.388578,  0.698619,  0.543632, -0.255741,
4281.915039, 51.243893,154.967743,-148.047867, 63.622326, 48.214287,-122.450485, 24.314653, -0.034260, -0.049480,  0.025528, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549677,  0.549683,  0.713181,  0.402790,  0.403386,  0.398904,  0.050062,  0.031397,  0.043574,  0.089827,  0.093984,  0.095007,  0.006726,  0.005949,  0.006420,  0.388579,  0.698620,  0.543630, -0.255741,
4281.916992, 51.243881,154.967484,-148.048172, 63.622028, 48.214287,-122.450485, 24.314602, -0.034281, -0.049503,  0.025505, -0.000365,  0.000062, -0.000151, -1.642408,  2.729473,  1.959144,  0.549804,  0.549811,  0.713281,  0.405313,  0.405920,  0.401350,  0.050066,  0.031400,  0.043577,  0.089827,  0.093984,  0.095007,  0.006727,  0.005950,  0.006420,  0.388580,  0.698620,  0.543629, -0.255741,

Slave at START of TyComm buffer:

Code:

3818.854492, 50.908184,154.948166,-147.808014, 63.862183, 48.214344,-122.450348, 38.209553, -0.027077,  0.012324,  0.028511, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.561699,  0.561739,  0.722489,  0.551630,  0.553113,  0.541443,  0.050278,  0.031663,  0.043886,  0.089855,  0.093997,  0.095026,  0.006753,  0.005975,  0.006444,  0.388100,  0.698763,  0.545300, -0.252499,
3818.856445, 50.908062,154.948029,-147.808121, 63.862076, 48.214344,-122.450348, 38.209503, -0.027046,  0.012394,  0.028514, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.561904,  0.561945,  0.722647,  0.553306,  0.554801,  0.543025,  0.050281,  0.031667,  0.043889,  0.089855,  0.093997,  0.095026,  0.006753,  0.005975,  0.006444,  0.388101,  0.698764,  0.545300, -0.252498,
3818.858154, 50.907978,154.947769,-147.808273, 63.861923, 48.214344,-122.450348, 38.209450, -0.027011,  0.012474,  0.028517, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.562141,  0.562183,  0.722830,  0.555215,  0.556725,  0.544828,  0.050285,  0.031670,  0.043893,  0.089855,  0.093997,  0.095026,  0.006753,  0.005976,  0.006445,  0.388102,  0.698764,  0.545300, -0.252497,
3818.860352, 50.907860,154.947617,-147.808380, 63.861828, 48.214344,-122.450348, 38.209385, -0.026983,  0.012591,  0.028536, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.562410,  0.562453,  0.723037,  0.557357,  0.558883,  0.546850,  0.050289,  0.031674,  0.043897,  0.089856,  0.093997,  0.095026,  0.006754,  0.005976,  0.006445,  0.388102,  0.698764,  0.545299, -0.252495,
3818.862305, 50.907806,154.947449,-147.808502, 63.861683, 48.214344,-122.450348, 38.209332, -0.026961,  0.012690,  0.028560, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.562651,  0.562695,  0.723223,  0.559255,  0.560795,  0.548640,  0.050293,  0.031678,  0.043901,  0.089856,  0.093998,  0.095026,  0.006754,  0.005977,  0.006445,  0.388102,  0.698765,  0.545299, -0.252495,
3818.864258, 50.907745,154.947266,-147.808670, 63.861519, 48.214344,-122.450348, 38.209274, -0.026944,  0.012787,  0.028569, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.562895,  0.562940,  0.723410,  0.561147,  0.562702,  0.550425,  0.050297,  0.031681,  0.043904,  0.089857,  0.093998,  0.095027,  0.006754,  0.005977,  0.006446,  0.388103,  0.698765,  0.545298, -0.252494,
3818.866455, 50.907692,154.947083,-147.808762, 63.861454, 48.214344,-122.450348, 38.209217, -0.026922,  0.012859,  0.028549, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.563140,  0.563186,  0.723598,  0.563035,  0.564604,  0.552204,  0.050301,  0.031685,  0.043908,  0.089857,  0.093998,  0.095027,  0.006755,  0.005978,  0.006446,  0.388104,  0.698765,  0.545298, -0.252493,
3818.868164, 50.907616,154.946884,-147.808884, 63.861324, 48.214344,-122.450348, 38.209164, -0.026900,  0.012924,  0.028524, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.563387,  0.563434,  0.723789,  0.564917,  0.566501,  0.553978,  0.050305,  0.031689,  0.043912,  0.089858,  0.093998,  0.095027,  0.006755,  0.005978,  0.006446,  0.388104,  0.698765,  0.545298, -0.252492,
3818.870361, 50.907574,154.946716,-147.809052, 63.861145, 48.214344,-122.450348, 38.209099, -0.026863,  0.012978,  0.028486, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.563668,  0.563716,  0.724005,  0.567028,  0.568629,  0.555967,  0.050309,  0.031693,  0.043916,  0.089858,  0.093999,  0.095027,  0.006755,  0.005978,  0.006447,  0.388105,  0.698766,  0.545297, -0.252491,
3818.880127, 50.907673,154.945770,-147.809601, 63.860600, 48.214344,-122.450348, 38.208824, -0.026637,  0.013120,  0.028306, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.564949,  0.565003,  0.724989,  0.576331,  0.578011,  0.564720,  0.050329,  0.031710,  0.043934,  0.089860,  0.094000,  0.095028,  0.006757,  0.005981,  0.006449,  0.388110,  0.698765,  0.545296, -0.252489,
3818.882324, 50.907593,154.945618,-147.809616, 63.860584, 48.214344,-122.450348, 38.208759, -0.026569,  0.013202,  0.028287, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.565244,  0.565299,  0.725216,  0.578407,  0.580104,  0.566672,  0.050333,  0.031714,  0.043938,  0.089861,  0.094000,  0.095028,  0.006757,  0.005981,  0.006449,  0.388110,  0.698765,  0.545296, -0.252488,
3818.884277, 50.907555,154.945557,-147.809631, 63.860573, 48.214344,-122.450348, 38.208706, -0.026521,  0.013279,  0.028280, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.565509,  0.565566,  0.725420,  0.580247,  0.581960,  0.568400,  0.050337,  0.031718,  0.043942,  0.089861,  0.094000,  0.095029,  0.006758,  0.005981,  0.006449,  0.388111,  0.698765,  0.545296, -0.252488,
3818.886475, 50.907459,154.945465,-147.809769, 63.860428, 48.214344,-122.450348, 38.208649, -0.026481,  0.013357,  0.028270, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.565776,  0.565834,  0.725625,  0.582083,  0.583811,  0.570124,  0.050341,  0.031722,  0.043946,  0.089861,  0.094001,  0.095029,  0.006758,  0.005982,  0.006450,  0.388111,  0.698765,  0.545296, -0.252487,
3818.888184, 50.907349,154.945358,-147.809967, 63.860229, 48.214344,-122.450348, 38.208595, -0.026457,  0.013432,  0.028278, -0.000266, -0.000464, -0.000517, -1.643200,  2.729435,  1.958753,  0.566046,  0.566105,  0.725832,  0.583913,  0.585657,  0.571842,  0.050345,  0.031725,  0.043950,  0.089862,  0.094001,  0.095029,  0.006758,  0.005982,  0.006450,  0.388110,  0.698766,  0.545295, -0.252486,

tonton81 · Apr 2, 2018

sure thing, you mean MINimum value in entire array and MAXimum value?

Why stop there, how about Median? :O

Don, please don't use 30, it must be a power of 2 the buffer or else undefined behaviour will occur with library
Use 32 instead if you want 30 items...

mjs513 · Apr 2, 2018

Hi Tim. Busy shoveling more snow this morning. Anyway I don't see the NaN. Which means it failed at the rollover? Probably my eyes.

Anyway, around 4282 or so was where the rollover occurred for me and I had the problem - once fixed rtcTime should start recounting. I don't see that occurring. Did you load the v3 Master?

tonton81 · Apr 2, 2018

Tim, how's this?

Code:

 Circular_Buffer<float, 16> myFloats;
  myFloats.push_back(3.14159);
  myFloats.push_back(12.3456);
  myFloats.push_back(78.91234);
  myFloats.push_back(7.91234);
  myFloats.push_back(11.91234);
  myFloats.push_back(58.91234);
  myFloats.push_back(18.91234);

  Serial.print("MIN: "); Serial.println(myFloats.min(), 5);
  Serial.print("MAX: "); Serial.println(myFloats.max(), 5);

Output:

Code:

MIN: 3.14159
MAX: 78.91234

https://github.com/tonton81/Circular_Buffer/commit/a62f2858c17d227e386363608c55d13cfff58904

defragster · Apr 2, 2018

tonton81 - MIN & MAX looks good.

mjs513 said:
Hi Tim. Busy shoveling more snow this morning. Anyway I don't see the NaN. Which means it failed at the rollover? Probably my eyes.

Anyway, around 4282 or so was where the rollover occurred for me and I had the problem - once fixed rtcTime should start recounting. I don't see that occurring. Did you load the v3 Master?

Running the last ZIP post that has this sketch name: ...\1Apr18\uNavINS_MPU9250_I2C_MAGV3\uNavINS_MPU9250_I2C_MAGV3.ino

Not sure where the rollover recounting code is actualized? This is the only place I see it being set.

Code:

  //Define RTC time for this iterationrt
  rtcTime = (float) micros() / 1000000.0 - rtcOffset;

mjs513 · Apr 2, 2018

Ok. I have something weird going on. I just looked in the v3 file and code for rollover correction is not there and I know I saved the file. Anyway, here's the fix:
In the uNAVINS.h tab delete (its about line 26)

Code:

    // timing
    float tnow, tprev, dt;

and add the following:

Code:

    // timing
    elapsedMicros _t;
    float dt;

In the .cpp delete (about line 188):

Code:

    // get the change in time
    tnow = (float) micros()/1000000.0f;
    dt = tnow - tprev;
    tprev = tnow;

and substitute this:

Code:

      // get the change in time
      dt = (float)_t /1000000.0;
      _t = 0;

That was the whole fix that was extracted from Brian's EKF code.

Really don't know what happened. All the other changes are there.
Mike

mjs513 · Apr 2, 2018

uNavINS_MPU9250_I2C_MAGV4

Here's the updated sketch with those corrections. Hopefully I did it right

defragster · Apr 2, 2018

T:\TEMP\arduino_build_439128\sketch\uNavINS.cpp: In member function 'void uNavINS::update(long unsigned int, double, double, double, double, double, double, float, float, float, float, float, float, float, float, float)':

uNavINS.cpp:51: error: 'tprev' was not declared in this scope
tprev = (float) micros()/1000000.0f;

edit:
// timing
elapsedMicros _t;
float dt, tprev;

Don Kelly · Apr 2, 2018

Mike,
I'm getting this error with V4 now:

IMU initialization unsuccessful
Check IMU wiring or try cycling power
Status: -1

I tried recycling power, so that's not it. Do I need to back off the IMU speed or something like that?
Don

Tim, I added "float tprev" to the h file and that fixed the compile error...
Don

Don Kelly · Apr 2, 2018

Mike,
I set IMU_SPD back to 1000000 and got to this now:

Setup Complete
-----Loop#:1 IMU#:0
#1 @18:8.34_800338219 [fix:3 #:18
ASSERT: NaN Fail index = 28
___ ASSERT FAILED ___ FILE, LINE#, Expression
C:\Users\User\Desktop\uNavINS_MPU9250_I2C_MAGV4\uNavINS_MPU9250_I2C_MAGV4\C_serialPrint.ino
64
!std::isnan( MST_PrintVals[jj] )
___ ASSERT FAILED ___ STOPPING !!!!

defragster · Apr 2, 2018

Me Too:

Gyro Calibration Underway
Gyro Calibration Complete

Setup Complete
ASSERT: NaN Fail index = 28
___ ASSERT FAILED ___ FILE, LINE#, Expression
T:\tCode\_GPSimuDK\1Apr18\uNavINS_MPU9250_I2C_MAGV4\C_serialPrint.ino
64
!std::isnan( MST_PrintVals[jj] )
___ ASSERT FAILED ___ STOPPING !!!!

Code:

// ...
  0.339668, 72.472931, 49.413803, 53.493053,265.491638, 48.214333,-122.450325, 42.571877,  1.386992,  1.362751,  4.127507, -0.003696, -0.002071,  0.002641,  8.298505,  0.977089, 14.375739,  2.944455,  2.948416,  4.763172,  0.905949,  0.880890,  1.001549,  0.167624,  0.109688,  0.222566,  0.684927,  0.816811,  0.112641,  0.012540,  0.013869,  0.013555,  0.315907,  0.623191,  0.116911, -0.705810,
  0.365640, 72.479248, 49.412224, 53.487080,265.485657, 48.214336,-122.450325, 42.461033,  1.511060,  1.485580,  4.434150, -0.003696, -0.002071,  0.002641,  8.298505,  0.977089, 14.375739,  2.945716,  2.949865,  4.765454,  0.940240,  0.912364,  1.047434,  0.167710,  0.109847,  0.222593,  0.684753,  0.816603,  0.112635,  0.012538,  0.013865,  0.013552,  0.315844,  0.623213,  0.116925, -0.705816,

mjs513 · Apr 2, 2018

Ok.... give me a couple of minutes and will retest....

mjs513 · Apr 2, 2018

Got it, something really dumb on my part - forgot to set _t=0 at the end of the initialization loop. Also, Don - you don't need Tprev at all. Here is the updated v4.

defragster · Apr 2, 2018

Just copied all but config.h over the prior - it compiled and it running - coming up on a minute.

I wondered about tprev when tnow was gone ...

<edit>: Had to set coutD to Serial ...

It shows :: -----Loop#:359573 IMU#:500

on T_3.6 @ 180 MHz
This means after the 500 IMU updates are caught and processed loop() is passed through another 359,000 times per second every 2.8 uS looking for something to do.

New code ran 363 seconds no trouble - I just stopped it for another restart with SRD at 3 for 250 Hz IMU

It shows :: -----Loop#:528144 IMU#:250

Mike - can you show that Loop# on your T_3.5 for reference

uNav INS

Well-known member

Well-known member

Senior Member+

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Well-known member

Senior Member+

Senior Member+

Well-known member

Senior Member+

Well-known member

Senior Member+

Senior Member+

Senior Member+

Attachments

Senior Member+

Well-known member

Well-known member

Senior Member+

Senior Member+

Senior Member+

Attachments

Senior Member+