search

pms67 / Attitude-Estimation

MatLAB and Python implementations for 6-DOF IMU attitude estimation using Kalman Filters, Complementary Filters, etc.
http://philsal.co.uk/projects/imu-attitude-estimation
MIT License
200 stars 63 forks source link

handling inversion #1

Open belm0 opened 5 years ago

belm0 commented 5 years ago

The imu get_acc_angles() function output is incorrect when the IMU is inverted (phi and theta converge back to 0).

It seems to be due to loss of sign in the 2nd argument to the atan2 calls. I'd like to figure out how to correct it.

https://github.com/pms67/Attitude-Estimation/blob/44a4970d33268dc9ac06fc548e2935200238ffd3/imu.py#L48-L52

Thank you for the excellent article and sample code!

belm0 commented 5 years ago

This seems to work (from https://stackoverflow.com/questions/3755059/3d-accelerometer-calculate-the-orientation):

phi = math.atan2(ay, az)

However from https://www.nxp.com/files-static/sensors/doc/app_note/AN4248.pdf, there also seems to be a different way to compute theta:

phi = math.atan2(ay, az)
theta = math.atan2(-ax, ay * math.sin(phi) + az * math.cos(phi))