Divergent Velocity

[memy009]

Good morning evryone,

Maybe this question is stupid or the response is obvious but there it is:

I'm currently trying to apply a control law (TECS: Total Energy Control System) on the horizental mode flight of a 737. To do this I kept the 737 of jsbsim, I linearized it with the FGLinearization(), then, I split up the hole dynamic to a horizental dynamic and a vertical dynamic by choosing up the states (for the horizental mode, I kept these states : Vvu, Vvw, alpha, theta and q).

Just when ploting the Vvu (velocity on x axis), we saw that it diverges completely and with huge values (more than 100000 ft/s actually). So my question is : is this normal for Vvu to diverge that much, I mean, even if the model is not controlled, there is not like a saturation or a limitation to it already??

Thank you in advance.

[seanmcleod]

So double-checking, are you saying that if you trim the 737 model at some altitude and speed, and then apply a step input to the throttle that you see velocities/u-fps increase to 100000 ft/s?

PropertyManager->Tie("velocities/u-fps", this, eU, (PMF)&FGPropagate::GetUVW);

The graph labels are very difficult to read, very fuzzy.

[memy009]

Yes, that's what I'm saying and I don't know what I'm messing. Below a screenshot of the code that make the linearization and the trim, I've change nothing in the trim or linarization functions ....

[memy009]

there is the genral code:

import jsbsim
import matplotlib.pyplot as plt
import numpy as np
from scipy import interpolate
from scipy.integrate import solve_ivp
from scipy.integrate import odeint

Launch JSBSim

fdm = jsbsim.FGFDMExec(None) # The path supplied to FGFDMExec is the location of the folders "aircraft", "engines" and "systems"
# Load model
fdm.load_model('737') # Load the aircraft 737
fdm.load_ic('cruise_init', True) # Load the initial conditions
fdm.run_ic() # Initialize the aircraft with initial conditions
# Set engines running
fdm['propulsion/engine[0]/set-running'] = 1
fdm['propulsion/engine[1]/set-running'] = 1
fdm.run()
# Trim
fdm['simulation/do_simple_trim'] = 1

Linearization with JSBSim

linearization = jsbsim.FGLinearization(fdm)
A=linearization.system_matrix
B=linearization.input_matrix
C=linearization.output_matrix
D=linearization.feedforward_matrix

Longitudinal dynamics

A_long = np.zeros((5,5)) # state vector: x_long = Vvu, Vvw, alpha, theta, q
A_long[0,:] = A[12,12], A[12,14], A[12,1], A[12,2], A[12,3]
A_long[1,:] = A[14,12], A[14,14], A[14,1], A[14,2], A[14,3]
A_long[2,:] = A[ 1,12], A[ 1,14], A[ 1,1], A[1, 2], A[1, 3]
A_long[3,:] = A[ 2,12], A[ 2,14], A[ 2,1], A[2, 2], A[2, 3]
A_long[4,:] = A[ 3,12], A[ 3,14], A[ 3,1], A[3, 2], A[3, 3]

B_long = np.zeros((5,2)) #input vector: u = D_t, D_q
B_long[0,:] = B[12,0], B[12,2]
B_long[1,:] = B[14,0], B[14,2]
B_long[2,:] = B[ 1,0], B[ 1,2]
B_long[3,:] = B[ 2,0], B[ 2,2]
B_long[4,:] = B[ 3,0], B[ 3,2]

C_long = np.identity(5)

D_long = np.zeros((5,2))

Load simulation setup

start = 0
stop = 10
ptsr = 200 # number of points between start and stop
tsolver = np.linspace(start, stop, ptsr)

x0_long = [0, 0, 0, 0, 0]
x0_lat = [0, 0, 0, 0, 0, 0]

in_t = np.array([0, stop/3-0.001, stop/3, 2stop/3-0.001, 2stop/3, stop+1])
#longi inputs
in_f1 = np.array([0, 0, 1, 1, 0, 0])
in_f2 = np.array([0, 0, 0, 0, 1, 1])
inpt_Delta_t = interpolate.interp1d(in_t,in_f1)
inpt_Delta_q = interpolate.interp1d(in_t,in_f2)

Longitudinal System Definition for simulation

#method odeint

def AClongi_a(x, t, inpt_Delta_t, inpt_Delta_q):
D_t = inpt_Delta_t(t)
D_q = inpt_Delta_q(t)
dxdt = [x[0]*A_long[0,0] + x[1]*A_long[0,1] + x[2]*A_long[0,2] + x[3]A_long[0,3] + x[4]A_long[0,4] + D_tB_long[0,0] + D_qB_long[0,1],
x[0]*A_long[1,0] + x[1]*A_long[1,1] + x[2]*A_long[1,2] + x[3]A_long[1,3] + x[4]A_long[1,4] + D_tB_long[1,0] + D_qB_long[1,1],
x[0]*A_long[2,0] + x[1]*A_long[2,1] + x[2]*A_long[2,2] + x[3]A_long[2,3] + x[4]A_long[2,4] + D_tB_long[2,0] + D_qB_long[2,1],
x[0]*A_long[3,0] + x[1]*A_long[3,1] + x[2]*A_long[3,2] + x[3]A_long[3,3] + x[4]A_long[3,4] + D_tB_long[3,0] + D_qB_long[3,1],
x[0]*A_long[4,0] + x[1]*A_long[4,1] + x[2]*A_long[4,2] + x[3]A_long[4,3] + x[4]A_long[4,4] + D_tB_long[4,0] + D_qB_long[4,1]]
return dxdt
#method solve_ivp
def AClongi_b(t, x, inpt_Delta_t, inpt_Delta_q):
D_t = inpt_Delta_t(t)
D_q = inpt_Delta_q(t)
dxdt = [x[0]*A_long[0,0] + x[1]*A_long[0,1] + x[2]*A_long[0,2] + x[3]A_long[0,3] + x[4]A_long[0,4] + D_tB_long[0,0] + D_qB_long[0,1],
x[0]*A_long[1,0] + x[1]*A_long[1,1] + x[2]*A_long[1,2] + x[3]A_long[1,3] + x[4]A_long[1,4] + D_tB_long[1,0] + D_qB_long[1,1],
x[0]*A_long[2,0] + x[1]*A_long[2,1] + x[2]*A_long[2,2] + x[3]A_long[2,3] + x[4]A_long[2,4] + D_tB_long[2,0] + D_qB_long[2,1],
x[0]*A_long[3,0] + x[1]*A_long[3,1] + x[2]*A_long[3,2] + x[3]A_long[3,3] + x[4]A_long[3,4] + D_tB_long[3,0] + D_qB_long[3,1],
x[0]*A_long[4,0] + x[1]*A_long[4,1] + x[2]*A_long[4,2] + x[3]A_long[4,3] + x[4]A_long[4,4] + D_tB_long[4,0] + D_qB_long[4,1]]
return dxdt

Launch Longitudinal simulation

#method odeint

y_a = odeint(AClongi_a, x0_long, tsolver, args=(inpt_Delta_t, inpt_Delta_q))
#method solve_ivp
y_b = solve_ivp(AClongi_b, [start, stop], x0_long, method='LSODA', t_eval=tsolver, args=(inpt_Delta_t, inpt_Delta_q))
# method='RK45'; 'RK23'; 'DOP853'; 'Radau'; 'BDF'; 'LSODA'

Extract results for plotting

#method odeint

Vvu1 = y_a[:,0]
Vvw1 = y_a[:,1]
Alpha1 = y_a[:,2]
Theta1 = y_a[:,3]
Q1 = y_a[:,4]
#method solve_ivp
Vvu2 = y_b.y[0]
Vvw2 = y_b.y[1]
Alpha2 = y_b.y[2]
Theta2 = y_b.y[3]
Q2 = y_b.y[4]

Plots

#method odeint

plt.figure(dpi=300)
plt.title('Longi ODEINT')
plt.plot(tsolver, Vvu1, 'g', label='Vvu')
plt.plot(tsolver, Vvw1, 'c', label='Vvw')
plt.plot(tsolver, Alpha1, 'b', label='Alpha')
plt.plot(tsolver, Theta1, 'r', label='Theta')
plt.plot(tsolver, Q1, 'b', label='Q')
plt.plot(tsolver, inpt_Delta_t(tsolver), 'b--', label='D_t')
plt.plot(tsolver, inpt_Delta_q(tsolver), 'g--', label='D_q')
plt.legend(loc='best')
plt.grid()

[seanmcleod]

Ah, you're doing something different to what I thought you were doing. I thought you were trimming the aircraft, retrieving the linearization matrices for use offline, e.g. to calculate PID gains etc., but were then running the aircraft in JSBSim after the trim and inputting a throttle step and then seeing wild u-fps values in JSBSim.

But I see you're taking the linearization matrices and then not using JSBSim any further, rather you're using odeint to integrate etc.

So possibly the linearization routine isn't returning a good result, or maybe you're misinterpreting which states are which?

[memy009]

Maybe, this is how I interpreted the states:

[seanmcleod]

I haven't double-checked the states yet, but you seem to have mixed up the body angular rates, at least with the names you've used.

Roll rate p is about x
Pitch rate q is about y
Yaw rate r is about z
.

[memy009]

Ok, I'll check what the linarization returns then .....