Forum Rule: Always post complete source code & details to reproduce any issue!
Results 1 to 15 of 15

Thread: Prop Shield NXPSensorFusion observations

  1. #1
    Senior Member
    Join Date
    Jul 2014
    Posts
    139

    Prop Shield NXPSensorFusion observations

    Firstly, a little bit of fun with making a compass with the Prop Shield using the Madgwick filter:
    Teensy Prop Shield Compass (Madgwick)

    Now onto the more analytical videos. This video shows the Madgwick algorithm's initial startup:
    Teensy Prop Shield Compass (Initial Madgwick orientation)

    As you might expect, the current implementation of Madgwick starts up instantly and gimbles slowly to North. So the Madgwick filter might benefit from having a FirstOrientationLock feature like NXPSensorFusion.

    Now over to the NXPSensorFusion filter:
    Teensy Prop Shield Compass (NXPSensorFusion FirstOrientationLock and ValidMagCal issue)
    0:04 - I plug in the teensy to my PC so I can see the serial monitor which is displaying the ValidMagCal and FirstOrientationLock flags from inside the NXPSensorFusion library.
    0:09 - I rotate the board until the conditions for setting FirstOrientationLock become valid (i.e. ValidMagCal -> true).
    0:27 - I rotate the board back to it's original position where ValidMagCal = false. This means the heading is now just using the Gyro data.
    0:27 - 1:50 - Due to not using the Mag data, the heading drifts due to Gyro offset. (Fast forward & compare frame at 0:30 to frame at 1:50 to see that heading has drifted)
    1:54 - I rotate the board to a position where ValidMagCal -> true again and you see the heading drift being corrected

    Interestingly the NXPSensorFusion heading is 180 degrees different to Madgwick. Before anyone asks - I used the MotionCal to get a very good calibration before all these videos.

    I was able to find a workaround for the loss of ValidMagCal in SensorFusion.cpp by changing

    Code:
    if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
      ValidMagCal = 1;
    } else {
      ValidMagCal = 0;
    }
    to

    Code:
    if(FirstOrientationLock == 0){
      if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
        ValidMagCal = 1;
      } else {
        ValidMagCal = 0;
      }
    } else{
      ValidMagCal = 1;
    }
    I don't think this workaround is technically correct but it might be helpful for others just to get things working.

    I'm interested to see what others have observed, please feel free to post on this thread.
    Last edited by macaba; 04-02-2016 at 10:03 PM.

  2. #2
    Senior Member Wozzy's Avatar
    Join Date
    Jan 2013
    Location
    Philadelphia, Pennsylvania USA
    Posts
    354
    I too noticed the startup drift with the Madgwick filter.
    It seems to settle out after minute and give good long term stability once settled however.

  3. #3
    Senior Member
    Join Date
    Jul 2014
    Posts
    139
    Magnetic jamming is a bit of an issue as seen here:
    YouTube: Teensy Prop Shield magnetic jamming issue (Madgwick)

    So I came up with a way to reject large changes in magnetic field/soft iron objects, the results are shown here:
    YouTube: Teensy Prop Shield magnetic jamming detection (Madgwick)

    Modified MadgwickAHRS library is here
    I had to make a tradeoff with sensitivity - you'll find its still possible to upset the heading with soft iron objects before they're detected but if I make the algorithm too sensitive then I found that noise in the sensor reading and deviation in calibration (i.e. changing rooms) would cause too many false triggers.

  4. #4
    Senior Member PaulStoffregen's Avatar
    Join Date
    Nov 2012
    Posts
    21,827
    Yes, Madgwick & Mahony need these features....

  5. #5
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    Hi All

    I hope you don't mind me jumping into this discussion. Its pretty cool what you did with accounting for strong magnetic fields. Based on this thread I did a little web searching and I came across a paper you might be interested in: Accurate Orientation Estimation Using AHRS under Conditions of Magnetic Distortion and Unscented Kalman filter and Magnetic Angular Rate Update (MARU) for an improved Pedestrian Dead-Reckoning. It does something very similar to what you are doing. Essentially, it performs the MARU calculation outside the filter calculation, like a ZUPT and if a magnetic distortion is detected it uses updateIMU() instead of update().

    I just ordered a prop shield last night and going to play around with your code and MARU and see what happens.

    As for the startup drift that is actually normal for the madgwick and mahony filters as they iterative solutions. What you are seeing is the algorithm converging to the solution from the initial startup conditions. You can speed this process up by increasing the filter factor (Beta) for a short period and then reducing it back to a normal value. This is actually what Madgwick did in his original paper.

    Cheeers
    Mike

  6. #6
    Senior Member
    Join Date
    Jul 2014
    Posts
    139
    Mike; thanks for the papers, I had not seen them before.

    I was already thinking about the implementation of what the first paper calls 'magnetic dip angle' as I think this is the final piece of the puzzle in combination with the magnetic field intensity detection that I've already got.

    Essentially - take advantage of the fact that the gyro drift error is very small so when the magnetic vector heading is more than 5-10 degrees different to the gyro-only heading then reject the readings. (Assuming we're past the initial startup drift, I would like to add in a 'first run' forced quaternion update soon to get rid of the startup drift) This would be in addition to the current checking of the magnetic field intensity. This should mean that the only way to influence the heading with soft-iron/magnets is in a narrow 10-20 degree arc approaching the sensor from the North which wouldn't really matter anyway!
    I'm also working with Paul on GitHub to get my code in a suitable state for merging into Teensy.
    Last edited by macaba; 04-05-2016 at 02:22 PM.

  7. #7
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    macaba. Not a problem. Like I said I found it interesting so I started digging.

    I would like to see the magnetic detection as a separate function so it would call either update(gx, gy, gz, ax, ay, az) or updateIMU(gx, gy, gz, ax, ay, az, mx, my, mz) think it might be cleaner and then can be ported to other AHRS filters like the Mahony filter.

    Please understand on startup you are not seeing "drift" you are seeing the algorithm converging to the correct solution. If you want to get it to converge faster change the Beta startup value for a short period of time and then reduce it to the final value or change the startup values of the quaternions. After the algorithm converges you wll get stable values.

    You could try to determine the startup quaternions using just the accelerometer to get initial yaw, pitch and roll angles. Matlab has a function called angle2quat which converts rotation angles to quaternion. If you want it let me know.

    Mike

  8. #8
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    Hi All

    Based on these discussions I thought I would look at the data itself and the effect on the AHRS Algorithm so I did a few experiments using the CurieIMU with a 5883L magnetic using the Madgwick Filter. For these tests I am using the FreeIMU lib but the data analysis should still be applicable. For the test I am using calibrated data which currently is almost impossible to get from the current library. However you can use the FreeIMU Cal Gui to do the calibration.

    Ok now for the data. Figure 1 shows the entire run I made moving the magnet normal and parallel to x-axis, parallel to y-axis and normal and parallel to z-axis and well as tilting the board and moving the magnet around in similar fashion.
    Click image for larger version. 

Name:	Figure 1.jpg 
Views:	244 
Size:	99.6 KB 
ID:	6951

    Figure 2 shows the effect on corresponding affect on the Heading as calculated from the magnetometer. As you can expect the magnet has a very large effect on the compass heading.
    Click image for larger version. 

Name:	Figure 1.jpg 
Views:	244 
Size:	99.6 KB 
ID:	6951

    Figure 3 zeros in on the portion of test where the magnet is being moved normal and parallel to x-axis.
    Click image for larger version. 

Name:	Figure 3.jpg 
Views:	165 
Size:	134.5 KB 
ID:	6952

    Based on the data a simple discriminator can be used determine the presence of a magnetic distortion using the normalized mag readings. Specifically +/-11% of the baseline MagCal value. When outside this range you use the filter with only the accelerometer and the gyro. When inside the range you use the full filter. Looking at the normal accel readings you can see that the magnet has no effect on it, figure 4:
    Click image for larger version. 

Name:	Figure 4.png 
Views:	140 
Size:	39.7 KB 
ID:	6953

    Figure 5 shows the effect in the above range on Yaw (does not affect it as much as the compass).
    Click image for larger version. 

Name:	Figure 5.jpg 
Views:	181 
Size:	59.2 KB 
ID:	6954

    As for correcting the compass that is another story - you might do a switch between Yaw and Compass reading reliance or do a blending to determing error. Still working this one.

    Mike

  9. #9
    Senior Member
    Join Date
    Jul 2014
    Posts
    139
    Nice charting Mike!

    Quote Originally Posted by mjs513 View Post
    Based on the data a simple discriminator can be used determine the presence of a magnetic distortion using the normalized mag readings. Specifically +/-11% of the baseline MagCal value.
    Have you tried my library fork? It does this.

    Interestingly it appears that the prop shield + related code gives back units of uT, I get an average of about 48 (in whatever unit that might be, hopefully uT) which correlates exactly to my location on this map:
    https://upload.wikimedia.org/wikiped...Field_2015.pdf

  10. #10
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    Haven't tried it yet. Just started going through your jamming code and if I am reading it right you are using absolute values for mag values. That part I need to still work through. I have this thing that I like looking at things myself to really understand what's going on. It was an interesting exercise.

    By the way if you want to get a more accurate reading on the field strength here is the NOAA Magnetic Field Calculators that I used in the past.

  11. #11
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    Hi mcaba

    Made a little video of implementing the method I proposed in my previous email, i.e., to use normalized values instead of absolutes. It is a private video on youtube so enjoy, its speed up by 2x. Now I can take a closer look at your library.

    BTW I got my prop-shield today so now I have to hook it up and give it a try.

  12. #12
    Senior Member
    Join Date
    Jul 2014
    Posts
    139
    Hi, I suggest you change your video to 'Unlisted' as I can't view it.

  13. #13
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    Ok - changed to unlisted.

    macaba, I was also thinking that if you are also using the magnetometer to get a compass reading you could also correct the compass turn off the compass calcs during periods of disturbance and use a delta yaw from the Madgwick filter to correct compass. Also, I think that after periods of magnetic disturbance the magnetometer should be degaussed. What do you think.
    Last edited by mjs513; 04-08-2016 at 03:18 PM. Reason: Updated info

  14. #14
    Senior Member+ mjs513's Avatar
    Join Date
    Jul 2014
    Location
    New York
    Posts
    5,105
    Hi namtan1234

    I received an email that you posted a request to change the youtube video to unlisted but I do not see it here. I just checked and the video is identified as unlisted here is link again.

    Mike

  15. #15
    @macaba I just found out about the NXP FXOS8700CQ compass from Adafruits new breakout, and then realized that design and code originated from Pauls Prop shield board.

    I'm receiving a breakout board Monday that has the compass NXP FXOS8700CQ & Gyro NXP FXOS8700CQ chips on them. I'll hook that up to a Teensy 3.2 for testing and calibration which I'm excited to do.

    I've had other compass sensors in the past but nothing with easy to use calibration software to go with it, those other compass sensors were not very accurate because of the lack of calibration I'm assuming.

    I want to use the compass for a solar panel tracker, so basically I need stable compass heading readings so I, can point the solar panel towards the sun all day long and I'm hoping these sensors are capable of providing accurate heading info consistently.

    I have a few questions that you may be able to help with.

    #1. After the initial calibration using the calibration software would it be possible to recalibrate the heading by only rotating the board in a circular 360 degree motion in the clockwise or counter clockwise motion? Like if I have a solar tracker that is shipped to the other side of the country I assume for best compass results it would need to be recalibrated where the tracker sits outside? Not sure if the sensor would need to be rotated in all directions or if just 360 degree spins would do it?

    #2. How well does the tilt compenstation for the compass readings work with your Sensor Fusion software?

    #3. I like your strong magnetic interfence rejection code. I see the latest GitHub update was the 8th of this month so I'm not sure how much has made it into Pauls library but it's nice to see the progress with these features.

    #4. How well do you think the chips and software are working as far as getting stable long term readings once setup?

    Any info about this is greatly apperciated!

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •