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}")