I have read William Premerlani's theory paper several times and a lot of it is beginning to soak in, but I must be missing somthing in my implementation.
I am trying to keep an accurate view of earth-relative pitch and roll angles on an embedded device with 3 axis of rate gyros and 3 axis of accelerometers.
My approach is apparently flawed, and I'd appreciate any and all feedback. The approach is to maintain only the bottom row of R matrix as follows:
(1) initialize the bottom row of R matrix floating point values (eventually I'll use integer but for now, I'd be happy to get the floating point working even though it's much CPU cycles) by performing necessary trig operations on an initial known, stable value for pitch (theta) and roll (phi), obtained from accelerometer with nothing but 1g acting on the accelerometers.
(2) thereafter, perform the following at a fixed, quick time interval (I've tried both 20 ms and 40 ms intervals).
(2A) measure the three gyro ADC values.
(2B) construct the assymetric matrix on page 15 of the theory paper by multiplying these readings (normalized to radians per second) by the time interval (ie, 0.020 or 0.040).
(2C) update the bottom row of the R matrix by multiplying its current row values by this assymetric matrix.
(2D) calculate current pitch from -asin ( R matrix row 3, col 1)
(2E) calculate current roll by knowing 2D value and using asin of R matrix row 3, column 2)
So... I'm only maintaining the bottom row of R matrix.
Somewhere, my logic is flawed! I have a fixture on which I can rotate around earth's yaw axis and also adjust and fix target's roll and pitch angles relative to earth. I am constantly feeding the values back for real-time display at PC.
I fix the pitch at 45 degrees and roll at 0 degrees, for example. Before turning the fixture, I see values at the PC for pitch, roll, and also the 3 values for the bottom row of the R matrix.
Before moving the fixture, I see a nice 45 degree pitch, 0 degree roll, and -0.707, 0, 0.707 displayed at the PC. These drift a bit after 20-30 seconds, but that's ok, for now, since I'm doing nothing to correct for drift yet.
But when I spin the fixture, the values immediately go very wrong.
What flaw(s) are in my approach?
Any and all comments are greatly appreciated.