Model Predictive Controller state space model

Hello members, I have a single-level MPC controller and trying to control the hummingbird drone. I considered the physical properties of the hummingbird drone from xacro and YAML files and used those values in the state-space model based on this paper ( https://arxiv.org/pdf/1908.07401.pdf page 9). Accordingly, my state-space looks like this:

State vector x is {position, velocity, orientation, angular velocity}

`A = numpy.zeros((12, 12)) A[0, 3] = 1 A[1, 4] = 1 A[2, 5] = 1 A[3, 3] = -Ax A[4, 4] = -Ay A[3, 7] = -g A[4, 6] = g A[6, 9] = 1 A[7, 10] = 1 A[8, 11] = 1

B = numpy.zeros((12, 4)) B[5, 0] = 1/mass B[9, 1] = 1/Ix B[10, 2] = 1/Iy B[11, 3] = 1/Iz`

where mass, Ix,Iy,Iz are from hummingbird xacro file.

I need to consider the mass, Inertias of the drone. I know that it's possible to control the drone with a single-level MPC controller (I have already implemented the controller in Python) but I am unable to simulate it in Gazebo. The drone lifts and flies away, also the behaviour is unstable and wrong. I highly doubt that my state-space is missing some important data or is not in the right format. In the hummingbird.xacro file, I see total mass, the inertia of the body, and inertia of the individual rotors. Do I have to consider rotor inertias as well in my state-space model? If so, how can I do that? Can someone please guide me on this? Even if I include the inertia values, they won't have significant effects since they are too small.

ETH has also published an MPC controller in this https://github.com/ethz-asl/mav_control_rw but the state space formulation is different and it is still 2 level controller (MPC as high level and PID as low low-level controller) therefore I didn't find it that useful for my problem. Also, if you have any other suggestion to implement a single level MPC controller, please share your thoughts.

Yours sincerely Sandeep

Can you share any of your code? There could be a lot of different issues causing unstable flight. What are the Ax, Ay on your A matrix? How are you implementing the MPC? Are you using the discretized dynamics for your implementation? MPC needs discrete time model of the system. How are you acquiring the state? A lot of these questions can be answered if you can share your implementation code. Cheers

Hi, thanks for your reply. I had almost given up :( Here is my implementation in brief. I might have defined my state vector slightly wrong in my post above. The states are actually {positions, velocities, angles, angular rates}. I have written angular velocities above and I found out recently that they might not be the same. I have also exchanged 'g' terms in A matrix. My SS data is:

mass = 0.68

# Inertias
Ix = 0.007
Iy = 0.007
Iz = 0.012
g = 9.81

# coefficients of air drag
Ax = 0.01
Ay = 0.01
Az = 0.0

# States
# {x, y, z, x_dot, y_dot, z_dot, roll, pitch, yaw, roll_d, pitch_d, yaw_d }

A = np.zeros((12, 12))
A[0, 3] = 1
A[1, 4] = 1
A[2, 5] = 1
A[3, 3] = -Ax
A[4, 4] = -Ay
A[3, 6] = g
A[4, 7] = -g
A[6, 9] = 1
A[7, 10] = 1
A[8, 11] = 1

B = np.zeros((12, 4))
B[5, 0] = 1/mass
B[9, 1] = 1/Ix
B[10, 2] = 1/Iy
B[11, 3] = 1/Iz

C = np.zeros((6,12))
C[0,0] = 1
C[1,1] = 1
C[2,2] = 1
C[3,6] = 1
C[4,7] = 1
C[5,8] = 1

D = np.zeros((6,4))

A couple of clarifications:

I am calculating discrete SS matrices from continuous matrices.
Gazebo odometry sensor gives position in inertial frame, linear and angular velocities in Body frame so I need to convert them into the inertial frame.
You can see many constraints on states in the problem because I tried to avoid crashing and to make it more 'stable'

import rospy
import cvxpy as cp
import numpy as np
import scipy.linalg as sc
import scipy.integrate as integrate
import math
from nav_msgs.msg import Odometry
from trajectory_msgs.msg import MultiDOFJointTrajectory 
from mav_msgs.msg import Actuators
from scipy.spatial.transform import Rotation

# Prediction horizon
N = 8
# sampling time
Ts = 0.1

rotor_force_constant = 8.54858e-6
rotor_moment_constant = 0.016000
arm_length = (1/np.sqrt(2))*0.17

CT = rotor_force_constant
CT_CM = rotor_force_constant*rotor_moment_constant
l_CT = arm_length*rotor_force_constant

Inertia_matrix = np.diag([0.007, 0.007, 0.012, 1])

allocation_mat_map = np.array([ [CT, CT, CT, CT], 
                              [0.0, l_CT, 0.0, -l_CT],
                              [-l_CT, 0.0, l_CT, 0.0], 
                              [-CT_CM, CT_CM, -CT_CM, CT_CM]])

allocation_mat = np.transpose(allocation_mat_map)@np.linalg.inv(allocation_mat_map@np.transpose(allocation_mat_map))

# Discrete system Matrices
Ad = sc.expm(Ts*A)
f = lambda tau:sc.expm(A*tau)
ncount = 100
tauV = np.linspace(0,Ts,ncount)
res = np.apply_along_axis(f,0,tauV.reshape(1,-1))
Bd = np.trapz(res,tauV)@B

# shapes
[nx, nu] = B.shape
ny = C.shape[0]

# reference trajectory

ref_traj = np.array([0, 0, 0.1, 0, 0, 0])

euler_angles = np.zeros(3)

# Penalties on states and inputs
Q = np.diag([50, 50, 100, 50, 50, 50, 50, 50, 50, 10, 10, 10])
R = np.diag([.1,1,1,1])
R_delta = 1*np.eye(nu)

# final cost
QN = sc.solve_discrete_are(Ad,Bd,Q,R)
LQR_K = np.linalg.inv(R + np.transpose(Bd)@QN@Bd) @ np.transpose(Bd)@QN@Ad

# Initial state
x0 = np.zeros(nx)
x0[2] = 0.1

def solve_linear_mpc(x0,reference):
    ref_state,ref_input = steady_state_calculation(reference)
    x = cp.Variable((nx,N+1))
    u = cp.Variable((nu,N))
    cost = 0
    constraints = []

    for t in range(N):
      cost+= cp.quad_form((u[:,t] - ref_input), R)
      cost+= cp.quad_form((x[:,t] - ref_state),Q)

      if (t<N-1):
        cost+= cp.quad_form((u[:,t] - u[:,t-1]),R_delta)

      constraints+= [x[:,t+1] == Ad@x[:,t] + Bd@u[:,t]]  

    cost+= cp.quad_form((x[:,N] - ref_state),QN)

    constraints+= [x[6,:]>=-np.pi/6,x[6,:]<=np.pi/6]     # constraint on roll
    constraints+= [x[7,:]>=-np.pi/6,x[7,:]<=np.pi/6]    # constraint on pitch
    constraints+= [x[9,:]>=-np.pi/6,x[9,:]<=np.pi/6]    # constraint on rolling rate
    constraints+= [x[10,:]>=-np.pi/6,x[10,:]<=np.pi/6]  # constraint on pitching rate
    constraints+= [x[11,:]>=-np.pi/10,x[11,:]<=np.pi/10]   # constraint on yaw rate
    constraints+= [x[:,0]==x0]

    problem = cp.Problem(cp.Minimize(cost),constraints)
    problem.solve(verbose = False,solver='OSQP')

    print(problem.status)

    x0 = x[:,0].value
    u0 = u[:,0].value 

    return x0,u0

def steady_state_calculation(reference):
    left_hand_side = np.vstack((np.hstack(((Ad - np.eye(nx)), Bd)),np.hstack((C, np.zeros((ny,nu))))))
    pseudo_inverse = np.linalg.inv(np.transpose(left_hand_side)@left_hand_side)@np.transpose(left_hand_side)

    dist_vec = np.zeros(ny)
    dist_matrix = np.zeros((nx,ny))

    ref_state = np.zeros(nx)
    ref_input = np.zeros(nu)
    right_hand_side = np.hstack((np.dot(-dist_matrix,dist_vec),reference))
    target_points = np.dot(pseudo_inverse,right_hand_side)
    ref_state = target_points[:nx]
    ref_input = target_points[nx:]
    return ref_state,ref_input

def trajectory_callback(data):
    ref_traj[0] = data.points[0].transforms[0].translation.x
    ref_traj[1] = data.points[0].transforms[0].translation.y
    ref_traj[2] = data.points[0].transforms[0].translation.z
    ref_rpy = Rotation.from_quat([data.points[0].transforms[0].rotation.x,
                      data.points[0].transforms[0].rotation.y,
                      data.points[0].transforms[0].rotation.z,
                      data.points[0].transforms[0].rotation.w])
    ref_rpy = ref_rpy.as_euler('xyz',degrees=False)
    ref_traj[3] = ref_rpy[0]
    ref_traj[4] = ref_rpy[1]
    ref_traj[5] = ref_rpy[2]

def gazebo_odometry_callback(data):
    x0[0] = data.pose.pose.position.x
    x0[1] = data.pose.pose.position.y
    x0[2] = data.pose.pose.position.z

    rpy = Rotation.from_quat([data.pose.pose.orientation.x,
data.pose.pose.orientation.y,data.pose.pose.orientation.z,data.pose.pose.orientation.w])
    rollPitchYaw = rpy.as_euler('xyz',degrees=False)
    orientation_W_B = rpy.as_matrix()
    velocity_B = [data.twist.twist.linear.x,data.twist.twist.linear.y,data.twist.twist.linear.z]
    x0[3:6] = orientation_W_B@velocity_B

    x0[6] = rollPitchYaw[0]
    x0[7] = rollPitchYaw[1]
    x0[8] = rollPitchYaw[2]

    ang_vel_B = [data.twist.twist.angular.x, data.twist.twist.angular.y, data.twist.twist.angular.z]
    x0[9:12] = orientation_W_B@ang_vel_B

def main():

    rospy.init_node('controller',anonymous=False)

    rospy.Subscriber('/hummingbird/command/trajectory',MultiDOFJointTrajectory,callback=trajectory_callback)
    rospy.Subscriber('/hummingbird/ground_truth/odometry',Odometry,callback=gazebo_odometry_callback,tcp_nodelay=True)
    vel_pub = rospy.Publisher('/hummingbird/command/motor_speed',Actuators,queue_size=10)

    msg = Actuators()

    while not rospy.is_shutdown():
      state = x0
      x,u = solve_linear_mpc(state,ref_traj)

      rotor_vel_squared =np.absolute(allocation_mat.dot(u))

      rotor_vel = np.sqrt(rotor_vel_squared)
      rotor_vel = np.maximum(rotor_vel,np.array([0,0,0,0]))
      # 838 rd/sec is max rpm of Hummingbird mentioned in Xacro
      rotor_vel = np.minimum(rotor_vel,np.array([838,838,838,838]))

      msg.angular_velocities = np.array([rotor_vel[0],rotor_vel[1],rotor_vel[2],rotor_vel[3]])

      rospy.loginfo(msg)
      vel_pub.publish(msg)

if __name__ == '__main__':
    main()

Please suggest to me what's wrong here.

Thanks a lot Sandeep

Okay first of all I'm not very familiar with python, so if the errors are stemming from python implementation mistakes e.g. the optimization solver, matrix operations being executed correctly, or inaccuracies in retrieving RPY from the quaternion for example, I'm not gonna be able to help. Apart from that, a couple of points:

I am not sure what you are trying to achieve with the following lines: ' f = lambda tau:sc.expm(Atau) ncount = 100 tauV = np.linspace(0,Ts,ncount) res = np.apply_along_axis(f,0,tauV.reshape(1,-1)) Bd = np.trapz(res,tauV)@B ' If their sole purpose is to compute the discrete version of your B matrix, that can simply be: Bd = TsB

I wouldn't bother including air drag in my model, it's not that important of an effect to need to be accounted for typically.
You shouldn't rotate your angular velocities. The mathematic model you are using has the attitude dynamics expressed in the Body-Fixed frame. So that's definitely an issue.
The R gains could be too large, although that's just a guess. Try having them very close to zero and see if this will change the controller's performance.
Your Ts=0.1 means you are controlling the drone at 10Hz. That is a very small controller bandwidth. You should check, e.g. using the rqt_gui and the topic monitor, the speed at which your controller publishes mav_msgs to the actuators. That speed should dictate your discretization step.
The result of 'allocation_mat.dot(u)' are the desired motor speeds squared. If any of those is negative, it should be zeroed out. Thus by taking the absolute value of 'allocation_mat.dot(u)' you are violating the controller's output significantly. I suggest you first take the maximum of 'allocation_mat.dot(u)' and [0,0,0,0], then you get the square root of the resulting values.

I hope the combination of all the above help you get closer to the desired performance. Cheers!

Hi, Thanks for the inputs, will certainly try them. A couple of questions. The angular velocity from the gazebo odometry sensor itself is expressed in the body frame. Please see nav_msgs/Odometry message and here https://github.com/ethz-asl/rotors_simulator/issues/577#issuecomment-506629855 Angular velocities in my state space formulation are considered in world frame.

Is angular velocity = angular rate here? Or are they different and need some sort of relationship as noted in https://towardsdatascience.com/demystifying-drone-dynamics-ee98b1ba882f (see section 10).

I do agree that my sampling rate of 10 hz might be too slow. However, I didn't find any other free fast solver in python. I see that CVXGEN is pretty fast but needs special license.

I will try not to confuse you here. Just know that the attitude dynamics state space representation seen in the arxiv paper you are using, is derived by linearizing , around hovering state (where angles and angular rates are zero ), the following equation: wdot ,where the image above is from the link you provided. The rotation matrix that is used afterwards in that link, transforms the body rates into Euler angle derivatives. This rotation matrix IS FUNDAMENTALLY DIFFERENT to the matrix ' orientation_W_B = rpy.as_matrix() ' , which is used to transform body-frame linear velocities into world frame linear velocities. The rotation matrix found in the link you shared is correct, but degenerates to identity close to hovering conditions, and you can feel free to disregard it. So to reiterate: -using ' orientation_W_B ' to rotate your body linear velocities into world linear velocities is necessary for your controller to match your modelled dynamics. -using the same matrix to rotate the body angular velocities is definitely wrong, and you don't really need to use the matrix provided in that link either. Simply use the body angular velocities as they are given to you by the Odometry message.

ethz-asl / rotors_simulator

Model Predictive Controller state space model #662