Go Down

Topic: Numerical Accuracy Issues - Possible coding bug? (Read 116 times) previous topic - next topic

vegeoku

Nov 22, 2014, 08:07 pm Last Edit: Nov 22, 2014, 08:25 pm by vegeoku Reason: Attaching full code
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
Code: [Select]
// 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
Code: [Select]
%   // 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.



AWOL

Unfortunately, your code doesn't compile, so until you post all of it, it's going to be hard to help.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

vegeoku

#2
Nov 22, 2014, 08:26 pm Last Edit: Nov 22, 2014, 08:27 pm by vegeoku
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.

jremington

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

AWOL

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"?
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

el_supremo

This C statement:
Code: [Select]
  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

vegeoku

#6
Nov 22, 2014, 10:22 pm Last Edit: Nov 22, 2014, 10:31 pm by vegeoku
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.

http://imgur.com/7QEKZsb

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

http://imgur.com/bHWOSNE

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.



jremington

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. http://blogs.mathworks.com/loren/2011/11/14/generating-c-code-from-your-matlab-algorithms/

Go Up