I am using accelerometer and gyroscope and try to compensate earth gravity.
I figured out use of complementary filter, that calculates pitch and roll angle.
On account of compensating gravity I am transforming the local acceleration in global world frame by rotation matrix.
Therefore I am using angles of gyroscope.
Problem: Angles of gyroscope are affected to drift over time.
Now I want to combine absolute angle of accelerometer and relative angle of gyroscope in rotation matrix, preferably in use of complementary filter.
Does someone now, how to fuse measuered angles of both sensors in rotation matrix?
That is what complementary filter of accelerometer and gyroscope looks like for calculating roll angle:
Roll_angle = 0.98(Roll_angle(-1)+Gyro_roll_angle*dt)+0.02*Accelerometer_roll_angle
I want to use the complementary filter in rotation matrix (R).
I can’t imagine that formula is correct:
R = 0.98(R(-1) * R_gyro)+0.02 * R_acc
Does someone know how to compute rotation matrix in terms of using angles of gyroscope and accelerometer?