Adding numbers near 1.0 and 0.0 yeilds 0.0

Projects Programming
1

After some debugging and head banging I have narrowed an issue to a few lines of code. All I'm trying to do is to normalize the 4 element global array q so that its L2 norm is 1. The relevant code, including debugging output, looks like:

  PrintMatrix(q, 1, 4); //prints the array q as a 1x4 matrix
  int i;
  float c2 = 0.0;
  
  //calculate the norm
  for(i = 0; i < 4; i++){
    c2 += q[i]*q[i];
    Serial.print(c2);  Serial.print("\n");
  }

  //perform a single division first to save time; only multiplies will be needed. 
  c2 = 1/sqrt(c2);
  for(int i = 0; i < 4; i++){
    q[i] = c2*q[i];
  }
  
  PrintMatrix(q, 1, 4); //prints the array q as a 1x4 matrix

This code appears at the end of an attitude propagation function and gets executed every 20 ms. In general, the code works as expected and everything is happy. However, between 0.1% and 0.2% of the time, the coefficient c2 is set to zero. This proceeds to clobber all attitude information and I have to restart the algorithm. The issue typically occurs in the 2nd, 3rd, or 4th pass through the first for loop. If the issue occurs, c2 changes from a value which is displayed as 1.00 to 0.00 after executing c2 += q[ i ]*q[ i ];.

I can't see a bug in my code so I have tried a number of other equivalent solutions in the hope that I've just been staring at this too long:

  1. I replaced the for loop (or rather started with) the function normalize(q, 4) which, as the name implies, normalizes an array of length 4 to a norm of 1. I have used this function in other places of the code where it works as expected.

  2. I replaced the for loop and the statement c2 = 1/sqrt(c2); with

float c2 = 1.0/sqrt(sq(q[0]) + sq(q[1]) + sq(q[2]) + sq(q[3]));
  1. The quaternion q tends to stay close to a norm of 1 for several hundred iterations without renormalization so I added the test
  if(c2 < 0.5 || c2 > 2){
    c2 = 1.0;
    Serial.print("if was true");
  }

before and after c2 = 1/sqrt(c2) to try an preserve the attitude information.

As you may have guessed, none of these solutions worked; c2 still occasionally gets set to zero. I find it especially disturbing that the inequality never gets evaluated as true. Here is a sample output:

1.00	-0.06	-0.04	0.00
0.99
1.00
1.00
0.00
0.00	0.00	0.00	0.00

The reason I think it's an addition problem is that inserting Serial.print(q[ i ]*q[ i ]) before or after c2 += q[ i ]*q[ i ]; correctly displays the square of q[ i ]. I am using an Arduino UNO with IDE version 1.0.1 and am completely out of ideas.

As a complete side note, how would you prevent the text from becoming italicized after the statement c2 += q[ i ]*q[ i ]; without adding spaces between the 'i' and the brackets?

2

Not sure if it's the problem, but you probably meant 1.0:

c2 = 1/sqrt(c2);

3

Have you tried compiling your algorithm on your desktop, maybe using g++ (there are windows versions available). Debug the code on your desktop and verify if it has the same behavior.

If it does, it will be easier to debug on your desktop. If it doesn't then it will help isolate where the problem is occurring.

Also you may want to do some research on over and under flow problems associated with floating point calculations--my suspicion is that may be part of what your seeing.

4

Debugging on the desktop was a good idea. It turns out that NaNs are displayed as zeros by Serial.print() which is why I didn't initially catch the problem. There are a couple of square roots upstream of the block of code which I posted. Nominally, the arguments are non-negative, but as one of the sensor axes aligns with an axis in the inertial frame, round off error is enough to occasionally push the argument negative. After that happens, everything downstream is unrecoverable.

Thanks for the advice.