A WASless system requires an encoder on the steering wheel whose data is essential to estimate the degrees of steering and to be more precise. This can also prevent play in the steering joints which can affect the steering precision of a was.
That to me is basically pointless, in the scope of what I was trying to achieve anyway. If you’re putting a sensor anywhere it might as well be on the steered axle. Wheel encoders are of limited value on hydrostatic steer as there is inherent slip and creep in the system that basically adds difficult to quantify error.
Neither of these sensors give the absolute yaw rate we are looking for.
4 Likes
Agree, you can’t use averaging. But a good Kalman filter is a different beast, it uses a prediction model to check the feasibility of measured values and then decides which values to use for the update. I checked you code and the bump rejection is a good idea, dead simple. Why it (probably) works is that the pitch rate also has an effect on yaw in the reference coordinate frame when there’s nonzero roll or pitch of the vehicle. The motivation is to use GPS heading to get rid of the sensor bias.
Looking at CPU power of Teensy, should be no problem to run a real time filter, especially for yaw only.
1 Like
I did try a simple kalman filter (in the code I think) but found it better in difficult conditions without it. Not sure if it’s enabled or not in the code I posted. I have that many versions 
. I always seemed to end up back at the most simple code.
I haven’t had time to look at it for many months but I still think it is an interesting and probably more accurate method in less ‘easy’ field conditions.
On a prairie with wide tools pretty much anything will make a good job and seem fine. In the sort of conditions / tool sizes I, and many other European farmers have to deal with, the tech has got to be more intelligent.
This is what I had in mind for the filter. Basically it’s using the roll / pitch rates and angles directly from IMU, and running an estimate for yaw angle, rate and bias. Just need to figure a sensible way to estimate the covariances and test it out.
The cosines etc. come from the transformation from IMU to navigation coordinates. Two different measurement models can be run at different rates, use the IMU data to update the rate & bias estimates at high rate (50 Hz possible from BNO08x?) and run the GPS update at 8 Hz. So it will need the GPS data coming into the Teensy as well, probably needs some switching rule for the GPS update in heavy cornering.
So in short the idea is to use the yaw angle to calibrate that against the yaw angle from GPS to resolve the gyro bias and then just keep running an update for the yaw rate with the bias removed. Makes sense? Don’t know 
1 Like
Hello,
Yes, up to 400hz for Game Rotation Reports, if only this report is active. And assuming the I2C bus is able to support this communication rate (depend of the speed of the I2C bus).
Math
1 Like
Got first rid of the yaw rate as a separate variable, and reduced the system to yaw angle and bias. So now it’s using the pitch and yaw rate as control inputs to the filter. Actually, it was just one set of brackets missing in the 3 variable code and that works just equally as this one. Just have one set of test data for now, but some results:
Here’s the integrated course angle from accelerometer pitch and yaw rates, with a filter update with 5 Hz GPS heading. First pic integrated, second GPS “true course” at 50 Hz (but the filter is using it only with 5 Hz rate, i.e. always going 10 steps in between with IMU integration.) and the error between the two
This is what it looks like with pure IMU integration:
And then the yaw rate values, first the yaw rate bias calculated by the Kalman filter and the second pic comparison of raw yaw rate measurement and the bias corrected (orange and blue lines, respectively)
Very preliminary, but at least it gives something reasonable. All the variances are just pulled from the sleeve for now, need to figure those out properly etc. Maybe use the GPS estimated position error and velocities to scale the measurement uncertainty. Process noise based on sensor characteristics and system matrix. And get a better dataset with known sensors, just pulled the csv data off from one of the linked articles above (Balzar et al)
2 Likes
And some progress with the 3 DOF state vector version, using now the GPS estimated position error as a scaling factor for the GPS angle estimate, need to refine those still a bit and set some tuning parameters, maybe add the GPS speed to the equation as well. At least the mean square error is now making sense so should be quite OK. Running IMU update at 50 Hz and GPS at 5 Hz in this one:
UPDATE: And oh what a difference a bit of filter tuning made 
Set up the process variance as it should be as a timestep dependent matrix and calculated a rough variance number based on max vehicle angular acceleration of 80 deg/s^2. Bias variance set as 10^-12. Measurement uncertainty for IMU assuming 0.5 deg/s bias and for GPS angle uncertainty usings GPS EPE^4. Shifted the yaw rates a bit for easier readability in the figures, the curves sit on top for the yaw rate. Basically it’s now trusting the model more and regarding the GPS heading jumps as gargabe, as it should.
9 Likes
Wow. I’m amazed by the smart people on this forum!
So once a reliable yaw rate can be determined, will that be a substitute for the steering angle? I suppose you could easily calculate a virtual steering angle, but maybe just the yaw rate is good enough to work with pure pursuit?
It occurs to me that I was sorely mistaken about the applicability of this work to terrain compensation. On the contrary, if the IMU through your math and filtering, can yield a reliable heading (and faster than the one you get from GPS), that would allow more accurate terrain compensation. Which brings up an interesting question (perhaps chicken vs egg), would the heading get even more accurate if the GPS portion of your code was fed by the terrain-correct GPS position? Or is it best to just stick to using the raw GPS calculated heading (perhaps filtered) even though the machine rolling over bumps and waving the GPS receiver back and forth would introduce some oscillations into the GPS-calculated heading. Hope that made sense.
4 Likes
As for the steering angle, you can convert it either way. Either calculate the virtual steering angle from yaw rate or target yaw rate from AOG (which is already calculated by AOG as noted by @BrianTee_Admin earlier in the thread) like this:
@Alan.Webb used the virtual steer angle in his code, using tan x = x as approximation.
As for using IMU + GPS fusion, yes you vould do it for the position and velocity as well, just increases the system size. Roll and pitch seem to work decently with the 6 DOF fusion taking place in BNO already.
1 Like
Me too! This is above me to be honest but still very interesting. I took a simplistic view given the quality of the BNO085 output and also the actual vagueness of a standard WAS on anything but very stable very flat ground.
2 Likes
It’s still the same simple working principle, just using a bit of extra data available to make the yaw rate signal more reliable. In the same way you could fuse the GPS speed and IMU acceleration sensors to get a more reliable speed value. Of course it needs a bit of extra hassle to get the GPS data to the Teensy.
The filtering is maybe a bit misleading name as what the Kalman filter is doing is a prediction step based on the model, i.e. integrating the gyro signal to get the yaw angle and then a correction step based on the measurements to update the estimate. On both steps it’s updating the covariance matrices, i.e. the algorithm is trying to figure out which it’s trusting more, the measurement or the model. So if you get a big jump in GPS, it’s reflected in the GPS yaw variance which goes up, hence the Kalman filter is trusting more on the integrated value from the model.
Why we’re interested in the yaw angle in the model, is because that’s easily measured from GPS. From this we can calculate what the difference between the vehicle heading and the heading computed based on the gyro yaw rate is, and this is how you end up with the bias estimate. So the yaw angle is just a means to an end to get a better estimate for the yaw rate.
4 Likes
So this could be used for the GPS+IMU heading fusion?
Yes, that’s what it’s doing basically.
1 Like
this is cool !!! I understood it was for the was less in the teensy, sorry
Well, it’s doing both of the things at once. The filter is for getting a better yaw rate estimate, but as a side product you get a fused IMU + GPS heading estimate
2 Likes
Some results now with BNO080 on desk setup, just a short set of data of first a yaw motion, then pitching, then rolling, then roll/pitch pose yawing motions. Here with constant covariance matrices:
Here scaling the IMU variance with the pitch rate:
You can see in the pic below that the signal is getting smoother during the pitching motion as it’s trusting the IMU less, which is reflected in the mean square error as well. Getting slowly somewhere… Next maybe move the filter to the Teensy, now I just recorded data and ran the filter on python. Of course it’s bit cheating as I used the BNO heading angle to replicate the compass for now, just to see that the fusion algorithm works OK.
Professional test rig:
1 Like
Alright, the testing continues, still had some sign errors in the code. Now it should be correct finally, and also took into account the coordinate transformation from IMU coordinates to the navigation coordinates, i.e. take the rotation matrix and calculate the yaw rate from gyroz and gyroy rates.
Now, the interesting bit. Here is the corrected version with the process covariance matrix (i.e. how reliable the model is) calculated from a maximum yaw acceleration of 80 deg/s^2, which would be a max value for passenger vehicles.
Now, that value is rather high. So I took from @Alan.Webb ino the yaw acceleration limit value of 0.075 rad/s^2, roughly 5 deg/s^2. So taking that same value and scaling the process uncertainty with the square of the ratio, we get:
PIcking an intermediate value, say 10 deg/s^2 for the angulr acceleration of the tractor, we get bit more of the dynamics back:
Of course this is just me waving the BNO080 around over the desk, for real filter tuning need to hook it up to the machine.
4 Likes
Really interesting. Doing my best to understand!.
Waving around on a desk Looking at graphs is how I played with ideas. Made it a lot easier to see any latency too.
