uNav AHRS

Status
Not open for further replies.
Don. Thanks for the update. Looking forward to see what you come up with. Been busy last night and today. I went back and try to do my own port from the original miconav_ahrs.c to Eigen like Brian did. Took take approach, figured it would force me to go line by line, couldn't get it working so I went back to Brian's port and did a couple tweaks for experimenting with since I got a better understanding (a little bit at a time). Got it working so yaw would vary +/- 0.5 degrees from where I put - very noisy but by controlling the quat variance in aQ got better.

Anyway one of the things I noticed when I started was that it would start drifting on me when I swiveled in my desk chair so that the back was about a foot away, when I rotated the back away it would stabilize back to where it started. Yes in all drift testing the back of my chair was a bit towards the 9250. I ran a couple of tests and I am posting the recent one just to give you an idea. At about the 28 minute mark I came back in the room and brought the chair closer. It started drifting upwards and after that it decreased further, probably because of I was oriented in the chair. testunav.jpg

I will leave the conclusion to you all. I am going to move on to next version that Brian created where he rederived the equations. Going to see if I can put in a different IMU as well.

Cheers
Mike
 
The sensitivity of the magnetometers to external metal and magnetic fields makes AHRS a bear! Unless the device is out in open space I suppose...

This would tend to make me think that keeping PR and Y in separate filters is likely the most robust method for AHRS. Another thing I've been thinking about is using GPS for heading, either once the unit is in motion or if two antennas (and possibly two GPS units) are used. Also assumes the GPS is in an accurate enough (DGPS or RTK) mode. I think we'll start seeing heading estimated this way soon.
 
For indoor environments GPS is a no-go which is the environment I usually work in. As for the sensitivity of the magnetometer, yes it is a bear but with the MARG filters not so much so. Can get pretty stable yaw readings.
 
Yay, success... Got routine that puts out accel in NED-inertial (with gravity removed). Turns out I had made a couple dumb mistakes in the DCM. Will post once I get it cleaned up, maybe later tonight.

So I believe I'm ready to integrate this (accel in NED-inertial) with GPS position (LLA converted to NED-inertial) and velocity (NED-inertial). I'm guessing the accel values will need smoothing, not sure yet. So now I need to go back and look at making sure I can accurately time-tag the IMU values relative to GPS, and then remember how I get both posn and vel readings from the uBlox. Any help/suggestions appreciated!
 
Did anyone ever see my GPS Serial# Rx isr() code to catch the GPS start bit? I think I never got it posted. Been busy with stuff past weeks and reading the forum but not actually looking at code since the Filters evolved.

My Teensy IMU/GPS is sitting here under a pile beside me . . . it does have a FIX though!

If GPS is about to be used I think getting the start bit will be a good uniform reference point. My code is tied to having the RTC ( or GPS PPS ) running and tracking the cycle count - if I get a pointer to a ready to use sketch that reads IMU and GPS data - I can post it back with the isr() active to record the start bit - based on Teensy RTC. I could modify it to run with micros() - but I think that is more compute intensive and lower resolution than cycle counts.

And if the IMU isr() uses the same clock base named PartSec() it could add some finer resolution and then the timestamps would be comparable.

Ideally it could be a Header.h file like a library so it can be added to any sketch. The problem is the int pins and serial ports ( and pins ) vary and the setup varies with that.

The other question I had was should it return a FLOAT or a uint32_t - or both in a struct? if a float then it messes up comparisons. But also may need a 'tick' count of elapsed seconds as it is reset to 0 each second?
Code: 
uint32_t PPS_PartSec() { // 4J18
  return 1000000 * (((int32_t)ARM_DWT_CYCCNT - PPS_cCntPri) / (float)PPS_cAVG);
}

That is the code I came up with - where PPS_cAVG is the last 8 second average of cycle counts per 'second' ( from PPS or RTC ). The cycles per second changes over time as the non temp corrected crystal responds to the environment - this silently affects micros() as well and may wash out as all time is relative measurement to measurement and I'm not sure measurements made over 5 seconds before affect ongoing calculations with some skew in the clock?
 
Don't think I saw the GPS isr code, so an example would be great.

The next thing I'm planning over the next couple of days is to modify a sketch to capture the following data:
- From IMU (~100Hz): pitch, roll, heading, accel NED-inertial
- From GPS (either 1 or 5Hz): UTC, position (LLA), velocity (NED-inertial), fix

That way I can drive my car around the 'hood and capture a block of data and then process it in either Matlab or Python to test out my "simple KF approach." For the simple approach, I'll use accel NED-inertial values as control inputs for the KF propagation step, and GPS position and velocity as measurements for the KF update step. Once I'm able to process through the data in Matlab, I'll quickly port it back to arduino code.

To capture the code, I'll just use TelemetryViewer's "Export CSV log" option, which is simple to use and works well. Maybe something better will come up, but I know that'll work well enough to get started.
 
Don Kelly said:
...
Here's V9.
...
Click to expand...

Pulled V9 down and have it compiling with uBlox added - into GPS.ino. More to do . . .

It doesn't seem like the IMU is being responsive to "Imu.setSrd();" in initIMU.ino? I see the "sincePrint" output limiter - but my unit is always giving 50 Hz updates - 10 for each 5Hz GPS update?
< I can't make it run faster or slower than 50 Hz >
 
@Brian - MPU9250 needs some edits::
Found the setSrd() issue - calibrateGyro was not done in my prior testing.

Quick WorkAround/Kludge/HACK for setSrd() in ::
Code: 
void initIMU(){
  // ...
  Imu.calibrateGyro();
  Imu.setDlpfBandwidth(MPU9250::DLPF_BANDWIDTH_41HZ);
  Imu.setSrd(200);
}

This code pattern:
Code: 
int MPU9250::setSrd(uint8_t srd) {
  // ...
[B]  _srd = srd;
[/B]  return 1; 
}

defeats this restoration code in:
Code: 
int MPU9250::calibrateGyro() {
  // ...
  // set the range, bandwidth, and srd back to what they were
  if (setGyroRange(_gyroRange) < 0) {
    return -4;
  }
  if (setDlpfBandwidth(_bandwidth) < 0) {
    return -5;
  }
  if (setSrd(_srd) < 0) {
    return -6;
  }

It looks like this issue follows in any use of these during setup/calibration where the class "_var" is updated on ANY successful change:
int MPU9250::setGyroRange(GyroRange range) {
int MPU9250::setDlpfBandwidth(DlpfBandwidth bandwidth) {
int MPU9250::setSrd(uint8_t srd) {

These (other?) instances need a local_var to restore the initial value on exit
int MPU9250::calibrateAccel() {
int MPU9250::calibrateMag() {
int MPU9250::calibrateGyro() {
 
defragster. A little confused. You have Imu.setSrd(200);, I guess to set the update rate to 200hz but for a 200hz update rate setSRD should be set 4.
Mike
 
mjs513 said:
defragster. A little confused. You have Imu.setSrd(200);, I guess to set the update rate to 200hz but for a 200hz update rate setSRD should be set 4.
Mike
Click to expand...

noted : < I can't make it run faster or slower than 50 Hz > that is from this code:
Code: 
/* estimates the gyro biases */
int MPU9250::calibrateGyro() {
  // set the range, bandwidth, and srd
  if (setGyroRange(GYRO_RANGE_250DPS) < 0) {
    return -1;
  }
  if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) {
    return -2;
  }
[B]  if (setSrd(19) < 0) {[/B]
    return -3;

I included the MPU9250 code that causes the problem. The snip from initIMU() just happens to be the number I had when the HACK was tested to show 5Hz MPU data interleaved in 5Hz GPS data.
 
Yes that's true but I had this discussion with Brian earlier on about calibration. What he does with his calibration code is sets it to the minimum values for all ranges does the calibration and resets the ranges back to the original values. Personally I like to calibrate at the settings I am using. Which might be a problem with my drifting (haven't a clue). If you look further down he resets it back the ranges back to original values:
Code: 
  // set the range, bandwidth, and srd back to what they were
  if (setGyroRange(_gyroRange) < 0) {
    return -4;
  }
  if (setDlpfBandwidth(_bandwidth) < 0) {
    return -5;
  }
  if (setSrd(_srd) < 0) {
    return -6;
  }

Maybe you have it limited to the GPS update rate somewhere else in your code - I've done that a bunch of times over the years as I am sure we all have.
 
mjs513: The problem is the setting back - there is more in post #362 than you are seeing.

By the time it is set back 'to the class stored value' of _srd for instance this code succeeded and set that _srd value to the intermediate calibration value:
Code: 
int MPU9250::setSrd(uint8_t srd) {
  // ...
[B]  _srd = srd;[/B]
 
The correct code for calibrateGyro() would look like this - see BOLD t_??? vars:

< This is not tested/compiled because I started back on task and went to sleep with it currently not compiling with added code >

Code: 
/* estimates the gyro biases */
int MPU9250::calibrateGyro() {
[B]    GyroRange t_gyroRange = _gyroRange;
    DlpfBandwidth t_bandwidth = _bandwidth;
    uint8_t t_srd = _srd;
[/B]
  // set the range, bandwidth, and srd
  if (setGyroRange(GYRO_RANGE_250DPS) < 0) {
    return -1;
  }
  if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) {
    return -2;
  }
  if (setSrd(19) < 0) {
    return -3;
  }

  // take samples and find bias
  _gxbD = 0;
  _gybD = 0;
  _gzbD = 0;
  for (size_t i=0; i < _numSamples; i++) {
    readSensor();
    _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples);
    _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples);
    _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples);
    delay(20);
  }
  _gxb = (float)_gxbD;
  _gyb = (float)_gybD;
  _gzb = (float)_gzbD;

  // set the range, bandwidth, and srd back to what they were
  if ([B]setGyroRange(t_gyroRange)[/B] < 0) {
    return -4;
  }
  if ([B]setDlpfBandwidth(t_bandwidth)[/B] < 0) {
    return -5;
  }
  if ([B]setSrd(t_srd)[/B] < 0) {
    return -6;
  }
  return 1;
}
 
Got it. Now I understand the dense person that I am. I am working with Kris's ST IMU right now trying to get it to work with the uNavAHRs library. Of course I am modifying the library a bit with some added functions so I will leave it at that.

Anyway think you are going to have to change the accel/magnetometer calibrate functions as well. I think, since they use the same format as the calibrate gryo.
 
mjs513 said:
... Anyway think you are going to have to change the accel/magnetometer calibrate functions as well. I think, since they use the same format as the calibrate gryo.
Click to expand...

Indeed at least those functions noted in post #362 will all need similar attention:
These (other?) instances need a local_var to restore the initial value on exit
int MPU9250::calibrateAccel() {
int MPU9250::calibrateMag() {
int MPU9250::calibrateGyro() {
Click to expand...


As shown I could readily make those changes - probably easier than explaining it :) , but I was already late for sleep - and then Brian would still need to Merge the code - and there may be other affected functions (or issues) that didn't catch my eye. And I was hoping to stay focused to get my timing code updated before I got distracted - or Version 10 of the Filter code evolved or the Need for GPS was here . . . so far the distractions ( like finding this ) are winning . . .
 
so far the distractions ( like finding this ) are winning . . .
Click to expand...

Tell me about. Seems like the distractions are more interesting that the what your working on for a time at least. Agree you need to wait for Brian on the changes. Think he said he was working on grants so we might not hear from him for awhile.
 
defragster said:
The correct code for calibrateGyro() would look like this - see BOLD t_??? vars:

< This is not tested/compiled because I started back on task and went to sleep with it currently not compiling with added code >

Code: 
/* estimates the gyro biases */
int MPU9250::calibrateGyro() {
[B]    GyroRange t_gyroRange = _gyroRange;
    DlpfBandwidth t_bandwidth = _bandwidth;
    uint8_t t_srd = _srd;
[/B]
  // set the range, bandwidth, and srd
  if (setGyroRange(GYRO_RANGE_250DPS) < 0) {
    return -1;
  }
  if (setDlpfBandwidth(DLPF_BANDWIDTH_20HZ) < 0) {
    return -2;
  }
  if (setSrd(19) < 0) {
    return -3;
  }

  // take samples and find bias
  _gxbD = 0;
  _gybD = 0;
  _gzbD = 0;
  for (size_t i=0; i < _numSamples; i++) {
    readSensor();
    _gxbD += (getGyroX_rads() + _gxb)/((double)_numSamples);
    _gybD += (getGyroY_rads() + _gyb)/((double)_numSamples);
    _gzbD += (getGyroZ_rads() + _gzb)/((double)_numSamples);
    delay(20);
  }
  _gxb = (float)_gxbD;
  _gyb = (float)_gybD;
  _gzb = (float)_gzbD;

  // set the range, bandwidth, and srd back to what they were
  if ([B]setGyroRange(t_gyroRange)[/B] < 0) {
    return -4;
  }
  if ([B]setDlpfBandwidth(t_bandwidth)[/B] < 0) {
    return -5;
  }
  if ([B]setSrd(t_srd)[/B] < 0) {
    return -6;
  }
  return 1;
}
Click to expand...

I must be dense this morning. What order of function calls reproduces the issue and what's the issue that you see?

Yes, grant writing for the next month...
 
Hi All.

I managed to get Kris Winer's LSM6DSM_LIS2MDL_LPS22HB imu/baro working with uNavAHRS. Still seeing the yaw drift. A couple of things I do see ay mean is offset from 0 where az and ax seem to be. Same thing for gbz. The other thing I am seeing is that I am tending to loose gbz (just keeps increasing) at which point yaw doesn't work. Wanted to get a completely non-9250 imu working with the filter just to see if that same thing occurred. There is no filters applied, so I have to add the code in the libraries to get that piece working. Just wanted to keep you all posted.
 
brtaylor said:
I must be dense this morning. What order of function calls reproduces the issue and what's the issue that you see?
...
Click to expand...

See post #362 ???

Any Call to the indicated Calibrate functions makes temp changes to the IMU. HOWEVER - when those temp changes succeed - the CLASS _srd ( and _gyroRange, _bandwidth ) prior values are overwritten with this temp value. So it is not a temp value - but the current CLASS value, so the restoration is a NOP.

in:
Code: 
void initIMU(){
  // ...
[B]  Imu.calibrateGyro();
[/B]}

This leaves the _srd set to 19. Because this:
Code: 
int MPU9250::calibrateGyro() {
  // ...
[B]  if (setSrd(19) < 0) {
[/B]    return -3;
  // ...
  }

call to setSrd(19) exits setSrd() with this on success:

Code: 
int MPU9250::setSrd(uint8_t srd) {
  // ...
[B]  _srd = srd;
[/B]  return 1; 
}

So this call - is a No-Op because it is restoring a changed value with a value that has already been changed:
Code: 
int MPU9250::calibrateGyro() {
    // ...
[B]  if (setSrd(_srd) < 0) {
[/B]    return -6;
  }
  return 1;
}
 
defragster said:
See post #362 ???

Any Call to the indicated Calibrate functions makes temp changes to the IMU. HOWEVER - when those temp changes succeed - the CLASS _srd ( and _gyroRange, _bandwidth ) prior values are overwritten with this temp value. So it is not a temp value - but the current CLASS value, so the restoration is a NOP.

in:
Code: 
void initIMU(){
  // ...
[B]  Imu.calibrateGyro();
[/B]}

This leaves the _srd set to 19. Because this:
Code: 
int MPU9250::calibrateGyro() {
  // ...
[B]  if (setSrd(19) < 0) {
[/B]    return -3;
  // ...
  }

call to setSrd(19) exits setSrd() with this on success:

Code: 
int MPU9250::setSrd(uint8_t srd) {
  // ...
[B]  _srd = srd;
[/B]  return 1; 
}

So this call - is a No-Op because it is restoring a changed value with a value that has already been changed:
Code: 
int MPU9250::calibrateGyro() {
    // ...
[B]  if (setSrd(_srd) < 0) {
[/B]    return -6;
  }
  return 1;
}
Click to expand...

Thanks for the longer explanation! Makes sense now.
 
A quick update...

I'm running a uBlox and one of the AHRS algorithms, and am now streaming GPS data and IMU data at 5Hz to serial. I need to add a few other terms to the stream, and then I'll go out and collect some data on the move. Then I will take the data and run a (very simple) INS/GPS 6-state KF offline in Matlab and/or Python as a quick check to make sure the simple 6-state concept works. Assuming it works, I'll circle back to of the difficult implementation issues that we've been kicking around as a group:
1) How to timetag the IMU data accurately relative to GPS time (defragster has been discussing an idea around syncing to PPS)
2) How to collect IMU data at some fast rate (like 100-500Hz) and collect GPS data concurrently at a slower rate (like 5Hz). Maybe others see how to do this, but I'm not seeing it yet! At least using a single main loop.

Don
 
Status
Not open for further replies.
Back
Top