Hi, guys I need some help with AirSim when I try to learn leader-follower method to implement basic formation

[zhangyan101511]

I used leader-follower control method and the tracking controller is LQR, the input of LQR is vx, vy, wL, and the state consists of 3 defined error, when I run my code I found the follower can't follow the leader, so I try to modify my convert matrix, and finally it worked but with some wrong. I hope the follower on the left side of the leader((-1, 0) in leader's body frame coordinate system) , but the follower is behind the leader. It seems that I figured out the wrong rotation matrix, could you tell me how I should figure this out, thanks
`while (time.time()-start)<10:
#state space form(discrete)
L_state = client.simGetGroundTruthKinematics(vehicle_name='UAV0')
F_state = client.simGetGroundTruthKinematics(vehicle_name='UAV1')
(L_pitch, L_roll, L_yaw) = airsim.to_eularian_angles(L_state.orientation)
(F_pitch, F_roll, F_yaw) = airsim.to_eularian_angles(F_state.orientation)
epsi = F_yaw - L_yaw
wl = L_state.angular_velocity.z_val
pos_Lx = L_state.position.x_val + origin_x[0]
pos_Ly = L_state.position.y_val + origin_y[0]
pos_Fx = F_state.position.x_val + origin_x[1]
pos_Fy = F_state.position.y_val + origin_y[1]
lambda_xd = 0
lambda_yd = -1
lambda_bodyL_Fx = (pos_Lx - pos_Fx) * np.sin(L_yaw) + (pos_Ly - pos_Fy) * np.cos(L_yaw)
lambda_bodyL_Fy = -(pos_Lx - pos_Fx) * np.cos(L_yaw) + (pos_Ly - pos_Fy) * np.sin(L_yaw)
ex = lambda_xd - lambda_bodyL_Fx
ey = lambda_yd - lambda_bodyL_Fy

A = np.array([[0, 1, 0],
            [-1, 0, 0],
            [0, 0, 0]])
B = np.array([[-np.cos(epsi),np.sin(epsi), 0],
            [-np.sin(epsi), -np.cos(epsi), 0],
            [0, 0, 1]])
Q = np.diag([10, 10, 10])#weight of state error
R = np.diag([5, 5, 5])#weight of input
K = DLQR(A, B, Q, R)


state_now = np.vstack((ex, ey, epsi))

#run LQR tracking
input = -np.dot(K, state_now)
#control acceleration
client.moveByVelocityZAsync(input[0, 0], input[1, 0], z, duration=1, vehicle_name='UAV1')
time.sleep(0.02)`

[thoraxLabs]

Hi zanghyan, it seems that the follower's coordinates/frame of reference is global instead of what you intend it to have. So, the follower follows, but follows who??? the leaders, so try fixing its frame of reference in accordance to the leader instead of treating it as independent.

Koby,

[thoraxLabs]

B = np.array([[-np.cos(epsi),np.sin(epsi), 0],
[-np.sin(epsi), -np.cos(epsi), 0],
[0, 0, 1]])

looks like here is the issue (signs +/-)
imagine a circle with degrees of 0-90 (first quad), 91-180 (second quad), 181-270 (third quad), fourth quad 271-360

• in the first quadrant all (sin, cos, tan) are positive
• in the second quadrant (cos is positive only)
• in the third quadrant (tan is positive)
• in the fourth quadrant (cos is positive)

[x'] =[ cos (@) sin (@)] [x]
[y'] =[-sin (@) cos (@)] [y]
^.............................................. ^
follower................................leader
try cos as positive? from my understanding of the math you are telling the follower drone to be at a location of y'= negative sin and negative cos which seems to be the 181-270 degree of the leader (if your frame of reference is the leader).

Check it out, although I haven't been able to install the AirSim package on my system i think the math checks out.

good luck
Koby,

[zhangyan101511]

Hi Koby, Thank you for your detailed explanation! You’re absolutely right. I misunderstood the signs in the transformation matrix. The signs in the sin and cos functions need to reflect the correct quadrant of the leader’s position, as you pointed out. I’ll make sure to adjust the signs in the matrix accordingly to reflect the correct transformations for each quadrant. Hopefully, this will solve the issue. Thanks again for your help! With appreciation, Yanjie,

…

On Fri, Feb 14, 2025 at 4:22 PM thoraxLabs ***@***.***> wrote: B = np.array([[-np.cos(epsi),np.sin(epsi), 0], [-np.sin(epsi), -np.cos(epsi), 0], [0, 0, 1]]) looks like here is the issue (signs +/-) imagine a circle with degrees of 0-90 (first quad), 91-180 (second quad), 181-270 (third quad), fourth quad 271-360 - in the first quadrant all (sin, cos, tan) are positive - in the second quadrant (cos is positive only) - in the third quadrant (tan is positive) - in the fourth quadrant (cos is positive) [x'] =[ cos (@) sin (@)] [x] [y'] =[-sin (@) cos (@)] [y] ^ ^ follower leader try cos as positive? from my understanding of the math you are telling the follower drone to be at a location of y'= negative sin and negative cos which seems to be the 181-270 degree of the leader (if your frame of reference is the leader). Check it out, although I haven't been able to install the AirSim package on my system i think the math checks out. good luck Koby, — Reply to this email directly, view it on GitHub <#5006 (reply in thread)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/BOQN4GZV4BHMRAWY2H7N4IL2PWRUNAVCNFSM6AAAAABVGVODZ6VHI2DSMVQWIX3LMV43URDJONRXK43TNFXW4Q3PNVWWK3TUHMYTEMJZG43DONY> . You are receiving this because you authored the thread.Message ID: ***@***.***>