Made a test version on Github - the “Sticky” branch. A short video explanation
2 Likes
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;
}
else
{
//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.
Examples:
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.
3 Likes
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.
7 Likes
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?)
HOWEVER,
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.
3 Likes
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
4 Likes
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.
6 Likes
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.
5 Likes
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.
The only challenge with dual antenna is steep slopes where the heading the tractor is pointing doesn’t match the direction it is travelling. Other then that, you do not need an imu.
i think for most people just setting the sidehill compensation for slopes gets pretty close. it is very difficult to keep a tractor where it should on a slope where soli conditions change, draft, steering traction - so many things.
But in General, What will Happen when i have a dual GPS setup, and click Fix Antenna heading type and select “Dual as Imu” an CMPS14 or any other Imu would be ignored or? Further, when using Dual as Imu does the Integral needs to be 0? Because it would aim for the same Problem or?
It would replace the normal IMU so that wouldn’t be used.
The integral is based on distance and from the line, so that is not affected.
I would think the better option maybe is to use the sidehill compensation and have it oversteer on a steeper slope. So maybe try both
This would be a great oppourtunity to try and use a seperate antenna and receiver mounted to the implement. This would be a way to tell the tractor how much side draft the implement is causing. Im just not smart enough to figure it out.