def test_find_eqpts(self):
"""Test find_eqpt function"""
# Simple equilibrium point with no inputs
nlsys = ios.NonlinearIOSystem(predprey)
xeq, ueq, result = ios.find_eqpt(
nlsys, [1.6, 1.2], None, return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(xeq, [1.64705879, 1.17923874])
np.testing.assert_array_almost_equal(
nlsys._rhs(0, xeq, ueq), np.zeros((2,)))
# Ducted fan dynamics with output = velocity
nlsys = ios.NonlinearIOSystem(pvtol, lambda t, x, u, params: x[0:2])
# Make sure the origin is a fixed point
xeq, ueq, result = ios.find_eqpt(
nlsys, [0, 0, 0, 0], [0, 4*9.8], return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(
nlsys._rhs(0, xeq, ueq), np.zeros((4,)))
np.testing.assert_array_almost_equal(xeq, [0, 0, 0, 0])
# Use a small lateral force to cause motion
xeq, ueq, result = ios.find_eqpt(
nlsys, [0, 0, 0, 0], [0.01, 4*9.8], return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(
nlsys._rhs(0, xeq, ueq), np.zeros((4,)), decimal=5)
# Equilibrium point with fixed output
xeq, ueq, result = ios.find_eqpt(
nlsys, [0, 0, 0, 0], [0.01, 4*9.8],
y0=[0.1, 0.1], return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(
nlsys._out(0, xeq, ueq), [0.1, 0.1], decimal=5)
np.testing.assert_array_almost_equal(
nlsys._rhs(0, xeq, ueq), np.zeros((4,)), decimal=5)
# Specify outputs to constrain (replicate previous)
xeq, ueq, result = ios.find_eqpt(
nlsys, [0, 0, 0, 0], [0.01, 4*9.8], y0=[0.1, 0.1],
iy = [0, 1], return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(
nlsys._out(0, xeq, ueq), [0.1, 0.1], decimal=5)
np.testing.assert_array_almost_equal(
nlsys._rhs(0, xeq, ueq), np.zeros((4,)), decimal=5)
# Specify inputs to constrain (replicate previous), w/ no result
xeq, ueq = ios.find_eqpt(
nlsys, [0, 0, 0, 0], [0.01, 4*9.8], y0=[0.1, 0.1], iu = [])
np.testing.assert_array_almost_equal(
nlsys._out(0, xeq, ueq), [0.1, 0.1], decimal=5)
np.testing.assert_array_almost_equal(
nlsys._rhs(0, xeq, ueq), np.zeros((4,)), decimal=5)
# Now solve the problem with the original PVTOL variables
# Constrain the output angle and x velocity
nlsys_full = ios.NonlinearIOSystem(pvtol_full, None)
xeq, ueq, result = ios.find_eqpt(
nlsys_full, [0, 0, 0, 0, 0, 0], [0.01, 4*9.8],
y0=[0, 0, 0.1, 0.1, 0, 0], iy = [2, 3],
idx=[2, 3, 4, 5], ix=[0, 1], return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(
nlsys_full._out(0, xeq, ueq)[[2, 3]], [0.1, 0.1], decimal=5)
np.testing.assert_array_almost_equal(
nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5)
# Fix one input and vary the other
nlsys_full = ios.NonlinearIOSystem(pvtol_full, None)
xeq, ueq, result = ios.find_eqpt(
nlsys_full, [0, 0, 0, 0, 0, 0], [0.01, 4*9.8],
y0=[0, 0, 0.1, 0.1, 0, 0], iy=[3], iu=[1],
idx=[2, 3, 4, 5], ix=[0, 1], return_result=True)
self.assertTrue(result.success)
np.testing.assert_almost_equal(ueq[1], 4*9.8, decimal=5)
np.testing.assert_array_almost_equal(
nlsys_full._out(0, xeq, ueq)[[3]], [0.1], decimal=5)
np.testing.assert_array_almost_equal(
nlsys_full._rhs(0, xeq, ueq)[-4:], np.zeros((4,)), decimal=5)
# PVTOL with output = y velocity
xeq, ueq, result = ios.find_eqpt(
nlsys_full, [0, 0, 0, 0.1, 0, 0], [0.01, 4*9.8],
y0=[0, 0, 0, 0.1, 0, 0], iy=[3],
dx0=[0.1, 0, 0, 0, 0, 0], idx=[1, 2, 3, 4, 5],
ix=[0, 1], return_result=True)
self.assertTrue(result.success)
np.testing.assert_array_almost_equal(
nlsys_full._out(0, xeq, ueq)[-3:], [0.1, 0, 0], decimal=5)
np.testing.assert_array_almost_equal(
nlsys_full._rhs(0, xeq, ueq)[-5:], np.zeros((5,)), decimal=5)
# Unobservable system
linsys = ct.StateSpace(
[[-1, 1], [0, -2]], [[0], [1]], [[0, 0]], [[0]])
lnios = ios.LinearIOSystem(linsys)
# If result is returned, user has to check
xeq, ueq, result = ios.find_eqpt(
lnios, [0, 0], [0], y0=[1], return_result=True)
self.assertFalse(result.success)
# If result is not returned, find_eqpt should return None
xeq, ueq = ios.find_eqpt(lnios, [0, 0], [0], y0=[1])
self.assertEqual(xeq, None)
self.assertEqual(ueq, None)
def linearize(config: str,
out: str,
template: bool = False,
trim: bool = False):
"""
Linearize the model specified via the config file
"""
if template:
generate_linearize_template()
return
with open(config, 'r') as infile:
data = load(infile)
aircraft = aircraft_property_from_dct(data['aircraft'])
ctrl = ControlInput.from_dict(data['init_control'])
state = State.from_dict(data['init_state'])
iu = [2, 3]
config_dict = {
"outputs": [
"x", "y", "z", "roll", "pitch", "yaw", "vx", "vy", "vz",
"ang_rate_x", "ang_rate_y", "ang_rate_z"
]
}
sys = build_nonlin_sys(aircraft, no_wind(), config_dict['outputs'], None)
idx = [2, 6, 7, 8, 9, 10, 11]
y0 = state.state
iy = [0, 1, 2, 5, 9, 10, 11]
xeq = state.state
ueq = ctrl.control_input
if trim:
print("Finding equillibrium point...")
xeq, ueq = find_eqpt(sys,
state.state,
u0=ctrl.control_input,
idx=idx,
y0=y0,
iy=iy,
iu=iu)
print("Equillibrium point found")
print()
print("Equilibrium state vector")
print(f"x: {xeq[0]: .2e} m, y: {xeq[1]: .2e} m, z: {xeq[2]: .2e} m")
print(
f"roll: {rad2deg(xeq[3]): .1f} deg, pitch: {rad2deg(xeq[4]): .1f} deg"
f", yaw: {rad2deg(xeq[5]): .1f} deg")
print(
f"vx: {xeq[6]: .2e} m/s, vy: {xeq[7]: .2e} m/s, vz: {xeq[8]: .2e} m/s"
)
print(
f"Ang.rates: x: {rad2deg(xeq[9]): .1f} deg/s, y: {rad2deg(xeq[10]): .1f} deg/s"
f", z: {rad2deg(xeq[11]): .1f} deg/s")
print()
print("Equilibrium input control vector")
print(
f"elevator: {rad2deg(ueq[0]): .1f} deg, aileron: {rad2deg(ueq[1]): .1f} deg"
f", rudder: {rad2deg(ueq[2]): .1f} deg, throttle: {ueq[3]: .1f}")
print()
actuator = actuator_from_dct(data['actuator'])
if isinstance(actuator, InputOutputSystem):
aircraft_with_actuator = add_actuator(actuator, sys)
states_lin = np.concatenate((ueq, xeq))
linearized = aircraft_with_actuator.linearize(states_lin, ueq)
else:
linearized = sys.linearize(xeq, ueq)
print("Linearization finished")
A, B, C, D = ssdata(linearized)
print("Found linear state space model on the form")
print()
print(" dx ")
print(" ---- = Ax + Bu")
print(" dt ")
print()
print("With observarion equation")
print()
print("y = Cx + Du")
print()
print("Eigenvalues of A:")
eig = np.linalg.eigvals(A)
print('\n'.join(f'{x:.2e}' for x in eig))
linsys = {
'A': A.tolist(),
'B': B.tolist(),
'C': C.tolist(),
'D': D.tolist(),
'xeq': xeq.tolist(),
'ueq': ueq.tolist()
}
if out is not None:
with open(out, 'w') as outfile:
json.dump(linsys, outfile, indent=2, sort_keys=True)
print(f"Linear model written to {out}")