uNav INS

Ok. Just tried something else - just assume if there is no motion yaw stays constant from the last updated position but the filter update still runs - seems to work pretty good. as well.
 
just lurking … Seems like a useful observation Mike. I recall working with the Prop Shield and coloring the R | G | B on my light strip positions - and the YAW would drift when stationary with regularity. The others would generally return when moved and of course not move when stationary.
 
Hi Tim
Happy Holidays and at the risk of being politically incorrect Merry Christmas. Yeah, I think that the yaw drift when it is stationary is something that is inherent with Kalman filters. If you look at the BNO055 IMU, it doesn't update values unless the IMU is moving. That is probably why.

Mike
 
Was actually thinking about that one, that was why I was trying it out yesterday. Especially after I saw the charts in that link I posted :)

From what I saw there is a lot of variation in the tilt compass data that was why I switched to using MahoneyAHRS. In just a quick look, since I was just trying things out (have to go back and try it out more carefully) there seems be several degrees of differences between what get calculated by the KF and the Mahoney filter and compass. Need to run a few more focused tests after the holidays - doing this between getting ready for the holiday :)

Will keep you posted.

EDIT: BTW here is my function for determining zero motion:
Code:
#include "MovingAvarageFilter.h"

//Setup Motion Detect Averages
MovingAvarageFilter accnorm_avg(5);
MovingAvarageFilter accnorm_test_avg(7);
MovingAvarageFilter accnorm_var(7);  
MovingAvarageFilter motion_detect_ma(10);

//======================================================================

bool MotionDetect() {
  
   float gyro[3];
   float accnorm;
   int accnorm_test, accnorm_var_test, omegax, omegay, omegaz, omega_test, motionDetect;
  
   gyro[0] = gx;
   gyro[1] = gy;
   gyro[2] = gz;

   float ax_n = ax/9.807;
   float ay_n = ay/9.807;
   float az_n = az/9.807;
  
   /*###################################################################
   #
   #   acceleration squared euclidean norm analysis
   #   
   #   some test values previously used:
   #       if((accnorm >=0.96) && (accnorm <= 0.99)){
   #                   <= 0.995
   #
   ################################################################### */
    //accnorm = (ax*ax+ay*ay+az*az);
    accnorm = (ax_n*ax_n+ay_n*ay_n+az_n*az_n);
    if((accnorm >=0.9) && (accnorm <= 1.3)){  //was 1.07 and .9
        accnorm_test = 0;
    } else {
        accnorm_test = 1; }

    /** take average of 5 to 10 points  **/
    float accnormavg = accnorm_avg.process(accnorm);
    float accnormtestavg = accnorm_test_avg.process(accnorm_test);

    /*####################################################################
    #
    #   squared norm analysis to determine suddenly changes signal
    #
    #####################################################################*/
    //accnorm_var.process(sq(accnorm-accnorm_avg.getAvg()));
    // was 0.0005
    if(accnorm_var.process(sq(accnorm-accnormavg)) < 0.0005) {
        accnorm_var_test = 0;
    } else {
        accnorm_var_test = 1; }
//Serial.print("ACCEL TEST:  ");
//Serial.print(accnorm); Serial.print(", "); Serial.print(accnorm_test);
//Serial.print(", "); Serial.println(accnorm_var_test);
    /*###################################################################
    #
    #   angular rate analysis in order to disregard linear acceleration
    #
    #   other test values used: 0, 0.00215, 0.00215
    ################################################################### */
    if ((gyro[0] >=-0.02) && (gyro[0] <= 0.02)) {
        omegax = 0;
    } else {
        omegax = 1; }
        
    if((gyro[1] >= -0.02) && (gyro[1] <= 0.02)) {
        omegay = 0;
    } else {
        omegay = 1; }
        
    //if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
    if((gyro[2] >= -0.005) && (gyro[2] <= 0.005)) {
        omegaz = 0;
    } else {
        omegaz = 1; }
        
    if ((omegax+omegay+omegaz) > 0) {
        omega_test = 1;
    } else {
        omega_test = 0; }
//Serial.print("GYRO TEST:"); Serial.print(gyro[2]);Serial.print(", ");
//Serial.print(omegax); Serial.print(", "); Serial.print(omegay);
//Serial.print(", "); Serial.print(omegaz);
//Serial.print(", "); Serial.println(omega_test);
    /* 
    ###################################################################
    #
    # combined movement detector
    #
    #################################################################### 
    */
    if ((accnormtestavg + omega_test + accnorm_var_test) > 0) {
      motionDetect = 1;
    } else {
        motionDetect = 0; }

    /* 
    ################################################################## 
    */     
    //motion_detect_ma.process(motionDetect);
    if(motion_detect_ma.process(motionDetect) > 0.5) {
      return true;
    } else {
      return false;
    }
}
 
Hi Tim
Happy Holidays and at the risk of being politically incorrect Merry Christmas. Yeah, I think that the yaw drift when it is stationary is something that is inherent with Kalman filters. If you look at the BNO055 IMU, it doesn't update values unless the IMU is moving. That is probably why.

Mike

Thanks Mike and Merry Christmas and Happy New Year - and Brian and Don … been a while since we saw the SPI_MSTransfer working and then new distractions.

Code:
        omega_test = omegax | omegay | omegaz;

Cool if that improves reliability/results - so that is not adding Kalman - but just deciding to exclude yaw math creeping when there isn't motion?
 
Well, there is actually 2 parts to that algorithm. The first does some magic to test acceleration (that's the accnorm stuff) and then it looks at the gyros to determine if there is rotation (that's the omega terms). At the end it puts it all together:
Code:
accnormtestavg + omega_test + accnorm_var_test) > 0
and if the test is greater than zero then you have motion. :)

From just initial tests looks like excluding yaw it will work - just have to figure out the best way to handle it - looking at the KF now to see how ref handles it with in the filter itself. Here is the presentation that I was referring to earlier. Here is the extract of the pertinent pages. Just have to figure out how to apply it to Brian's model.
View attachment Pages from foot_mounted_zero_velocity.pdf
 
I saw other parts - I just dropped that if free sample version of the one to keep my post 'on topic' :)

Code:
          omega_test = omegax | omegay | omegaz;
          // ...
          motionDetect = accnormtestavg | omega_test | accnorm_var_test;

Was wondering what happens when movement starts - if the Yaw is saved unchanged when motion stops - or if it will creep and show up way off when motion starts.
 
I ran a simple test using Brian's Teensy Backpack (T3.2 plus MPU9250). I aligned to x-axis pointer to close to magnetic North and let the filters run for awhile and then started movement up to about 90degs and then brought it back to North. This way zero degrees should be close for both filters. Going to run another test Brian's compass sketch and do a comparison.
Capture.jpg

UPDATE: I added in the TiltCompass code and did a quick comparison of the yaw. Essential the yaw from the tiltcompass code matches the yaw output from the Mahoney filter. This is when its flat of course.
 
Last edited:
Hi Mike, Brain, Tim,
Happy Holidays! Am following along with your testing Mike, looks interesting.

I'm still refining my M8P RTK work using the SparkFun M8P-02 boards. The M8P-02 requires RTCM3 base inputs, so I've switched my base station from UBX binary now to RTCM3 and it's working great. I posted over on Brian's UBLOX thread earlier today as I'm trying to parse UBX binary via I2C using the M8P's Qwiic cables (which are kinda cool). My thinking is to have my RTK base stations output RTCM3 via UART (for input into the Rover's M8P) and concurrently output UBX-NAV-PVT via I2C for base station status display (I2C LCD).

I'd eventually like to start pulling in this super-accurate RTK solution into the uNav INS algorithm. With the 20Hz F9P chip coming soon, will be fun to try some different Kalman configs out!

Don
 
Last edited:
I ran a simple test using Brian's Teensy Backpack (T3.2 plus MPU9250). I aligned to x-axis pointer to close to magnetic North and let the filters run for awhile and then started movement up to about 90degs and then brought it back to North. This way zero degrees should be close for both filters. Going to run another test Brian's compass sketch and do a comparison.
View attachment 15418

UPDATE: I added in the TiltCompass code and did a quick comparison of the yaw. Essential the yaw from the tiltcompass code matches the yaw output from the Mahoney filter. This is when its flat of course.

Mike is the KF the 15 state INS or the Euler AHRS?
 
Any plans on using the BME280 in the motion backpack for the INS or AHRS libraries? Typically baro, GPS, and vertical acceleration are fused to determined altitude.

Thanks

Bruce
 
Long term, yes, I would like to incorporate air data into the INS estimate when available. I'm not sure what the effect would be on output accuracy, but, it would help arrest drift in cases where GPS is unavailable for extended periods and provide an estimate of wind speed and direction when all sensors are available and working.
 
Just wondering why the update statement has negation and swapping of axis for the accel and the gyro?

Code:
Filter.update(uBloxData.iTOW,uBloxData.velN,uBloxData.velE,uBloxData.velD,uBloxData.lat*PI/180.0f,uBloxData.lon*PI/180.0f,uBloxData.hMSL,Imu.getGyroY_rads(),-1*Imu.getGyroX_rads(),Imu.getGyroZ_rads(),Imu.getAccelY_mss(),-1*Imu.getAccelX_mss(),Imu.getAccelZ_mss(),Imu.getMagX_uT(),Imu.getMagY_uT(),Imu.getMagZ_uT());

The axis system for the motion back pack is NED.
MPU-9250-AXIS.png

The axis system for the uNavINS is missing from the wikki.

Why are the axis swapped around?

Thanks

Bruce
 
Calibration Issue with 9250

Hi Don,

I tried your calibration routine, CalAnalysisMPU9250Ver2, and I'm getting a static IMU bias calibration that doesn't look correct. The parameter, azb, magnitude looks too large. I'm thinking there is one of two possibilities, 1) either there's a code issue or, 2) I have a bad breakout card. Here are the results that I obtain:
Code:
Wait... Conducting IMU static calibration...
Static Calibration Bias Values:
axb: -0.647463
ayb: -0.115094
azb: -4.477447
gxb: 0.000910
gyb: 0.001690
gzb: 0.000004
hxb: -13.498309
hyb: 41.154903
hzb: -26.354893

I looked at your code and don't see any issues but I started getting lost in the circular buffer area. I appears the large bias is preventing me from calculating the accelerometer Z-axis calibration. Any thoughts?

Thanks,
Willie

UPDATE:
Just thought to try another experiment. I have a second 3.6 and 9250 that I was using for prototyping and here is the result that I obtain from the static calibration:
Code:
Wait... Conducting IMU static calibration...
Static Calibration Bias Values:
axb: 0.157945
ayb: -0.456676
azb: -3.711194
gxb: -0.000832
gyb: 0.000367
gzb: 0.000229
hxb: -31.889704
hyb: 10.246424
hzb: -47.167191
It looks about the same.
Thanks,
-W
 
Last edited:
Update On Inverted ?

Any update on why some of the IMU input are inverted within the update statement?

Thanks

Bruce
 
Hi Bruce, sorry I've been down with an upper respiratory bug for the past few weeks. And now I'm playing catch up.

I haven't had a chance to track down exactly what I was experimenting with. The MPU9250 library I wrote puts everything in the correct orientation first, so there shouldn't need to be that transformation. BUT, maybe I was testing with a different IMU and experimental code accidentally got committed to github. My best guess - the axis system shouldn't be swapped, I don't think you'll need the negative signs with the MPU-9250.
 
So the library expects the IMU input in the NED convention?

Also, is the any available paper/presentation/literature concerning the EKF in this library?

I ask because I am very interest in adding baro as an altitude input to fuse of the GPS and IMU.

Thanks

Bruce
 
So the library expects the IMU input in the NED convention?

Also, is the any available paper/presentation/literature concerning the EKF in this library?

I ask because I am very interest in adding baro as an altitude input to fuse of the GPS and IMU.

Thanks

Bruce

Yes, NED. I can recommend Dan Simon's book for EKF. There are a few papers that mention the EKF presented here at a high level, but none (to my knowledge) go into the specifics. It's a very straightforward filter, however.
 
I have reference for EKF implementation.

The question I am asking is about this specific EKF implementation as the code is not really documented with comments and such.

So anything that would help with variable meanings, what is being calculated where and so on would be very helpful.

Thanks

Bruce
 
Back
Top