Using Roll-Pitch Gyro Drift Compensation for Tractor Navigation

[Chris Koutras]

Greetings fellow open source software travelers. This is slightly off the UAV related topic but closely related. If this is not allowed, please let me know and i will not bother the group. I am part of the development group for AgOpenGPS (AOG). Our goal is brining affordable precision GPS tractor navigation to small farmers. Our Github is https://github.com/farmerbriantee/AgOpenGPS and Discourse https://discourse.agopengps.com/

To keep costs down we use low cost IMU's which suffer from the effect of acceleration causing roll errors. I have read the aper by William Premerlani and it appears this group has solved this challenge for UAV's. I have looked through the MatrixPilot source but I am having difficulty following how Bill's algorithm is implemented. The folks on the AOG project would be grateful if someone from this group could help me understand how the roll and pitch compensation is implemented. We use the Teensy 4.1 MCU in our project so we would look to port the solution to c++ / Arduino.

Thanks, Chris

[Chris Koutras]

Apologies for replying to my own topic. I neglected to include a link to dev repo for the Teensy firmware. https://github.com/chriskinal/AOG-AiO-RVC-100hz/tree/main 

[Paul Bizard]

I will let Bill answer.. 

Regards, Paul

--
--
---
You received this message because you are subscribed to the Google Groups "uavdevboard" group.
To unsubscribe from this group and stop receiving emails from it, send an email to uavdevboard...@googlegroups.com.
To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/2c79e045-3236-41c5-8ec2-292f0f640464n%40googlegroups.com.

--

Paul Bizard

[jlde...@gmail.com]

Hello Chris,

 

While you wait for Bill, maybe you can explain a bit more what your concern is.

In your mail title, you refer to gyro drift and in the email content you refer to effect of acceleration causing roll errors.

 

Because most attitude solutions use IMU accelerometers to estimate gravity and provide a vertical reference, accelerations from trajectory will affect gravity estimation and eventually pitch and roll. This can be corrected by taking trajectory accelerations into account.
MEMS Gyro drift is primary due to temperature variation and, to a small effect to accelerations when they are significant. This can be corrected by compensating gyro bias in function of temperature, regulating IMU temperature and/or using external sources to estimate biases.

 

This makes me wonder which solution you use to compute pitch/roll attitude from the IMU and if you have sources beside IMU (i.e. GNSS speed vector, RTK heading/roll/pitch, etc.).

 

Jean-Luc

--

--
---
You received this message because you are subscribed to the Google Groups "uavdevboard" group.
To unsubscribe from this group and stop receiving emails from it, send an email to uavdevboard...@googlegroups.com.

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/aa351f08-ff68-4ee2-b286-4990cf191d37n%40googlegroups.com.

[Chris Koutras]

Hi Jean-Luc,

Thank you for taking interest in the work we are doing with AOG. I will try to provide a better description of the issues we are seeing.

AOG uses a planar navigation model like an Earth bound rover. There are two different setups; a single antenna/receiver precision GPS module like a U-blox F9P and a BNO085 IMU or a dual antenna/receiver GPS module like a Unicore UM982. Both setups use RTCM corrections to provide an RTK fix. 

The single GPS setup uses a fix to fix calculation to determine heading which can be inaccurate. The magnetometer can not be used due to all the ferrous metal in a tractor. If the IMU is placed high on the roof away from metal any pitch or roll causes a high acceleration in the X and/or Y axis resulting in large roll errors but the magnetometer heading works better. If the IMU is placed low on the chassis the pitch and roll motion causes much less X and/or Y acceleration and the roll errors are smaller. However, all the ferrous metal distorts the magnetometer. The roll reading is used to calculate the actual on the ground GPS position when the tractor is on a side hill or the ground is not level like 2 wheels on one side are in a furrow or ditch. The GPS reading feeds the look ahead algorithm to keep the tractor on the line or execute a turn via the autosteer motor. During a u-turn the roll errors cause the tractor to wiggle while trying to acquire the line. During straight line driving the roll errors cause the tractor veer away from the line resulting in wavy rows.

The dual setup minimizes the problem with roll and heading inaccuracies while it has an RTK fix. The accuracy begins to fall off with an RTK float fix and more so with a DGPS fix. In these situations it would be advantageous to fuse the GPS readings with the IMU readings to calculate an accurate roll reading.    

I have started to look at an improved version of Madgwick's algorithm from XIO Technologies. The source shows it was written by Seb Madgwick. It is showing some promise on the bench and in a car. Next is to put it on a test tractor in a bumpy field with hills and ditches.

Thanks Again,

Chris

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/00fd01db3c45%24686151e0%243923f5a0%24%40gmail.com.

[jlde...@gmail.com]

Comments in the text in green.

 

From: uavde...@googlegroups.com <uavde...@googlegroups.com> On Behalf Of Chris Koutras
Sent: Friday, November 22, 2024 1:57 AM
To: uavde...@googlegroups.com
Subject: Re: Using Roll-Pitch Gyro Drift Compensation for Tractor Navigation

 

Hi Jean-Luc,

 

Thank you for taking interest in the work we are doing with AOG. I will try to provide a better description of the issues we are seeing.

 

AOG uses a planar navigation model like an Earth bound rover. There are two different setups; a single antenna/receiver precision GPS module like a U-blox F9P and a BNO085 IMU or a dual antenna/receiver GPS module like a Unicore UM982.

Both setups use RTCM corrections to provide an RTK fix. 
=> It is a different topic but I would be interested to hear about the performance of the UM982. In particular, the percentage of time you maintain RTK fix especially when there is significant roll or pitch.

 

The single GPS setup uses a fix to fix calculation to determine heading which can be inaccurate. The magnetometer can not be used due to all the ferrous metal in a tractor. If the IMU is placed high on the roof away from metal any pitch or roll causes a high acceleration in the X and/or Y axis resulting in large roll errors but the magnetometer heading works better. If the IMU is placed low on the chassis the pitch and roll motion causes much less X and/or Y acceleration and the roll errors are smaller. However, all the ferrous metal distorts the magnetometer. The roll reading is used to calculate the actual on the ground GPS position when the tractor is on a side hill or the ground is not level like 2 wheels on one side are in a furrow or ditch. The GPS reading feeds the look ahead algorithm to keep the tractor on the line or execute a turn via the autosteer motor. During a u-turn the roll errors cause the tractor to wiggle while trying to acquire the line. During straight line driving the roll errors cause the tractor veer away from the line resulting in wavy rows.

• For the heading, which is also the track for ground applications, have you tried to use the heading computed directly from the 3D GNSS speed vector?
• The phenomenon you describe when moving the location of the IMU is due to the fact the IMU is not located at the center of mass/rotation of the body. Therefore, when the body moves/rotates, there are parasitic accelerations sensed by the IMU. We have similar issues on large airborne applications when IMU is not close to center of gravity. This can be corrected if you can calculate the distances on the 3 axes between your IMU and the center used for reference. Similarly, you need to correct accelerations when the IMU axis are not aligned with the tractor body axes. However, in your case, you probably don’t need the trajectory of the center of gravity of the tractor but instead the trajectory of the projection of the center of gravity on the ground. In any case, you can calculate the corrections from the actual IMU position to any place and angles you want.

 

The dual setup minimizes the problem with roll and heading inaccuracies while it has an RTK fix. The accuracy begins to fall off with an RTK float fix and more so with a DGPS fix. In these situations it would be advantageous to fuse the GPS readings with the IMU readings to calculate an accurate roll reading.    

 

I have started to look at an improved version of Madgwick's algorithm from XIO Technologies. The source shows it was written by Seb Madgwick. It is showing some promise on the bench and in a car. Next is to put it on a test tractor in a bumpy field with hills and ditches.

• We have used Magdwick for some applications and it works well, as long as you can correct all the effects discussed above.

=> I am sure other members of this group,  might provide good guidance to help resolve your problem.

 

Thanks Again,

Chris

 

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/CAPmX6v0h-BALPzk9fTWL3vDm%3DS9cy06w2Lnj2wij7z1-3E31xw%40mail.gmail.com.

[Chris Koutras]

Responses in blue.

On Fri, Nov 22, 2024 at 4:09 AM <jlde...@gmail.com> wrote:

Comments in the text in green.

 

From: uavde...@googlegroups.com <uavde...@googlegroups.com> On Behalf Of Chris Koutras
Sent: Friday, November 22, 2024 1:57 AM
To: uavde...@googlegroups.com
Subject: Re: Using Roll-Pitch Gyro Drift Compensation for Tractor Navigation

 

Hi Jean-Luc,

 

Thank you for taking interest in the work we are doing with AOG. I will try to provide a better description of the issues we are seeing.

 

AOG uses a planar navigation model like an Earth bound rover. There are two different setups; a single antenna/receiver precision GPS module like a U-blox F9P and a BNO085 IMU or a dual antenna/receiver GPS module like a Unicore UM982. Both setups use RTCM corrections to provide an RTK fix. 
=> It is a different topic but I would be interested to hear about the performance of the UM982. In particular, the percentage of time you maintain RTK fix especially when there is significant roll or pitch.

 => Typically tractor manufacturers specify  no more than 10 degrees of roll. Occasionally up to 15 degrees with certain equipment. Above these values and roll over accidents increase exponentially. At these shallow roll angles RTK fix usually does not degrade in open fields. Where it degrades is in places with overhead cover like fruit and nut groves and in headlands. Headlands are a perimeter around the field that is not used for crop production, often trees with overhangs create a GPS black out shadow. It is used for service vehicles and tractors to make U or K turns to work the next row of crops.

 

The single GPS setup uses a fix to fix calculation to determine heading which can be inaccurate. The magnetometer can not be used due to all the ferrous metal in a tractor. If the IMU is placed high on the roof away from metal any pitch or roll causes a high acceleration in the X and/or Y axis resulting in large roll errors but the magnetometer heading works better. If the IMU is placed low on the chassis the pitch and roll motion causes much less X and/or Y acceleration and the roll errors are smaller. However, all the ferrous metal distorts the magnetometer. The roll reading is used to calculate the actual on the ground GPS position when the tractor is on a side hill or the ground is not level like 2 wheels on one side are in a furrow or ditch. The GPS reading feeds the look ahead algorithm to keep the tractor on the line or execute a turn via the autosteer motor. During a u-turn the roll errors cause the tractor to wiggle while trying to acquire the line. During straight line driving the roll errors cause the tractor veer away from the line resulting in wavy rows.

• For the heading, which is also the track for ground applications, have you tried to use the heading computed directly from the 3D GNSS speed vector?

         => Thanks for this thought. I'll have to check which GPS modules output messages with this data. I know the UM982 has vendor specific messages with ecef data. I think there are a few with the speed of the vectors.

• The phenomenon you describe when moving the location of the IMU is due to the fact the IMU is not located at the center of mass/rotation of the body. Therefore, when the body moves/rotates, there are parasitic accelerations sensed by the IMU. We have similar issues on large airborne applications when IMU is not close to center of gravity. This can be corrected if you can calculate the distances on the 3 axes between your IMU and the center used for reference. Similarly, you need to correct accelerations when the IMU axis are not aligned with the tractor body axes. However, in your case, you probably don’t need the trajectory of the center of gravity of the tractor but instead the trajectory of the projection of the center of gravity on the ground. In any case, you can calculate the corrections from the actual IMU position to any place and angles you want.

 

               => This is a really good point. I often see IMU's mounted behind the seat on the rear cab wall. I would suspect this location is behind and above the CG of the tractor. Manufacturers don't really provide information on where the design CG of the tractor is located. I suppose in the 2D plane it could be calculated from the weight on each axle and/or wheel. I know AOG currently does not have a place to input CG or offset from CG nor weights. I'll bring this up in the Dev group to see if there is history on it I am not aware of.  

 

The dual setup minimizes the problem with roll and heading inaccuracies while it has an RTK fix. The accuracy begins to fall off with an RTK float fix and more so with a DGPS fix. In these situations it would be advantageous to fuse the GPS readings with the IMU readings to calculate an accurate roll reading.    

 

I have started to look at an improved version of Madgwick's algorithm from XIO Technologies. The source shows it was written by Seb Madgwick. It is showing some promise on the bench and in a car. Next is to put it on a test tractor in a bumpy field with hills and ditches.

• We have used Magdwick for some applications and it works well, as long as you can correct all the effects discussed above.

          => I have done some work with Madgwick as I mentioned above. It definitely shows improvement over no filtering. Perhaps correcting for CG offset as mentioned above can get to an acceptable level of accuracy.

=> I am sure other members of this group,  might provide good guidance to help resolve your problem.

   => Thank you all for taking the time to read and reply. I really appreciate the dialog. 

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/007401db3cc6%24a8c9a4e0%24fa5ceea0%24%40gmail.com.

[Lenny Iszak]

Chris,

A couple of years ago I was working on an AHRS system for OnSpeed, an open source angle of attack system for aircraft. You can find it here: https://github.com/flyonspeed/OnSpeed-Gen2/tree/master/Software/OnSpeedTeensy_AHRS

I'll offer up some of the things I learned, hopefully it helps you out.

You can correct the accelerometer and gyroscope values for installation error, then also correct it for centripetal accelerations and then feed it into the Madgwick algo.

We didn't want to use a magnetometer because the end user would have to calibrate it, and keep it calibrated. Also we used airspeed for the centripetal correction. So, not very useful to you, but there are some valuable lessons you can take from there.

After going through a bunch of military papers, I found the math on how to correct the data. It's a pretty basic matrix calculation. See above source code at https://github.com/flyonspeed/OnSpeed-Gen2/blob/be1481c45685caad3d47b60283ef5b2cc313c4db/Software/OnSpeedTeensy_AHRS/AHRS.ino#L22

These are for orientation errors only, displacement from CG would have to be added. If you go down this path let me know and I'll dig up the math for you.

Also found this book very helpful in explaining the dynamics at play. It's a good resource for Kalman filtering too.

Small Unmanned Aircraft: Theory and Practice 2nd Edition, by Randal W. Beard 
Ultimately, you'd probably be better off with a Kalman filter (described in the above book). Madgwick can be made to work, but it isn't tracking gyro and accelerometer drift. Most of these chips have a serious drift caused by temperature and as a result the attitude won't be stable for very long.. I found that one of the accelerometer axis was also sensitive to the rate of change of its temperature. Created a calibration curve to correct even for second order temp errors but then it wouldn't translate to another chip. Each one would need to be separately calibrated. Uggh. In the end we found an IMU chip that has better factory temp correction and very low drift.
To properly tune the system, we used an expensive tactical grade VectorNav IMU, and then wrote a minimizing function in Matlab to search for the optimal parameters. We ended up matching the VectorNav attitude pretty closely, except our signal is somewhat noisier. Kalman would have been way smoother but due to computational constraints we were not able to use it.
Regards,

Lenny Iszak

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/CAPmX6v0bvfDVEwPfGWFFfMqj4c8pvmNW6RL5TKcHVTUOB%2BtvJA%40mail.gmail.com.

[Chris Koutras]

Thanks a lot Lenny.

Some really insightful dialog. I have been looking for the math on displacement from CG. The information I have been able to find on a few tractors places the CG quite low. Many tractors use the engine / transmission as a major frame. In some cases the CG is somewhere very close to or in the bellhousing holding the clutch. Not a place one could easily put an IMU. I would be grateful if you could dig up the math on CG displacement as I see this being key to getting accurate IMU measurements on a tractor. I have been testing out a WitMotion WT901C IMU. according to the documentation it has built in Kalman filtering for temperature compensation and bias. I need to do some more testing to see how effective it is in practice.

Thanks Again for your help,

Chris

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/CADyKchWmmizNjO_-nVZmnMaWaVnmYdX1gy0n139mQ863V__S-A%40mail.gmail.com.

[Lenny Iszak]

Chris,

Here's the Matlab script of a Kalman implementation i mentioned earlier. It has the CG displacement correction. Look for installX, installY, installZ.
https://github.com/flyonspeed/OnSpeed-Gen2/blob/be1481c45685caad3d47b60283ef5b2cc313c4db/Software/Matlab/Extended_Kalman_Filter_with-alpha.m#L189

Using these corrections you can mount your IMU wherever you want, and in almost any orientation. In aircraft they usually place the magnetometer separately away from ferrous materials.

Lenny

To view this discussion, visit https://groups.google.com/d/msgid/uavdevboard/CAPmX6v0%3DsXgPz0PgB2ppEHf16tzZq3PDAx8BXFsV_wzf2b7ntg%40mail.gmail.com.