Thursday, February 11, 2016

How to Dampen/Smooth Degrees Between 360 and 0 for Heading, Course, Speeds and Other Sailboat Instruments



I believe one of the issues I'm facing is the resolution of the readings from my boat's sensors. The MPU can output a reading incredibly fast (at speeds up to every 51 milliseconds). But that's kind of overkill for my chartplotter. Further more, the only application that I can think of that would need a heading reading of greater than once per second is radar stabilization, in which case you need 10 readings per second. Which is no problem for the MPU.

But when I print out a heading sentence once per second, it's simply taking the heading at that very instant. And as we all know, boat's sway back and forth, and that heading might not be very accurate. It's the same thing with speeds too--the second you take a reading, it might be unusually fast or slow, and then between readings it returns to its normal speed.

The way to fix this is to dampen or smooth out the sensor readings.

Dampening on an Arduino

Velocities are easy to dampen out. I adapted the Arduino tutorial found here, and came up with a quick Arduino C/C++ function to dampen out readings.

The idea is really simple. Take the average of, say, the last 10 readings, and the result is your dampened reading. This is pretty easy to implement in an Arduino code. In fact, it's a good idea, I've realized, to dampen out pretty much everything.

Since I'm only taking one reading per second for most things, I want that reading to reflect the average over the last second. So, let's start with something like roll, or vessel heel.

Before we even begin, we need to declare a total number of readings variable (this doesn't change at all), a timer variable (think of the "i" in a for loop), an array to store each individual reading, and then finally a variable that contains the sum total of all the raw readings

 // Dampening variables   
 const int ten = 10;  // the total number of readings we will dampen
 int t1 = 0;          // the timer variable that ticks up every cycle of the MPU
 float rollrun[ten];  // the array that stores each individual raw reading
 float rolltotal = 0; // the sum total of all readings

Next, we have to "initialize" the dampening variables. We do this inside the void setup() function of the code:

 for (int i = 0; i < ten; i++) {  
   rollrun[i] = 0;  
  }  

Finally, since I'm going to be doing this over and over again for pretty much every reading, I decide to declare it as a function to save on code space and to make it easier. Here's the function code, which you declare outside of anything else (don't include it in the setup or loop portions of the code).

 // Dampen Function  
 float dampen(float array[], int arraysize, float data, float &total, int t){  
  total = total - array[t];  // subtract the oldest reading from the total
  array[t] = data;           // redeclare the oldest reading in the array as the latest
  total = total + array[t];  // add the latest reading to the total
  return total / arraysize;  // divide the sum total by the total number of readings
 }  

You can refer to this or this for the entire code, or wait for an update in a couple of weeks in which I post the fully updated code file. For this, we're only going to look at the roll of the boat (for now). Here's how we call this function:

 roll = pose.y() * -1 * 180.0f / M_PI;             // declares roll in degrees
 roll = dampen(rollrun, ten, roll, rolltotal, t1); // redeclares roll as a dampened value
   
 t1++;      // t1 ticks up once every iteration, which is set to 10hz (10 times/second)
 if(t1>9){  // if t1 = 9 (one second has elapsed), reset it to 0
     t1=0;  
 }  

dampen(rollrun, ten, roll, rolltotal, t1). "rollrun" is the array in which all values are stored, "ten" is the total number of readings, "roll"is the newest/latest reading, "rolltotal" is the sum total of all values, and "t1" is the timer that ticks up every iteration. Why do I have t1 reset when it equals 9 instead of 10? Because it starts at 0, not 1. And this is important because the array has a position for point "0". An array with 10 total values is actually an array from point 0 to point 9.

The resulting output is the running average over the last second. It doesn't matter that the roll might fluctuate between -6 degrees (negative is to the left) and +5 degrees (positive is to the right). The average of those will make sense and give a more accurate reading than without dampening.

But what about degrees of heading that pass between 360 and 0 degrees?

Dampening Headings and Courses between 360 and 0 degrees

The average of a 355 heading and a 005 heading is due north, either 360 or 0 (I prefer to use heading 360 in this case, since a heading of 0 degrees just doesn't sound right). But if you plug that into the function about, it will give you a dampened value of 180 degrees--the complete opposite. It's actually really easy to fix it, and as usual, I've done all the ground work for you.

The key is to split the heading into its cosine and a sine, and take the dampened value of those. You have to use both, however. You can't just take the cosine and dampen it out. The cosine of 10 degrees is .98, while the cosine of 350 degrees is also .98. It's an identical value. So if you just dampened out the cosine, when you turn it back into a degree value, it could either be 10 degrees, or 350 degrees.

But if you incorporate the sine as well, you can figure it out. The secret lies in the atan2() function on the Arduino. I've used this before here when I derived the speed and direction of the current. Using the magic of math and other stuff, if you take the sine of the heading, and the cosine, and call atan2(sine,cosine), it will give you the correct info.

So let's dampen out our heading.

We have to declare two dampening sets, one for the sine and the cosine.

 // dampening variables  
 const int thirty = 30;  
 int t3 = 0;  
 float hdgxrun[thirty];  
 float hdgyrun[thirty];  
 float hdgxtotal = 0;  
 float hdgytotal = 0;  

However, I want to dampen the heading over the past three seconds. Since the MPU runs at 10hz, that means I'll have 30 total readings to dampen.

Then initialize the variables in the setup():

 for (int i = 0; i < thirty; i++) {  
   hdgxrun[i] = 0;  
   hdgyrun[i] = 0;  
  }  

We've already declared the dampen function, so here's the code to dampen out the heading.

 hdm = yaw - 90;
 if (yaw < 90 && yaw >= -179.99) {
   hdm = yaw + 270;  
 }  

 // Dampen it out  
 float hdgxave = dampen(hdgxrun, thirty, cos(hdm * M_PI / 180.0f), hdgxtotal, t3);  
 float hdgyave = dampen(hdgyrun, thirty, sin(hdm * M_PI / 180.0f), hdgytotal, t3);  
 hdm = atan2(hdgyave,hdgxave) * 180.0f / M_PI;
 if (hdm<0.0){  
  hdm=360+hdm;  
 }

 t3++;
 if(t3>29){
     t3=0;  
 } 

Please note that the mathematical functions use radians, instead of degrees. So I convert the heading to radians before I get the cosine or sine, and then reconvert it back into degrees after I use the atan2 function. Also, since sometimes that spits out negative degrees, I have to add 360 if needed.

Supposedly, if you try to do some sort of atan2 function or dampen between 090 and 270, it will return an error. I haven't ops tested that, and I wouldn't worry too much, since if you can move your boat 180 degrees in less than 3 seconds, you probably have bigger things on your mind (such as you massive bank account to afford such an incredible boat).

If you are trying to dampen out something like the wind, which has a degree value and a velocity, then you use the cosine  and sine routine above for the angle, but the velocity value itself can be dampened out no problem like I did for the roll.

Dampened and Smoothed Sailboat Sensors

My new code dampens out my GPS course over ground / speed over ground over the last 5 seconds, the boat speed through the water over the last 5 seconds, the heading over the last 3 seconds, the roll, pitch, rate of roll, rate of pitch, and rate of yaw over the last 1 second (why not yaw itself? Well, I do dummy, it's called heading!), and tidally induced current set/drift over the last 10 seconds.

The only thing left to do is dampen out heading as a function of rate of turn, but that might be a bit beyond me for now. I think three seconds is fine enough.


1 comment:

  1. Get the best speed reading course in India at very reasonable prices only from Rajmin Academy. With this course you can greatly improve your mind concentration and memorising ability.

    ReplyDelete