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)`