0

Sorry in advance if this kind of question has been asked. I am creating two sets of angles phi and theta. Also I have a direction cosine matrix constructed and out of it I am calculating Euler angles roll, pitch and yaw. Later, I use those angles to calculate angular speeds. The problem I'm having is that in the yaw angle calculation the values jump once they reach -pi/2. This causes a jump in the angular speeds. Can you help me to smooth the values out?

r = 0.18
h = 0.01
dt = 0.02
time = np.arange(0, 140, dt)
Amp = 0*np.pi/8
fphi = 0.5
ftheta = 1

roll = []
pitch = []
yaw = []

def angles(A, fp, ft, t):
    phi = np.pi * fp * t
    theta = A * np.sin(2 * np.pi * ft * t)

    return  phi, theta

phi, theta = angles(Amp, fphi, ftheta, time)

def Euler_angles(phi, theta):
    roll = np.arctan(-np.cos(phi)*np.sin(theta)/ np.cos(theta))
    pitch = np.arcsin(np.sin(phi)*np.sin(theta))
    yaw = np.arctan(-np.sin(phi)*np.cos(theta)/ np.cos(phi))
    yaw = np.unwrap(yaw)

    return roll, pitch, yaw

roll, pitch, yaw = Euler_angles(phi, theta)


def Angular_speeds(roll, pitch, yaw, dt, theta):
    omegax = []
    omegay = []
    omegaz = []
    for t in range(len(theta)):
        omegax.append((roll[(t+1)%len(theta)] - roll[t-1])*0.5/dt)
        omegay.append((pitch[(t+1)%len(theta)] - pitch[t-1])*0.5/dt)
    
        omegaz.append(-(yaw[(t+1)%len(theta)] - yaw[t-1] + (0. if yaw[(t+1)%len(theta)]-yaw[t-1] > -np.pi*0.8 else np.pi) )*0.5/dt) 
   
return omegax, omegay, omegaz

omegax, omegay, omegaz = Angular_speeds(roll, pitch, yaw, dt, theta)
Wandrille
  • 6,267
  • 3
  • 20
  • 43
SvylantisCPU
  • 9
  • 3
  • You could try adding post-treatment to `yaw`: search for 'gaps', and whenever there's a gap, remove pi * number_of_gaps_found to all subsequent values. It's easy to do with a for loop, and is probably doable with vectorized methods (but my np skills are too low for that at this time :) ). – Swifty May 09 '23 at 10:11
  • I have tried this method, but I believe I implemented this wrong. Could you by chance provide a code snippet? @Swifty – SvylantisCPU May 09 '23 at 10:17

1 Answers1

0

Here's my take on this, based on my comment (it's quite ugly, but seems to work; I just checked the values around the 1st gap though):

gaps = 0
yaw_fixed = np.zeros(len(yaw))
yaw_fixed[0] = yaw[0]
for i in range(1, len(yaw)):
    if yaw[i] - yaw[i-1] > 3:
        gaps += 1
    yaw_fixed[i] = yaw[i] - gaps * np.pi

And here's an attempt at (partially) vectorizing it:

from itertools import accumulate
gaps_array = np.array([*accumulate((int(round((yaw[i]-yaw[i-1])/np.pi, 0)) for i in range(1, len(yaw))), initial=0)])
yaw_fixed_2 = yaw - np.pi * gaps_array
Swifty
  • 2,630
  • 2
  • 3
  • 21