PySINDy for a 6DoF Quadrotor

[kaynanmatos]

I've been trying to use PySINDy to get the model of a 6dof quadrotor using simulated data, but the output looks very wacky and not even close to the dynamic equations of a quadrotor. What am I doeing wrong?

The expected output was something like this:

instead, I'm seeing results like this:

(x')' = -501152270.221 1 + 3033142288.747 theta' + 2703564331.995 phi' + 2297263501.750 psi' + 222390513.631 u1 + -58329272.820 u2 + 778224355.667 cos(1 psi) + 778182230.120 1 cos(1 psi) + 2883392367.850 theta' sin(1 theta) + 192612590.079 theta' cos(1 theta) + 2091771507.491 theta' sin(1 phi) + 1348526413.964 theta' cos(1 phi) + -2356904155.747 theta' sin(1 psi) + 181563866.573 theta' cos(1 psi) + 2968286466.895 phi' sin(1 theta) + -667115670.123 phi' cos(1 theta) + 2135231594.399 phi' sin(1 phi) + 65858937.842 phi' cos(1 phi) + 39060956.143 phi' sin(1 psi) + 2063526367.743 phi' cos(1 psi) + -615835027.109 psi' sin(1 theta) + -2654765597.021 psi' cos(1 theta) + 2711228434.008 psi' sin(1 phi) + 1466449534.126 psi' cos(1 phi) + -938186406.429 psi' sin(1 psi) + -37797600.169 psi' cos(1 psi) + -40203730.561 u1 sin(1 theta) + 159505686.644 u1 cos(1 theta) + -35118891.012 u1 sin(1 phi) + -94009266.913 u1 cos(1 phi) + -9199736.943 u1 sin(1 psi) + -373965224.258 u1 cos(1 psi) + 121626266.163 u2 sin(1 theta) + 123665070.423 u2 cos(1 theta) + -81200271.108 u2 sin(1 phi) + 25286356.740 u2 sin(1 psi) + -147464735.514 u3
sin(1 theta) + 136343650.959 u3 cos(1 theta) + 44216232.281 u3 sin(1 phi) + 116129407.314 u3 sin(1 psi) + 59359414.876 u3 cos(1 psi) + -32334082.392 u4 sin(1 theta) + -48083847.657 u4 cos(1 theta) + 91603255.000 u4 cos(1 phi)

(theta')' = -45221537455.637 phi' + -1163663932.324 psi' + -2809638565.803 theta' sin(1 theta) + -1547350071.588 theta' cos(1 theta) + -85501066622.305 theta' sin(1 phi) + 4066016051.240 theta' cos(1 phi) + 17844046243.438 phi' sin(1 theta) + 12937415299.758 phi' cos(1 theta) + -38930706497.730 phi' sin(1 phi) + 41212380399.296 phi' cos(1 phi) + -9434175514.385 phi' sin(1 psi) + -39671850929.060 psi' sin(1 phi) + 581125903.651 psi' cos(1 phi) + 35947924086.101 psi' sin(1 psi) +
-11054187291.237 psi' cos(1 psi) + 42958971.263 u1 sin(1 theta) + 26368663.760 u1 sin(1 phi) + 28803266.093 u1 sin(1 psi)

The code is as follows:

drone.py:
`import numpy as np
from RPY2Rot import RPY2Rot

class Drone:
def init(self, gravity, mass, armLength, Ixx, Iyy, Izz, init_states, init_inputs, drone_gains, simulation_time):
self.g = gravity
self.t = 0
self.dt = 0.01
self.ft = simulation_time

    self.mass = mass
    self.armLength = armLength 
    self.Ixx = Ixx 
    self.Iyy = Iyy 
    self.Izz = Izz 
    self.I = np.array([[self.Ixx, 0, 0], [0, self.Iyy, 0], [0, 0, self.Izz]])

    self.st = init_states
    self.x = init_states[0:3]
    self.dX = init_states[3:6]
    self.euler = init_states[6:9]
    self.deuler = init_states[9:12]
    
    self.dStates = np.zeros(12)

    self.U1 = init_inputs[0] 
    self.U234 = init_inputs[1:4] 
    
    self.p_err = 0
    self.p_err_prev = 0
    self.p_err_sum = 0
    self.q_err = 0
    self.q_err_prev = 0
    self.q_err_sum = 0
    self.r_err = 0
    self.r_err_prev = 0
    self.r_err_sum = 0

    self.p_des = 0
    self.q_des = 0
    self.r_des = 0


    self.KP_p = 0.008
    self.KI_p = 0.004
    self.KD_p = 0.001

    self.KP_q = self.KP_p
    self.KI_q = self.KI_p
    self.KD_q = self.KD_p

    self.KP_r = 0.01
    self.KI_r = 0.006
    self.KD_r = 0.0


def getState(self):
    state = np.concatenate((self.st[0:6], self.dStates[3:6], self.st[6:12], self.dStates[9:12]))
    return state

def EvalOEM(self):
    
    bRi = RPY2Rot(self.euler)

    R = bRi.transpose()
    self.dStates[0:3] = self.dX[0:3] # dStates = dx dy dz, ddx ddy ddz, dtheta dphi dpsi, ddtheta ddphi ddpsi
    self.dStates[3:6] = (1/ self.mass) * (np.array([0, 0, self.mass*self.g]) + R @ (self.U1 * np.array([0, 0, -1])))

    theta = self.euler[0]
    phi = self.euler[1]

    self.dStates[6:9] = np.array([[1, np.sin(theta)*np.tan(phi), np.cos(theta)*np.tan(phi)],
                                  [0, np.cos(theta), -np.sin(theta)],
                                  [0, np.sin(theta)*1/np.cos(phi), np.cos(theta)*1/np.cos(phi)]]) @ self.deuler
    
    self.dStates[9:12] = np.linalg.solve(self.I, self.U234 - np.cross(self.deuler, self.I @ self.deuler))

def UpdateState(self):
    self.t += self.dt

    self.EvalOEM()

    self.st += self.dStates*self.dt

    self.x = self.st[0:3]
    self.dX = self.st[3:6]
    self.euler = self.st[6:9]
    self.deuler = self.st[9:12] 


def AttitudeController(self, refSig):
    self.theta_des = refSig[0]
    self.phi_des = refSig[1]
    self.psi_des = refSig[2]
    self.z_des = refSig[3]

    self.theta_err = self.theta_des - self.euler[0]
    self.phi_err = self.phi_des - self.euler[1]
    self.psi_err = self.psi_des - self.euler[2]

    self.U234[0] =  (self.KP_p*self.theta_err + self.KI_p*self.p_err_sum + self.KD_p*(self.theta_err - self.p_err_prev))
    self.U234[1] =  (self.KP_q*self.phi_err + self.KI_q*self.q_err_sum + self.KD_q*(self.phi_err - self.q_err_prev))
    self.U234[2] =  (self.KP_r*self.psi_err + self.KI_r*self.r_err_sum + self.KD_r*(self.psi_err - self.r_err_prev))

    self.p_err_prev = self.p_err
    self.p_err_sum = self.p_err_sum + self.p_err
    self.q_err_prev = self.q_err
    self.q_err_sum = self.q_err_sum + self.q_err
    self.r_err_prev = self.r_err
    self.r_err_sum = self.r_err_sum + self.r_err

    self.U1 = self.mass*self.g

def getCommands(self):
    command = np.array([self.U1, self.U234[0], self.U234[1], self.U234[2]])
    return command`

main.py:

`import numpy as np
from numpy import pi
from parameters import gravity, mass, armLength, Ixx, Iyy, Izz
from drone import Drone
from RPY2Rot import RPY2Rot
from handleData import write_simulation_to_csv, plot_data

R2D = 180/pi
D2R = pi/180

init_states = np.zeros(12) # x y z, dx dy dz, theta phi psi, dtheta dphi dpsi
states_total = np.zeros((400, 18)) # x y z, dx dy dz, ddx ddy ddz, theta phi psi, dtheta dphi dpsi, ddtheta ddphi ddpsi
commands_total = np.zeros((400, 4))
time_total = np.zeros(400)
init_states[2] = -6
init_inputs = np.zeros(4) # u1 u2 u3 u4

drone_body = np.array([
[ 0.265, 0.000, 0.000, 1.0],
[ 0.000, -0.265, 0.000, 1.0],
[-0.265, 0.000, 0.000, 1.0],
[ 0.000, 0.265, 0.000, 1.0],
[ 0.000, 0.000, 0.000, 1.0],
[ 0.000, 0.000, -0.150, 1.0],
])

drone_body = drone_body.transpose()

drone_gains = np.array([
[0.2,0.0,0.15], # P_theta I_theta D_theta
[0.2,0.0,0.15], # P_phi I_phi D_phi
[0.8,0.0,0.3], # P_psi I_psi D_psi
[10.0,0.2,0.0] # P_zdot I_zdot D_zdot
])

simulation_time = 4

drone = Drone(gravity, mass, armLength, Ixx, Iyy, Izz, init_states, init_inputs, drone_gains, simulation_time)

drone_state = drone.getState()

wHb = np.block([
[RPY2Rot(drone_state[6:9]).T, drone_state[0:3].reshape(3,1)],
[np.zeros((1,3)), np.ones((1,1))]
])

drone_world = wHb @ drone_body

drone_atti = drone_world[0:3, :]

commandSig = np.zeros(4)
commandSig[0] = -10.0 * D2R
commandSig[1] = 10.0 * D2R
commandSig[2] = 10.0 * D2R
commandSig[3] = 1.0

for i in range(int(simulation_time/0.01)):
drone.AttitudeController(commandSig)
drone.UpdateState()

drone_state = drone.getState()
drone_command = drone.getCommands()

wHb = np.block([
    [RPY2Rot(drone_state[6:9]).T, drone_state[0:3].reshape(3,1)],
    [np.zeros((1,3)), np.ones((1,1))]
])

drone_world = wHb @ drone_body

drone_atti = drone_world[0:3, :]  
states_total[i, :] = drone_state
commands_total[i, :] = drone_command
time_total[i] = drone.t

write_simulation_to_csv(filename='DATA.csv', time = time_total, states= states_total, controls=commands_total)

plot_data(time_total, states_total)`

fitData.py:

`import numpy as np
import pandas as pd
import pysindy as ps
from scipy.integrate import odeint
from scipy.integrate import solve_ivp
from handleData import load_data, organize_data, viz_result

def get_states_and_controls(df):
t, x, y, z, dx, dy, dz, ddx, ddy, ddz, theta, phi, psi, dtheta, dphi, dpsi, ddtheta, ddphi, ddpsi, u1, u2, u3, u4 = organize_data(df)
X = np.stack((x, y, z, theta, phi, psi, dx, dy, dz, dtheta, dphi, dpsi), axis=-1)
dataX = np.stack((x, theta, phi, psi, dx, dtheta, dphi, dpsi), axis = -1)
ddX = np.stack((ddx, ddy, ddz, ddtheta, ddphi, ddpsi), axis=-1)
U = np.stack((u1, u2, u3, u4), axis=-1)
return t, X, dataX, ddX, U

file_dir = './DATA.csv'

df = load_data(file_dir)

t, X, dataX, ddX, U = get_states_and_controls(df)

t = t.to_numpy()
dt = t[1] - t[0]

differentiation_method = ps.FiniteDifference(order=1)
poly_library = ps.PolynomialLibrary(degree=1)
fourier_library = ps.FourierLibrary()

n_inputs = 12
n_libs = 2
inputs_temp = np.tile(np.arange(0, n_inputs, 1), n_libs)
inputs_per_library = np.reshape(inputs_temp, (n_libs, n_inputs))

inputs_temp = np.tile(np.arange(0, n_inputs, 1), n_libs)
inputs_per_library = np.reshape(inputs_temp, (n_libs, n_inputs))

inputs_per_library[0, 0] = 5
inputs_per_library[0, 1] = 5
inputs_per_library[0, 2] = 5
inputs_per_library[0, 3] = 6
inputs_per_library[0, 4] = 7
inputs_per_library[0, 5] = 8
inputs_per_library[0, 6] = 9
inputs_per_library[0, 7] = 10
inputs_per_library[0, 8] = 11
inputs_per_library[0, 9] = 11
inputs_per_library[0, 10] = 11
inputs_per_library[0, 11] = 11

inputs_per_library[1, 0] = 1
inputs_per_library[1, 1] = 2
inputs_per_library[1, 2] = 3
inputs_per_library[1, 3] = 3
inputs_per_library[1, 4] = 3
inputs_per_library[1, 5] = 3
inputs_per_library[1, 6] = 3
inputs_per_library[1, 7] = 3
inputs_per_library[1, 8] = 3
inputs_per_library[1, 9] = 3
inputs_per_library[1, 10] = 3
inputs_per_library[1, 11] = 3

print(inputs_per_library)

tensor_array = [[1, 1]]

generalized_library = ps.GeneralizedLibrary(
[poly_library, fourier_library],
tensor_array=tensor_array,
inputs_per_library=inputs_per_library,
)

optimizer = ps.SR3(threshold=100, thresholder="l1", max_iter=1000, tol=1e-1)

feature_namesX = ["x", "theta", "phi", "psi", "x'", "theta'", "phi'", "psi'", "u1", "u2", "u3", "u4"]

modelx = ps.SINDy(differentiation_method=differentiation_method,
feature_library=generalized_library,
optimizer=optimizer,
feature_names=feature_namesX)

modelx.fit(dataX, u=U, t=dt, ensemble=True)

modelx.print()
print(modelx.get_feature_names())

x_dot_test_predicted = modelx.predict(dataX, u=U)
x_dot_test_computed = modelx.differentiate(dataX, t=dt)

feature_name = ["x", r"$\theta$", r"$\phi$", r"$\psi$"]

viz_result(t, x_dot_test_computed, x_dot_test_predicted, feature_name)
`

[Jacob-Stevens-Haas]

Interesting use case! Generally speaking, there two things to look at first - (a) differentiation and smoothing, and (b) sparsification. Can you share a plot of the raw data to see what the timestep/noise looks like? and (b) what's going on with the repeated feature names? And were controls in the equation you're fitting?

Also, it looks like you're using an old version of pysindy. Ensembling is now it's own optimizer wrapper, and feature names are passed into model.fit(). A threshold of 100 should still produce more sparsity than you're seeing, but SR3 did receive a mathematical bugfix since the version you're using.

[kaynanmatos]

Thanks for answering! Here's the ploted data:

As for the feature names, I believe they are not repeated but (x theta phi psi) and their derivatives (x' theta' phi' psi').

In the expected equations the controls u1 u2 u3 u4 should be the same as F tau_theta tau_phi and tau_psi:

I will try to use the newer pysindy version.