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

[zyj101511]

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,