Corrected angle calculations based on accelerometer gravity vector reading.  It seems to work now, steadily at a rate of 125 Hz.  Still need to add better initialization which calculates means and variances for sensor signals.  Means should be used to offset sensor readings, and variances should be used to initialize Kalman Filter covariance matrixes.