Calculate the yaw, roll, pitch of the bicycle frame from the Vector Nav

BodyFixed.all

   (1) % this code calculates the equations for the yaw, roll and pitch rates of the
   (2) % bicycle frame as functions of the body fixed rates measured by the VN-100
   (3) newtonian n
   (4) % y : yaw frame
   (5) % r : roll frame
   (6) % p : pitch frame
   (7) % v : vn-100 frame
   (8) frames y, r, p, v
   (9) motionvariables yaw', roll', pitch'
   (10) simprot(n, y, 3, yaw)
-> (11) n_y = [COS(yaw), -SIN(yaw), 0; SIN(yaw), COS(yaw), 0; 0, 0, 1]

   (12) simprot(y, r, 1, roll)
-> (13) y_r = [1, 0, 0; 0, COS(roll), -SIN(roll); 0, SIN(roll), COS(roll)]

   (14) simprot(r, p, 2, pitch)
-> (15) r_p = [COS(pitch), 0, SIN(pitch); 0, 1, 0; -SIN(pitch), 0, COS(pitch)]

   (16) % steer axis tilt
   (17) constants lam
   (18) simprot(p, v, 2, lam)
-> (19) p_v = [COS(lam), 0, SIN(lam); 0, 1, 0; -SIN(lam), 0, COS(lam)]

   (20) % get the body fixed angular velocity of the VN-100
   (21) angvel(n, v)
-> (22) W_v_n> = pitch'*p2> + roll'*r1> + yaw'*y3>

   (23) express(w_v_n>, v)
-> (24) w_v_n> = (COS(lam+pitch)*roll'-COS(roll)*SIN(lam+pitch)*yaw')*v1> + (
        pitch'+SIN(roll)*yaw')*v2> + (SIN(lam+pitch)*roll'+COS(roll)*COS(lam+
        pitch)*yaw')*v3>

   (25) % name the body fixed angular rates of the VN-100
   (26) constants wx,wy,wz
   (27) % solve for the rates of the bicycle generalized coordinates
   (28) A = [wx - dot(w_v_n>, v1>); wy - dot(w_v_n>, v2>); wz - dot(w_v_n>, v3>)]
-> (29) A[1] = wx + COS(roll)*SIN(lam+pitch)*yaw' - COS(lam+pitch)*roll'
-> (30) A[2] = wy - pitch' - SIN(roll)*yaw'
-> (31) A[3] = wz - SIN(lam+pitch)*roll' - COS(roll)*COS(lam+pitch)*yaw'

   (32) B = [yaw'; pitch'; roll']
-> (33) B = [yaw'; pitch'; roll']

   (34) solve(A,B)
-> (35) yaw' = -(wx*SIN(lam+pitch)-wz*COS(lam+pitch))/COS(roll)
-> (36) pitch' = wy + wx*TAN(roll)*SIN(lam+pitch) - wz*TAN(roll)*COS(lam+pitch)
-> (37) roll' = wx*COS(lam+pitch) + wz*SIN(lam+pitch)

   (38) % assuming pitch is zero, the equations reduce to this
   (39) yawrate = evaluate(yaw', pitch=0)
-> (40) yawrate = -(wx*SIN(lam)-wz*COS(lam))/COS(roll)

   (41) pitchrate = evaluate(pitch', pitch=0)
-> (42) pitchrate = wy + wx*SIN(lam)*TAN(roll) - wz*COS(lam)*TAN(roll)

   (43) rollrate = evaluate(roll', pitch=0)
-> (44) rollrate = wx*COS(lam) + wz*SIN(lam)

   (45) % assuming pitch is zero and roll is small, the equations reduce to this
   (46) yawrate = evaluate(yaw', pitch=0, roll=0)
-> (47) yawrate = wz*COS(lam) - wx*SIN(lam)

   (48) pitchrate = evaluate(pitch', pitch=0, roll=0)
-> (49) pitchrate = wy

   (50) rollrate = evaluate(roll', pitch=0, roll=0)
-> (51) rollrate = wx*COS(lam) + wz*SIN(lam)

   (52) % save the output