References

• Controller (it may be modified in this implementation)

[1] G. P. Falconi and F. Holzapfel, “Adaptive Fault Tolerant Control Allocation for a Hexacopter System,” Proc. Am. Control Conf., vol. 2016-July, pp. 6760–6766, 2016.

• Reference model (e.g., xd, vd, ad, addot, adddot)

[2] S. J. Su, Y. Y. Zhu, H. R. Wang, and C. Yun, “A Method to Construct a Reference Model for Model Reference Adaptive Control,” Adv. Mech. Eng., vol. 11, no. 11, pp. 1–9, 2019.

NOTICE

• (Tracking control) Add the argument pos_cmd_func (a function of time such that (t) -> position(t) ∈ ℝ^3) of constructor,

please do not add the argument pos_cmd in the inner function of Dynamics!(env::BacksteppingPositionController).

• (Set-point control) Do not add the argument pos_cmd_func (a function of time such that (t) -> position(t) ∈ ℝ^3) of constructor,

please add the argument pos_cmd in the inner function of Dynamics!(env::BacksteppingPositionController).

Wing-rock phenomenon in the roll motion of slendar delta wings [1].

Notes

Nondimensional values

According to [2], the time, state, and control input of this model are nondimensional. But here, it is used as time [s], state [rad, rad/s].

Input constraint

Input constraint in [3] is used.

Ideal parameters

The default ideal parameters, W_true, are set as the specific model obtained at the angle of attack of 25 degrees in [1]. Note that the ideal parameters in [2] are (1000 x the ideal parameters in [1]).

Refs

[1] J. M. Elzebda, A. H. Nayfeh, and D. T. Mook, “Development of an analyt- ical model of wing rock for slender delta wings,” J. Aircr., vol. 26, no. 8, pp. 737–743, 1989. [2] N. Cho, H.-S. Shin, Y. Kim, and A. Tsourdos, “Composite Model Reference Adaptive Control with Parameter Convergence Under Finite Excitation,” IEEE Trans. Automat. Contr., vol. 63, no. 3, pp. 811–818, Mar. 2018, doi: 10.1109/TAC.2017.2737324. [3] J. H. Tarn and F. Y. Hsu, “Fuzzy Control of Wing Rock for Slender Delta wings,” in 1993 American Control Conference, San Francisco, CA, USA: IEEE, Jun. 1993, pp. 1159–1161. doi: 10.23919/ACC.1993.4793048.

Geometric tracking controller for multicopter [1].

References

[1] T. Lee, M. Leok, and N. H. McClamroch, “Geometric Tracking Control of a Quadrotor UAV on SE(3),” in 49th IEEE Conference on Decision and Control (CDC), Atlanta, GA, Dec. 2010. doi: 10.1109/CDC.2010.5717652.

Notes

• 1; see [2].

References

[1] F. A. Goodarzi, D. Lee, and T. Lee, “Geometric Adaptive Tracking Control of a Quadrotor Unmanned Aerial Vehicle on SE(3) for Agile Maneuvers,” Journal of Dynamic Systems, Measurement, and Control, vol. 137, no. 9, p. 091007, Sep. 2015, doi: 10.1115/1.4030419. [2] PX4 Airframes Reference, https://docs.px4.io/master/en/airframes/airframe_reference.html.

Variables

u ∈ R^4: rotor forces

References

[1] F. A. Goodarzi, D. Lee, and T. Lee, “Geometric stabilization of a quadrotor UAV with a payload connected by flexible cable,” in 2014 American Control Conference, Jun. 2014, pp. 4925–4930, doi: 10.1109/ACC.2014.6859419.

Variables

p ∈ R^3: inertial position v ∈ R^3: inertial velocity R ∈ so(3): inertial-to-body rotation matrix (R_I2B) ω ∈ R^3: angular rate of body in inertial frame u ∈ R^4: [f, M...] where f ∈ R: total thrust, M ∈ R^3: moment

References

[1] M. Islam, M. Okasha, and M. M. Idres, “Dynamics and control of quadcopter using linear model predictive control approach,” IOP Conf. Ser.: Mater. Sci. Eng., vol. 270, p. 012007, Dec. 2017, doi: 10.1088/1757-899X/270/1/012007.

Variables

u ∈ R^4: square of rotor angular speed

Infinite-horizon continuous-time linear quadratic regulator (LQR).

Notes

• [1] is modified for hexa-x configuration; see [2].

References

[1] T. Lee, M. Leok, and N. H. McClamroch, “Geometric Tracking Control of a Quadrotor UAV on SE(3),” in 49th IEEE Conference on Decision and Control (CDC), Atlanta, GA, Dec. 2010, pp. 5420–5425. doi: 10.1109/CDC.2010.5717652. [2] PX4 Airframes Reference, https://docs.px4.io/master/en/airframes/airframe_reference.html. [3] M. C. Achtelik, K. M. Doth, D. Gurdan, and J. Stumpf, “Design of a multi rotor MAV with regard to efficiency, dynamics and redundancy,” AIAA Guid. Navig. Control Conf. 2014, no. August, pp. 1–17, 2012, doi: 10.2514/6.2012-4779. [4] https://github.com/fdcl-ftc/fault-tolerant-control/issues/62

Variables

u ∈ R^6: rotor forces

Notes

• 1; see [2].

References

[1] T. Lee, M. Leok, and N. H. McClamroch, “Geometric Tracking Control of a Quadrotor UAV on SE(3),” in 49th IEEE Conference on Decision and Control (CDC), Atlanta, GA, Dec. 2010, pp. 5420–5425. doi: 10.1109/CDC.2010.5717652. [2] PX4 Airframes Reference, https://docs.px4.io/master/en/airframes/airframe_reference.html. [3] M. C. Achtelik, K. M. Doth, D. Gurdan, and J. Stumpf, “Design of a multi rotor MAV with regard to efficiency, dynamics and redundancy,” AIAA Guid. Navig. Control Conf. 2014, no. August, pp. 1–17, 2012, doi: 10.2514/6.2012-4779. [4] https://github.com/fdcl-ftc/fault-tolerant-control/issues/62

Variables

u ∈ R^6: rotor forces

Refs

[1] C. Mracek and J. Cloutier, “Full Envelope Missile Longitudinal Autopilot Design Using the State-Dependent Riccati Equation Method,” Guidance, Navigation, and Control Conference, Aug. 1997, pp. 1697-1705, doi:10.2514/6.1997-3767.

Proportional-integral-derivative (PID) controller.

Notes

The derivative term is obtained via second-order filter. For more details, see [1] and singular perturbation theory.

References

[1] J. A. Farrell, M. Polycarpou, M. Sharma, and W. Dong, “Command Filtered Backstepping,” IEEE Transactions on Automatic Control, vol. 54, no. 6, pp. 1391–1395, Jun. 2009, doi: 10.1109/TAC.2009.2015562.

Moore-Penrose inverse control allocator.

References

• Reference model (e.g., xd, vd, ad, addot, adddot)

[1] S. J. Su, Y. Y. Zhu, H. R. Wang, and C. Yun, “A Method to Construct a Reference Model for Model Reference Adaptive Control,” Adv. Mech. Eng., vol. 11, no. 11, pp. 1–9, 2019.

Notes

d: degree of the reference model autodiff: autodiff = true for tracking time-varying command; otherwise for set-point regulation xcmdfunc(t): function of time

Wing-rock phenomenon in the roll motion of slendar delta wings at AoA=25 deg [1].

Notes

x = [x1, x2]^T x1 = ϕ (roll angle) x2 = ϕ̇ (roll rate)

Refs

[1] J. H. Tarn and F. Y. Hsu, “Fuzzy Control of Wing Rock for Slender Delta wings,” in 1993 American Control Conference, San Francisco, CA, USA: IEEE, Jun. 1993, pp. 1159–1161. doi: 10.23919/ACC.1993.4793048.

A two dimensional nonlinear oscillator introduced in [1]. Note that the system is constructed by converse HJB method [2].

References

[1] J. A. Primbs, “Nonlinear Optimal Control: A Receding Horizon Approach,” California Institute of Technology, Pasadena, California, 1999. [2] J. Doyle, J. A. Primbs, B. Shapiro, and V. Nevistic, “Nonlinear Games: Examples and Counterexamples,” in Proceedings of 35th IEEE Conference on Decision and Control, Kobe, Japan, 1996, vol. 4, pp. 3915–3920. doi: 10.1109/CDC.1996.577292.

References

[1] T. Bian and Z.-P. Jiang, “Value Iteration, Adaptive Dynamic Programming, and Optimal Control of Nonlinear Systems,” in 2016 IEEE 55th Conference on Decision and Control (CDC), Las Vegas, NV, USA, Dec. 2016, pp. 3375–3380. doi: 10.1109/CDC.2016.7798777.

Several functions are exported from utils.jl, e.g., Tuinv(T).

R is defined as the rotation matrix in [1, Eqs.(3), (4)]. Please check if it is the transpose of the rotation matrix defined in FSimZoo multicopters.

Notes

When obtaining Rddot and ωddot, one may need to obtain the derivative of b3d, which contains ev. This implies that we need to obtain acceleration (a=vdot) to get the control input f (total thrust), but the acceleration is induced by f, which results in algebraic loop.

Therefore, the a_d and higher-order derivatives are obtained from derivative filters.

p in R^3 v in R^3 u_nom: nominal control input, in R^m

Minimise J = ∫ (x' Q x + u' R u) from 0 to ∞

Variables

ν: virtual input

Notes

ν = B*u where u: control input

Time derivative estimator

Notes

running_cost: integrand; function of (x, u)

Notes

refmodel.x0 = xd refmodel.x1 = vd refmodel.x2 = ad refmodel.x3 = ȧd refmodel.x4 = äd

Notes

• r denotes integrand of integ.

α: Angle of attack [rad] q : Pitch rate [rad/s] mach : Mach number -] γ : Flight path angle [rad]

Common state structure of Multicopter.

∫e: integral of the error ê: the estimate of the error ė̂: the estimate of the error rate

x_i denotes ith (time) derivative of x

Even for scalar integrator, you should put an array of length 1; due to the limitation of in-place method Dynamics!.

Out-of-place dynamics

Out-of-place dynamics

LinearSystem + SingleIntegrator

A basic example of dynamics for multicopter considering rotor inputs u. You can use the following closure or extend the above __Dynamics! for more general models, e.g., faulted multicopters.

Variables

State

p ∈ ℝ^3: (inertial) position v ∈ ℝ^3: (inertial) velocity R ∈ so(3): direction cosine matrix (DCM) that maps a vector read in Inertial (I)-coord. to the same vector read in Body (B)-coord. For example, vB = R*vI. Or, it can be interpreted as "inverse rotation" from I-frame to B-frame. For example, x̂I = R'*[1, 0, 0] where x̂I is the x-axis of B-frame read in I-coord. ω ∈ ℝ^3: angular rate of body frame w.r.t. inertial frame (I to B)

(Virtual) input

f ∈ ℝ: total thrust M ∈ ℝ^3: torque

Parameters

D denotes the drag coefficient matrix [2].

BE CAREFUL

1. The default state is changed to unit quaternion from rotation matrix.
2. The definition of rotation matrix (R) is now the same as the DCM introduced in 1.

This is for compatibility with Rotations.jl (rotation matrix from I to B frames).

Reference

[1] T. Lee, M. Leok, and N. H. McClamroch, “Geometric Tracking Control of a Quadrotor UAV on SE(3),” in 49th IEEE Conference on Decision and Control (CDC), Atlanta, GA, Dec. 2010, pp. 5420–5425. doi: 10.1109/CDC.2010.5717652. [2] M. Faessler, A. Franchi, and D. Scaramuzza, “Differential Flatness of Quadrotor Dynamics Subject to Rotor Drag for Accurate Tracking of High-Speed Trajectories,” IEEE Robot. Autom. Lett., vol. 3, no. 2, pp. 620–626, Apr. 2018, doi: 10.1109/LRA.2017.2776353.

cross(x, y) = _hat(x) * y

y = ||x|| y_ddot = ?

y = ||x|| y_dot = ?

y = x / ||x|| y_ddot = ?

y = x / ||x|| y_dot = ?

Inverse map of _hat

Euler angle to unit quaternion, corresponding to ZYX rotation (here, RotXYZ). η = [roll, pitch, yaw] q = [s, v1, v2, v3] R ∈ ℝ^(3×3)

p in R^3 v in R^3 u: Convex.Variable(m)

Common input to force and moment transformation (default: not applied)

Common input saturation (default: not applied)

skew(x): ℝ³ → ℝ⁹ such that x×y = skew(x)*y

[1] MATLAB, https://kr.mathworks.com/help/aeroblks/6dofquaternion.html#mw_f692de78-a895-4edc-a4a7-118228165a58