Dual Heading as IMU

A discussion about using the dual heading as the imu heading - to overcome the tractor pointing in a different direction then it is travelling.

In regards to Darren ‘s slope, heading, course, track… Yup, the tractor is pointing to the line, but travelling parallel to it since the heading negates the XTE since the tractor is already pointing the right way to get to the line. Further complicating the matter, he is close enough now that the differential term starts telling it to "turn away’ from the line to not cross over it.

So you need to balance the fusion ( how quickly the fusion code will bring the error of the difference of fix to fix heading (the course heading) with the true vehicle heading it is pointing via the dual antenna. You can do it very quickly, but then get a wiggly set of fixes but that maybe more desirable on steep ground.

Fix to fix distance is another variable to the mix. The farther you go back the smoother the values of heading will be as the longer the line, the less the effect of slight lateral movement of position. BUT, the longer the fix to fix distance, the slower the heading is to react when turning and causes other errors. With RTK, you should be able to use 0.5 meter or less.

Yin and yang is the name of the guidance game. Constant tradeoffs.



This is what I experience with almost all hitch mounted implements. Tractor is pulled a little angle by implement so implement is off line but front of tractor pointing to the line. AG thinks no correction is needed.
But is it correct that i read in above post that dual antenna is also not getting this solved at the moment, or is very tricky to set imu fusion and fix to fix to get it to the line?

Would a compass that knows exact north be an option to see long term draft with single antenna?

Indeed, glad its not just me… It takes very little sidehill to get this effect with heavy load on the tractor, offset load (mowers etc) obviously exagerate the issue expodentially.

I am looking foward to trying a job with dual being used as IMU with fusion, I think it could make a huge difference. But sadly today its raining and so foggy I cant even see the tractor in the yard at the moment! :frowning:

What if you worked out the distance from the line you are happy with (for example 10cm) then what ever that steering angle to the line equals say 5deg?

Then you can do all the steering angle calculations off dual heading but limit that dual heading within a 5deg band of the fix/fix heading without any fusion?

Made a test version on Github - the “Sticky” branch. A short video explanation


One other thing on fusing imu/gps. Some code.

                        //if the gyro and last corrected fix is < 5 degrees, super low pass for gps
                        if (Math.Abs(gyroDelta) < 0.075) // && Math.Abs(mc.actualSteerAngleDegrees) > 2)
                            //a bit of delta and add to correction to current gyro
                            gyroCorrection += (gyroDelta * (ahrs.fusionWeight));
                            if (gyroCorrection > glm.twoPI) gyroCorrection -= glm.twoPI;
                            if (gyroCorrection < -glm.twoPI) gyroCorrection += glm.twoPI;
                            //a bit of delta and add to correction to current gyro
                            gyroCorrection += (gyroDelta * (0.2));
                            if (gyroCorrection > glm.twoPI) gyroCorrection -= glm.twoPI;
                            if (gyroCorrection < -glm.twoPI) gyroCorrection += glm.twoPI;

The line if “(Math.Abs(gyroDelta) < 0.075)” first determines how far away the gps heading and the imu heading are. If it is greater then a few degrees, then use a very fast value to merge the imu offset to match the gps heading. But then as you get closer, start using the user setting from config for that last few degrees of aligning the difference between imu and gps headings.

0.2 is quite fast considering the setting from config mught be about .005 to 0.02. It really helps eliminate that endless draft from fusion delay. All a balancing game.

Just a thought… what if roll is used in the mix?

If you know your going to slide on hills, automatic set a heading error based on the roll. Then the roll can help the intergral out by giving it a head start.

Its hard work for it when the problem is strong correction from the Left, then 5sec later strong from the right. If roll fast forwords the correction would it help?

Almost just add abit of steer angle set point up the hill, the integral can fix the mower or constant side draft and roll can cancel the hill out of it.

Is this idea I had today stupid or good?

The current intergal thing works well but it is only one point. If we have three points and for example roll moves us between those three points we can build a graph/line/table whatever you call it.

WAS is not correct - No roll, integral adjusts the centre point, Roll left integral adjusts left point, Roll right integral adjusts right point. So it builds a flat line if we were switching between them no differnt than before.

Heavy Load Pulling Us Down Hill - No roll, integral adjusts the centre point, Roll left integral adjusts left point, Roll right intergral adjusts right point. Now we can roll between theses 3 points and not have to move them too much (Roll left will have a positve integral, Roll right will have negative integral)

Then I had another thought, there is not always a hill involved. But then the options are open because the same system applys. Say we have a ground engaged tool thats offset, the integral changes with load so then all we need is just use a load cell signal to move us left or right along our three points.

Roll signal is easy because its there for everybody, but if we can use any signal depending on whats causing your integral to change the most you could use anything: Roll, Hitch Postion, Load cell on drawbar, Load cell on hitch stabilzer arms, older tractors could have analog sensor on draft spring/shaft? just some ideas.

Even the hitch postion might be enough for most if the tool is always one side? Hitch up adjust 1st point, Hitch down adjust the 2nd? Or maybe Down left, Up, Down right

About a year or so ago, maybe more tried a lot of those ideas, but the trouble is the ground is never consistent and so the compensation can make things worse and better. It really needs to do it by following the line and moving the steering accordingly on it own, ie no “special settings”.

Its really tough and very time consuming to try this stuff out, and then you think you have it and it fails in lots of other situations. I encourage people to write actual code and make it work, show it works.

I think some important math and or concepts are missing in the control system for when times are difficult for guidance.


Yes, the ground is sometimes slippery, sometimes it holds the wheels well. But if we applied a roll based drift compensation (as in 4.3.1), the tractor would be closer to the line and require less integral correction. The tractor driver, when looking at the traverse, assumes from the beginning that there will be a drift and drives next to the marker, drives a bit, looks in the mirror, sees the overlap or bypass and makes a correction. In 4.3.1 it worked perfectly well, if this “lateral drift compensation” mechanism was added to 5, this in combination with the integral would have a good effect for sure.

One more setting to mess things up is the downside, but…

A slider like 4.3 that manually added steer angle based on roll from 0 to whatever as you say would help offset some draft immediately. Like Integral it can easily be set to too much and cause wandering back and forth - but then that is what the guy in the seat is for - making sure it isn’t too much in worst case scenario and letting integral do the rest.

Can put in Config/Data/Roll settings.

So setup would be to make your autosteer as good as possible, then set a bit of roll/steerangle compensation, then set integral.

OR if you have flat land you would just leave to 0. It would also not help with side mounted equipment since there is no roll factor.

with integral working over time, it is going to take some gymnastics to add steer angle without affecting algorithms for steering and the slowly building integral.

Hill compensation also added steer angle when going over bumpy ground as well since it thought it was on a sidehill.

You can set how many degrees of additional steer angle delivered per degree of roll. It’s on the Master branch.



The Degrees of Steer Angle per Degree of roll means just that. The more you roll, the more you steer. So if you set to 0.2 then at 10 degrees of roll you will steer up the hill an additional 2 degrees. This is added after the “set” steer angles are calc’d from Stan and Pure.

I haven’t tried it in the actual tractor - but if someone has the opportunity, crank it up and see what it does. It is on the master branch on github.

So the cal would be - set your autosteer to work nice with integral and sideHill both set to zero. Go on some sidehills and set sidehill comp so you get closer to the line. Set the integral up a bit to bring you on the line.

1 Like

Would a comparison between WAS angle and actual rotation not be the best way? A rotation average over say a second compared with WAS angle over speed would quickly show if the WAS angle is roughly corresponding with what rotation it should be producing.

What all of this actually boils down to is requested WAS angle not producing the rotation required. Even if that required rotation is zero.

1 Like

Update to this,

Despite currently fighting real issues with GPS / Glitching (The GPS position will randomly glitch to somewhere miles away from the field for a split second during work, causing the steering to go hard around and cause quite chaos, even on old ESP code that worked fine with 4.3!) so really struggling to get to the bottom of that at the moment. (Anyone else seen this issue with MTZ or Franz ESP code?)


In between the random glitchs, the GPS works great, and have been testing with “Dual as IMU” and playing with the fusion, and can report that 100% this is a huge improovement over pure dual as heading, or at least, when we are on a sidehill, or have side draft… With this feature, it WILL pull you back on the line, simply said, its wonderfull!

Some improovements from my testing that would be good…

1 - Direction now very often gets confused while turning on foreends etc, requiring you to reset direction…Obviosuly having used pure dual for so long, I have never had to do this, so it gets very frustrating fast… So what would the possiblity of the code that determines “direction” to still look at pure dual heading for this calculation? So direction is always correct…

2 - Any way to add into code so it uses pure dual if heading is changing > x deg a second or something? So that when turning sharp on foreends, or using U-Turn, it will revert to pure dual as heading? Only because you simply cannot beat pure dual for being so smooth and perfect…using fusion with f2f makes the U-Turn, or even just turning on the ends manually very skippy on the display… And obviously the slightly sidehill / draft loss while turning is of no real issue…

Overall though, huge step fowards IMO, Thanks @BrianTee_Admin !

i’m working on the forward reverse thing, it seems we pretty much have to use the imu when backing up etc. I have added a “no reverse” selection as well.

Good to hear the “wrong” heading in dual is the culprit on a sidehill. I think between sidehill compensation added back in, dual as imu, and integral, it should work quite well on any crazy slope. On a 10 degree slope i can easily put the tractor 20 cm above the line.

Backing up is a bit of a pickle because now the fixes are behind the antenna - which is actually in history between the antenna and the front wheels so that cannot be used to calculate heading. Seems you need dual, an imu, sidehill compensation, etc at different times in order to get the best out of each. That makes for some nightmare programming indeed.


The main problem is the kinematic model used to calculate the steering angle (as pointed above) which cannot account for wheels slipping but is assuming a purely kinematic relation. So you end up giving a steer angle command based on a model that’s assuming the wheels are glued to the ground while they are not. The second is maybe how the fusion is working between the IMU and GPS, now it’s trying to find a midway between the two. There’s two options:

Trying to get the vehicle attitude as an output, this is basically telling where the nose is pointing. Not really necessary here, except if you go to some more refined control algorithms that can utilize the slip angle information acquired.

Second, like it’s working now, is to try to fuse the signals to reflect the true course of the vehicle. This gives you the true heading of the vehicle, which is basically what’s happening when you use the dual antenna as IMU. More importantly, you can resolve the yaw rate bias, i.e. the correction to the yaw rate measured by the gyro, which produces the correct global motion. So this way you get the “true” yaw rate of the vehicle, which you can use to control the vehicle even without a WAS as discussed here AOG without wheel angle sensor. Or you can use it to calculate how much more you need to turn the wheels to achieve the desired steering rate is you have a WAS, etc.

As the fusion needs to happen at a high rate, you need to do it on the receiver, IMU, or arduino, as you are updating the heading at say 8 Hz RTK rate, and utilizing maximum IMU update rate available to update with the IMU signal. This is what’s is roughly happening inside the commercial high-end RTK receivers that have a built-in IMU, you have position, velocity, attitude, angular velocity and accelerations as state variables, then you augment the state space with the biases for accelerations and angular velocities. For those you use a discrete time state transition model based on more or less Newton’s law and run the prediction step based on those. For the measurements you’d use for example GPS for position and yaw angle, IMU for acc/angular acc, something for velocity (in cars you typically use the ABS sensor speeds) etc. The performance of the filter is then much up to how well you choose the covariance matrices for the model and measurements. These basically define how much you trust the measurement and the model, these can and should be time varying. So if you get gargabe in from GPS for example, the variance for the yaw data measured goes up and the filter is trusting more on the model instead.

Here’s a good MSc thesis on the topic to get started on sensor fusion: https://aaltodoc.aalto.fi/bitstream/handle/123456789/47095/master_Sari_Onur_2020.pdf?sequence=1&isAllowed=y


As i always say, its open source. If someone is willing to write the fusion code so it works better, that would be beyond awesome. In a commercial environment you also have control of how things are mounted and built and a single well designed system is installed and calibrated. You would need noise models for every type of diy installation and parts and GPS systems.

I wish i had the math skills to do the same as 25 Trimble or JD engineers - but i certainly do not.


I’ve been working on this in the background a bit, as a by product of the wasless stuff. Target is to build a single board that contains IMU and GPS and a teensy to run the fusion code (the ugly proto from wasless done on a PCB). Or a separate IMU + Teensy that takes serial NMEA input.

Output would be the lat/lon aided by the IMU in case RTK drops out and a fused heading and WAS reading. Was thinking of just sending a VTG sentence out with the new heading.


Is there still an Imu used for dual GPS used aa an IMU? Or does it makes sense to fuse also the dual heading with an IMU (CMPS14) ?

What is the Best concept an dual GPS Or better to go for the Single antenna with synchronized Panda roll compensation?

Maybe a stupid question… I need to get depper into the topic.