Numerical Accuracy Issues - Possible coding bug?

Official Hardware Mega Due
1

Hey all, first off thank you for any help you can provide.

I'm trying to calculate some values on an Arduino Due. My problem is that I have run the calculations in Matlab and I get the right answer, but when I run the same equations on my Arduino the output is not the same. I'm guessing it has to be a coding issue, but I can't find it. I've been staring at my code for hours. The other possibility is just numerical accuracy issues. A few of the numbers are very small, but the Due has true double capability, and changing all of my variables to doubles did help improve the calculations, but they are still off.

Overview of code: Take in gyroscope and accelerometer data, ax, az, and gy specifically, process the readings to generate estimates of the pitch angle, output the data.

There's a lot, I hope this is enough to find the problem. I can't include it all as it's too long to post. As far as I can tell the error lies in this bit of code:

Arduino Code

// state update 
  // 
  fhat1 = W[0][0]+W[1][0]*phi[0]+W[2][0]*phi[1];
  fhat2 = W[0][1]+W[1][1]*phi[0]+W[2][1]*phi[1];
  thetahat    = thetahat + F[1][0]*thetahatdot + fhat1 + Kmso[0]*ytildetheta;
  thetahatdot = thetahatdot                    + fhat2 + Kmso[1]*ytildethetadot;

  // weight updates
  W[0][0] = W[0][0] + deltat*Gamma*(P[0]*ytildetheta        - sigma*W[0][0]);
  W[1][0] = W[1][0] + deltat*Gamma*(P[0]*phi[0]*ytildetheta - sigma*W[1][0]);
  W[2][0] = W[2][0] + deltat*Gamma*(P[0]*phi[1]*ytildetheta - sigma*W[2][0]);
  
  W[0][1] = W[0][1] + deltat*Gamma*(P[1]*ytildethetadot        - sigma*W[0][1]);
  W[1][1] = W[1][1] + deltat*Gamma*(P[1]*phi[0]*ytildethetadot - sigma*W[1][1]);
  W[2][1] = W[2][1] + deltat*Gamma*(P[1]*phi[1]*ytildethetadot - sigma*W[2][1]);

Compared to

Matlab code

%   // state update
  fhat(1) = w(1,1)+w(2,1)*phi(1)+w(3,1)*phi(2);
  fhat(2) = w(1,2)+w(2,2)*phi(1)+w(3,2)*phi(2);
  xhat(1) = x(1) + F(1,2)*x(2) + fhat(1) + K1*ytildetheta;
  xhat(2) = x(2) + fhat(2) + K2*ytildethetadot;

%   // weight updates
  what(1,1) = w(1,1)+ deltat*gamma*(P(1)*ytildetheta        - sigma*w(1,1));
  what(2,1) = w(2,1)+ deltat*gamma*(P(1)*phi(1)*ytildetheta - sigma*w(2,1));
  what(3,1) = w(3,1)+ deltat*gamma*(P(1)*phi(2)*ytildetheta - sigma*w(3,1));
  
  what(1,2) = w(1,2)+ deltat*gamma*(P(2)*ytildethetadot        - sigma*w(1,2));
  what(2,2) = w(2,2)+ deltat*gamma*(P(2)*phi(1)*ytildethetadot - sigma*w(2,2));
  what(3,2) = w(3,2)+ deltat*gamma*(P(2)*phi(2)*ytildethetadot - sigma*w(3,2));

The values of fhat do not match, which leads me to believe that the w equations are not being calculated correctly. Possibly an indexing issue? I'm used to Matlab's indexing, so zero indexing and arrays of arrays get a little confusing when dealing with C code.

twip_v4.zip (189 KB)

2

Unfortunately, your code doesn't compile, so until you post all of it, it's going to be hard to help.

3

I've attached the actual code to my original post. It's not boiled down, but I can try to cut out the unnecessary code if you give me some time. The problem is the Arduino code won't work unless there is an accelerometer and gyroscope hooked up at the bare minimum.

4

What do you mean by "do not match"? Are they completely dissimilar, or just not quite the same? Some examples would help.

5

I'm sorry, I'm not about to wade through 190K of zip file.
Can you say if the variables on the Arduino are declared as "float" or "double"?

6

This C statement:

  thetahat    = thetahat + F[1][0]*thetahatdot + fhat1 + Kmso[0]*ytildetheta;

references F[1][0] but the Matlab code refers to F(1,2).
Did you transpose the F array when converting to C or is there an error in this reference?

I notice that you've declared some variables "double". On other Arduinos a "double" is actually only a 32-bit "float". I don't know whether the Due library emulates a "double" or whether it also uses only "float". If you're only having problems with precision that would be worth finding out.

Pete

7

jremington,

The calculations follow the same trend, but are not accurate. Here is an image of the two signals compared. Ideally the error should be less than one degree. At the beginning the two signals match fairly closely, but diverge a few second into running.

Imgur

Also, this is the comparison of the fhat variables. They are no where close to accurate.

Imgur

Awol,

That is why I tried posting a snippit. I have declared all variables relating to this calculation as double's. Also note that most of that is the data used in comparing the results in Matlab, not actual code. The functions of note are the DMSO.m file and inside the twip_v4 folder the MSO.ino file.

el_supremo,

Yeah I must have declared the F variable wrong, but I did testing and verified that referencing F[1][0] is the correct variable for that part of the calculation.

I also know about the limit on Arudino regarding floats, however I am running this on an Arduino Due, which is listed as supporting full double types.

8

You pictures don't help much, but your statement "They are no where close to accurate" indicates a programming error, not numerical precision problems.

Because of the confusing difference in array indexing between Matlab and C, you should print out values of as many variables as you have patience for, for both implementations, to determine the location of the programming error.

It is possible to have Matlab output C code but I have never experimented with that option. Generating C Code from Your MATLAB Algorithms » Loren on the Art of MATLAB - MATLAB & Simulink

9

Alright. I've got my code working finally. I couldn't find the error in the scalar equations, as far as I could tell they were right, but clearly there is a bug in that part of the code. I didn't go into each variable splitting up the equations as much as possible, which would probably have helped, it's just a lot to sift through. What I figured out was a workaround.

I found a C++ matrix library called Eigen that I implemented and used to do the calculations. I've done some testing on it, double type was not even necessary for accuracy. Some of the numbers are small, but floats work just fine. The library is slightly slower than my old code, which was expected, but it's not too bad. It still works out for my application.

Now just to figure out that original bug. That kind of stuff bugs me (heh).