uNav INS

on the other hand, this mine be a ONE LINE patch! can you do a quick test for me? im not home
try this
make your queue to ascending order, (i hope c++ std:: orders them right) via CB.sort_ascending();
then get the first and last value:
CB.peek();
CB.peek(CB.size()-1);

results??
if ascending works for the floats i can have the library patched quickly in a few hours
 
if that works its just a matter of printing the tail-1 after calling ascending, simple library fix using already implemented methods :)

edit, if that works, i might use it for .min() as well, to be safe both ways
 
it’s also my fault, i started search at 0...
Code:
T _find = 0;
I should have started searches from endpoints...

Also if you got a small snippet of just the CB to test against that would be nice, while i patch the lib i can test against it
 
Hi all. Decided to try an old approach for accel and magnetometer that is reminiscent of what is done for the propshield. I am attaching two images - you might say accel and mag data before calibration and one after calibration.
Capture.PNG Capture1.PNG

It is based on the raw counts to generate the plots and the offsets and scales. Here is the cal values generated by the sketch:
Code:
Accel axes offset/scale factors:
0.003418, 0.998897, 
0.008789, 1.005023, 
-0.041993, 1.001877
Magnetometer axes offset/scale factors:
8.244299, 1.014075, 
-0.537672, 1.002969, 
-30.582275, 0.982956

The interesting thing is that roll still shows at 30 degrees. Yaw has stabilized quite a bit.. varying around +/- 1 deg and strays as velN varies up and down.

Don - I have to dig into the code but guessing is that ypr is linked to GPS in EKF integration.
 
tonton81,
Tried this, but got error of 'class Circular_Buffer has no member named sort_ascenting'

cbaZ.sort_ascending();
Serial.println(" ");
Serial.print(" val1 =: "); Serial.println(cbaZ.peek());
Serial.print(" val2 =: "); Serial.println(cbaZ.size()-1);
Serial.println(" ");

I'm out of pocket next several hours....
Don
 
Mike,
Looks interesting, is that in c?

I'm getting what seem to be really good results now. tonton81 is going to fix the circular-buffer max issue, but knowing what the issue is, I'm seeing what seem to be good cal values.

The scale factor on the magnetometer, as calculated in the MPU9250 Library, seems to grab a bit tighter heading numbers than my static cal heading numbers. Especially when I rotate the IMU to 90 or 270 degrees. At 90 and 270 degrees, the static cal numbers seem to be off by ~5-10 degrees or so.

Another thing I noticed on the mag heading readings is that when I pitch up while pointed North (0 deg heading), the heading increases, almost one-to-one for degrees. So if I point a level IMU North, I get 0 deg heading, when I then pitch up 45 deg, I get heading of ~45 deg. I need to dig into the EKF code, as I noticed some sort of pitch/roll compensation for heading I think.

I'm going to do a bit more testing outside on the mag cal, and then I might go back to the EKF (w/out mag) to see how things look.

Don
 
Don

Its written in Python but I converted it a windows executable file. But there is a matlab equivalent once you have the accel and mag data files if you are interested.

Still have play around with it some more because I seem to be getting some strange results when I move the sensor.

Mike
 
Nice! I use both Python and Matlab. I'm really best on Matlab, but have been frustrated the last few years with their outlandish pricing for individuals, so have heavily migrated to Python recently except for few things. Had just started mixing Python with Arduino stuff until I got involved with this EKF effort!

Don
 
Hi Don
If you want the files I will zip them up and post the link. Since I am a hobbyist I got matlab for free - imagine that. Just don't get the extras :)

By the way did some more testing and here are a couple of notes that you might be interested in:
  • Yaw rotation - when I move it from my starting position (around heading of 280 degres) and align to north heading takes way to long to reach zero degrees (especially if I move too fast or overshoot and try and bring back). Update rates have to be increased some how - if I change GPS to 10 hz it might be better.
  • Once I get it to zero (heading) roll decreases to a couple of degrees off zero.
  • Once I get heading aligned to north and try and do a pitch or roll for that matter yaw starts oscillating like crazy as well as all the axes.
  • There appears to be some weird coupling going on. This occurred for my calibration or the libraries calibration.
Like I said with the static cal it just hangs the sketch.

Good luck. Don't know enough about the EKF to be of any help here.

Mike
 
Mike: stationary GPS is no help for heading Don will confirm.

As far as the 'weird coupling' - indeed this may be what was noted on the PJRC PropShield thread in beta. Not sure if it offers an clues?

I ran into it here page #17: Prop-Shield-Beta-Test
 
Tim. You are right - with out movement GPS is no good for heading. If its moving you can actually read it from the COG variable in the PVT message. But it VelN, VelE, VelD are used while stationary you are going to get some erroreous readings. Since when stationary they are not zero. Usually less than (abs) 0.1 m/s.

I have seen the weird coupling before but I can't remember what happened. Think is was calculating Euler angles incorrectly - have to see the quaternions to tell you if something is up. Since they drive the angles.

Don is qw your q0 or q3?
 
Don - Tim
Just did a test displaying the quaternions as well as the YPR values. While I said the YPR values were strange so are the quaternions during and after movement.

Before I pitched I rotated to about 0 heading. Ok so far so good. I then changed pitch and, well see for your self:
Capture.jpg
 
Ok just to confirm, .max() should be closer to 0 right? and .min() is further back?

Code:
.min(): -78.91
.max(): -3.14
-78.91234 -58.91234 -18.91234 -12.34560 -11.91234 -7.91234 -3.14159

min max correct?
 
Is this correct max being closer to 0???

sketch snippet:
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("SUM: "); Serial.println(myFloats.sum(), 5);
  Serial.print("MIN: "); Serial.println(myFloats.min(), 5);
  Serial.print("MAX: "); Serial.println(myFloats.max(), 5);
  Serial.print("MEDIAN: "); Serial.println(myFloats.median(1), 5);
  Serial.print("AVG: "); Serial.println(myFloats.average(), 5);
  Serial.print("Deviation: "); Serial.println(myFloats.deviation(), 8);
  uint8_t _size = myFloats.size();
  for ( uint8_t i = 0; i < _size; i++ ) {
    Serial.print(myFloats.pop_front(), 5); Serial.print(" ");
  } Serial.println();

Output:
Code:
SUM: -192.04887
[COLOR="#FF0000"]MIN: -78.91234
MAX: -3.14159[/COLOR]
MEDIAN: -12.34560
AVG: -27.43555
Deviation: 27.13280106
-78.91234 -58.91234 -18.91234 -12.34560 -11.91234 -7.91234 -3.14159

I also opted to not sort the original buffer, because if your constantly keeping the latest values, it shouldnt be sorted. min() and max() memmove the buffer and do a sort, returns the value, leaving your queue as-is

if you want it sorted, you can call sort_ascending or sort_descending, otherwise the other calls wont touch your buffer
 
Mike,

What filter are you running for your plots above? One of the AHRS variants?

By qw, if you mean the term that usually 1.0, it's q0 in Brian's filter I believe.

Don
 
Don,
Yep - qw is usually 1 to start with., or q0 in Brian's filter.

"What filter are you running for your plots above? One of the AHRS variants" - version 6 of your filter. Using the library calibration from your calibration routine.

Mike
 
Tim. You are right - with out movement GPS is no good for heading. If its moving you can actually read it from the COG variable in the PVT message. But it VelN, VelE, VelD are used while stationary you are going to get some erroreous readings. Since when stationary they are not zero. Usually less than (abs) 0.1 m/s.

I have seen the weird coupling before but I can't remember what happened. Think is was calculating Euler angles incorrectly - have to see the quaternions to tell you if something is up. Since they drive the angles.

Don is qw your q0 or q3?

Here was a post today on onehorse github: If there is a Gimbal lock in RPY(Roll, Pitch, Yaw)? issues/267

Yeah - some time back my stationary unit was showing crazy heading updates - and Don made note for me.
 
Hi Tim. Yep experienced gimbal lock before but I don't think this is the same behavior. Gimbal lock usually occurs at the crossover points, for instance pitch around 90degrees. But as Onehorse put it, gimbal lock should occur when using quaternions, that is one advantage of using quaternions.

As a test I went back to uNavINS_CB_Ver4 of the code and YPR behaved very nicely when rotating in pitch and roll as well yaw. So the question becomes what changed between version 4 and version 6. I know in the main sketch I added all your timing changes to match GPS with IMU as well as dt changes. I don't know what changed in the KF code block.

One of the other tests I did with version 6 was that if the velocities are below 0.1m/s I forced them to zero and fed them into the filter. YPR - no change when rotating slowly. So confirms that YPR is dependent on the velocities and if you loose GPS you loose your attitude. For a practical filter don't think you want that to happen.

Mike
 
The posted description as I scanned it made it seems like it had some element of it so mentioned it.

As far as 'dt' - think using a constant for that would be best - of course that constant changes with SRD selection. Though the avg of the dt's was good - missed updates gave bad ones. Maybe in setup while the isr() is running the IMU dt could be observed and then that value used as a constant? The change I made to detect missed updates , i.e. DOUBLE, could still be used to do the right thing - which is a tough call - discarding data as bad or worse than applying 'current' data across multiple dt's it may not represent? So best to keep the system able to catch and handle each IMU update.
 
Tim - my post was just trying to document the delta's I incorporated between version 4 and 6. I agree that using a constant dt (depending on SRD) is the way to go. I don't think the changes made to detect misses would cause the problem - I really should do a code compare of the uNavAHRS code to check but I figured Don would know better what changed between updates.

Mike
 
Back
Top