

In [1]:

    
import sympy as sym



In [2]:

    
import sympy.physics.mechanics as me

The bicycle rear frame is oriented in the Newtonian reference frame by the yaw, roll and pitch angles which are Euler 312 body fixed rotation coordinates.



In [3]:

    
yaw, roll, pitch = sym.symbols('yaw roll pitch')



In [4]:

    
N = me.ReferenceFrame('N')



In [5]:

    
rearFrame = N.orientnew('C', 'Body', [yaw, roll, pitch], '312')

The VectorNav accelerometer reads out the acceleration in the three rear frame body fixed coordinates and give the measure numbers of the gravity vector. $$\bar{g} = c_x\hat{c}_x + c_y\hat{c}_y + c_z\hat{c}_z$$



In [6]:

    
cx, cy, cz = sym.symbols('cx cy cz')



In [7]:

    
gravity = cx * rearFrame.x + cy * rearFrame.y + cz * rearFrame.z

The gravity vector must equal $|\bar{g}|\hat{n}_z$ because it is always in the direction of the gravitational field. This means the measure numbers of $|\bar{g}|\hat{n}_z$ in the rear frame must equal the accelerometer readings.



In [8]:

    
eqx = me.dot(gravity.magnitude() * N.z, rearFrame.x) - cx



In [9]:

    
eqy = me.dot(gravity.magnitude() * N.z, rearFrame.y) - cy



In [10]:

    
eqz = me.dot(gravity.magnitude() * N.z, rearFrame.z) - cz



In [11]:

    
sym.solve(eqy, roll)









    Out[11]:





[asin(cy/sqrt(cx**2 + cy**2 + cz**2))]



In [12]:

    
sym.solve(eqx, pitch)









    Out[12]:





[-asin(cx/(sqrt(cx**2 + cy**2 + cz**2)*cos(roll)))]



In [12]: