To get thing rolling l offer what l use as a PID loop, so Peter will most probably be able to confirm that l really don't have any idea what l'm doing.
There is a bit of dribble about sailing so you understand what l’m up to and it might be an insight into your own process.
There is a lot of other code removed for clarity, but the main PID concept is here.
I must admit l couldn’t believe how basic a PID block is (mine anyway), just a very small amount of basic maths.
This PID is written in C/C++ as it is coded for Arduino (but of course you could write a AOI very easily) and controls an auto pilot for a Yacht.
So the error component is how many degrees the “Boat Heading” ( PV) is from the heading we wish to go to “Heading to Steer” (setpoint).
The only thing that is different about this loop, that maybe very relevant to process control, is that l made the CMD (Control Output CV) for the control of the rudder drive motor non linear. Maybe some PID algorithms’ have this? The need for non linear (NL) CMD isn’t required all the time, but as it really costs nothing to use and really adds little to the CMD (CV) until it is needed, it seems foolish not to have it at all times IMHO.
I did this to correct when l was under spinnaker and the boat broached because there wasn’t enough rudder applied (CV) for the amount of error to correct the drive of the spinnaker, so the boat stayed broached until something happened to make it apply more CMD (a little bit more heading error) or the wind eased and the amount of helm was enough to bring the boat back on course. I could have lifted up the overall gain, but then there is too much CMD when it mostly isn’t needed (and when you are solo, broached by a spinnaker in 15knts of wind, you don't want to be playing with your AP, l will give you that tip for free).
The Compass is an IMU so hence were the madgwick/Mahoney filter question came from, which is integrating at 400hz. The Filtering and PID block are scanned at 50hz
Below is an example of basic filtering to the output of the Pots used to adjust the gains.
float Kcmd = 0.05; // 0.05 = 1/50 = 1 second smoothing
PID_Ks[1] = array for the 4 x Pots used to adjust the gains. Ks [0,1,2,3] 0= overall gain, 1= proportional gain, 2= derivative, 3=integral
integral_smoothed_1 = (1 - Kcmd ) * integral_smoothed_1 + Kcmd * PID_Ks[1];
PID_Ks[1] = integral_smoothed_1;
Below is the PD controller l use. Integral isn’t require, but when it is used it tends to wreck things.
ROT= rate of turn = how many degrees the boat is turning in 1 second ( change in PV). This gives counter rudder i.e. removes overshoot.
In my case l get this from a rated gyro, process would use something like, PV rate of change = PV - PV_old, over a time frame.
ROT is filtered like above.
Heading_error = Heading to steer – heading (setpoint- PV)
Rudder_offset = how much helm is required to correct for weather/lee helm when boat is going straight = set point offset
PID_output = PID_Ks[0] * (PID_Ks[1] * heading_error - PID_Ks[2] * ROT);
PID_output = PID_output + PID_output * abs(PID_output) / 70; // to increase rudder disproportionately to errors X= X + X* ABS(X)/70
rudder_command = PID_output + Rudder_Offset; // rudder offset set when AP key is pressed when on desired course.
CMD = rudder command
Just to show the full PID.
Integral error = 0.001;// or what ever you want
integral_error = integral_error + PID_Ks[3] * heading_error; //integral error is changing and adding back on itself.
PID_output = PID_Ks[0] * (PID_Ks[1] * heading_error - PID_Ks[2] * ROT + (Ks[3] * integral_error));
rudder_command = PID_output;
CMD= rudder_command
Unlike process, tuning is very easy, just dependent on boat speed. Set speed (say 6 knts) set P_Overall = 2, P= 0.5, D=1, get AP to do 90 degree turn and see whether there is over/under shoot. Alter Ks[2] until there is no fish tailing. Have different settings for different speeds. Normally lower Ks[0] (overall gain), unless weather conditions require more rudder, so there is less drive action, saving battery power
Hopefully Peter can see something that's maybe incorrect and then l will get a better PID for my auto pilot and we all learn something more.
But l will add that this AP steers better than any pleasure craft bought unit can and IMHO would equal a NKE (commonly used by the world solo guys). But this is really due to the compass, as the heading error (PV) is the critical component.