Calculating Angles from MPU6050
If I understand your question, the main issues are that the axis don't match up, and z from the accel is not accurate?

with respect to the axis: just swap the values. Rotation math is self-consistent and "correct" in many different permutations. What makes it correct is the set of equations, not any single equation; So, copying equations from one reference frame to another will yield incorrect results even if they were correct originally.

your link addresses this difference:


  "Interpreting the diagram at left is a little tricky, since the tilt
  of the X-axis actually shows rotation around the Y-axis, and the
  angling of the Y-axis shows (negative) rotation around the X-axis."


  ary = -(180/3.141592) * atan(ax / sqrt(square(ay) + square(az))); 
  arx =  (180/3.141592) * atan(ay / sqrt(square(ax) + square(az)));


should work better for you

On the second point: accelerometers can't measure yaw, the "z" equation can safely be ignored. that isn't yaw it calculates. Calculating z from the gyro alone should fix your second issue.

Other considerations:


The gyroscope tracking can be made more accurate by using the midpoint rule in your integration
Your code is not integrating the gyroscope's signal with the best estimate of its last position (rx, ry, rz), instead it integrates the gyro with separate numbers (grx, gry, grz). In effect information from the accelerometer is discarded each loop and gyro drift is not corrected for.
Thanks for including those graphs with your code samples =] rotation math is extremely tricky, and graphs help a lot