![Page 1: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/1.jpg)
Attitude control and autonomous navigation ofquadcopters
Pantelis Sopasakis
KU Leuven, Dept. of Electrical Engineering (ESAT), STADIUS Center for DynamicalSystems, Signal Processing and Data Analysis.
v3.1.12 – 2017/06/25 at 19:21:12
![Page 2: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/2.jpg)
Outline
I Problem statement and objectivesI Control Theory
X State space systemsX Stabilisation with linear state feedbackX Linear Quadratic RegulatorX Reference trackingX Linear State Observers
I ApplicationX Quaternions and rotationsX Attitude dynamicsX ImplementationX Autonomous navigation
1 / 174
![Page 3: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/3.jpg)
I. Attitude control problemstatement
![Page 4: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/4.jpg)
Attitude control
2 / 174
![Page 5: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/5.jpg)
Intuition
3 / 174
![Page 6: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/6.jpg)
Attitude control: Problem statement
1. By controlling the voltages V1, V2, V3 and V4 at the four motors andobtaining measurements from the inertial measurement unit (IMU) makethe quadcopter hover, that is φ = 0 (no pitch), θ = 0 (no roll), ψ = 0 orψ = 0 (no yaw).
2. Follow the reference signals (φr, θr, ψr) and the throttle reference signalτr from the RC.
4 / 174
![Page 7: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/7.jpg)
Attitude control: Workflow
Nonlinear model
Choose inputs,states and outputs
Find equilibriumpoints Linearise
DiscretiseSimulate
Check controllabilityand observability
no
yes
Design controller
for ref. tracking
Design Observer
Simulateand evaluate
Plot poles
not satisfied satisfiedImplement in C
Testthoroughly
Plug into Zyboand integrate
Done!
5 / 174
![Page 8: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/8.jpg)
II. State Space Systems
![Page 9: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/9.jpg)
States, inputs and outputs
I States: Those variables that provide all we would need to know aboutour system to fully describe its future behaviour and evolution in time,
I Outputs: Variables, typically transformations of the state variables,that we can measure
I Inputs: Variables which we have the ability to manipulate so as tocontrol the system
I Disturbances: Signals which affect the system behaviour and can bethought of as input variables over which we do not have authority(e.g., weather conditions).
6 / 174
![Page 10: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/10.jpg)
State space systems
State space systems are described in continuous time by
x(t) = f(x(t), u(t)),
y(t) = h(x(t), u(t)),
where x ∈ IRnx is the system state vector, u ∈ IRnu is the input vectorand y ∈ IRny is the vector of outputs.
Regularity conditions on f for the system to have unique solutions: check out Caratheodory’s Theorem.
7 / 174
![Page 11: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/11.jpg)
Discrete-time state space systems
The discrete time version is
xk+1 = f(xk, uk),
yk = h(xk, uk),
where k ∈ N.
8 / 174
![Page 12: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/12.jpg)
Why state space?
I Can be directly derived from first principles of physics
I Suitable for multiple-input multiple-output systems
I Enable the study of controllability, observability and other interestingproperties
I Time-domain representation: more intuitive
9 / 174
![Page 13: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/13.jpg)
Equilibrium points
A pair (xe, ue) is called an equilibrium point of the continuous-timesystem
x(t) = f(x(t), u(t)),
iff(xe, ue) = 0.
If u(t) = ue and x(0) = xe, then x(t) = xe for all t > 0.
10 / 174
![Page 14: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/14.jpg)
Equilibrium points
A pair (xe, ue) is called an equilibrium point of the discrete time system
xk+1 = f(xk, uk)
iff(xe, ue) = xe.
Then, if uk = ue for all k and x0 = xe, then xk = xe for all k ∈ N.
11 / 174
![Page 15: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/15.jpg)
Discretisation
Excerpt from Wikipedia:
In mathematics, discretization concerns the process oftransferring continuous functions, models, and equations intodiscrete counterparts.
12 / 174
![Page 16: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/16.jpg)
Discretisation: the Euler method
Recall that
x(t) = limh→0
x(t+ h)− x(t)
h,
so, for small h
x(t) ≈ x(t+ h)− x(t)
h.
Using this approximation
x(t) = f(x(t), u(t)),
is approximated — with sampling period h — by the discrete system
xk+1 = xk + hf(xk, uk),
where xk is an approximation for x(kh).
13 / 174
![Page 17: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/17.jpg)
Discretisation: the Euler method
0 0.2 0.4 0.6 0.8 1
0.4
0.5
0.6
0.7
0.8
0.9
1
x = −x2, x(0) = 1
Exact
Euler
14 / 174
![Page 18: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/18.jpg)
Discretisation: linear systems
When the continuous-time system has the form
x(t) = Ax(t) +Bu(t),
then we know that its solution is
x(t) = eAtx(0) +
∫ t
0eA(t−τ)Bu(τ)dτ
Assume that u(t) is constant on [kh, (k + 1)h) and equal to u(k) = ukand define Ad = eAh and Bd =
∫ h0 e
AτBdτ . Then
xk+1 = Adxk +Bduk,
is an exact discretisation of the original system.
In MATLAB use c2d — read the documentation for help.
15 / 174
![Page 19: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/19.jpg)
Linearisation
The best linear approximation of a function f : IRn → IRm about a pointx0 is given by
f(x) ≈ f(x0) + Jf(x0)(x− x0)
For functions f : IRn × IRm → IRn this reads
f(x, u) ≈ f(x0, u0) + Jxf |(x0,u0) (x− x0) + Juf |(x0,u0) (u− u0).
16 / 174
![Page 20: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/20.jpg)
Jacobian matrix
Let x = (x1, x2, . . . , xn) and
f(x) =
f1(x1, x2, . . . , xn)f2(x1, x2, . . . , xn)
...fn(x1, x2, . . . , xn)
The Jacobian matrix of f is a function Jf : IRn → IRn×n defined as
Jf(x) =
∂f1∂x1
(x) ∂f1∂x2
(x) · · · ∂f1∂xn
(x)
∂f2∂x1
(x) ∂f2∂x2
(x) · · · ∂f2∂xn
(x)...
.... . .
...∂fn∂x1
(x) ∂fn∂x2
(x) · · · ∂fn∂xn
(x)
17 / 174
![Page 21: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/21.jpg)
Jacobian matrix
Let x = (x1, x2, . . . , xn) and
f(x) =
f1(x1, x2, . . . , xn)f2(x1, x2, . . . , xn)
...fn(x1, x2, . . . , xn)
The Jacobian matrix of f is a function Jf : IRn → IRn×n defined as
Jf(x) =
∂f1∂x1
(x) ∂f1∂x2
(x) · · · ∂f1∂xn
(x)
∂f2∂x1
(x) ∂f2∂x2
(x) · · · ∂f2∂xn
(x)...
.... . .
...∂fn∂x1
(x) ∂fn∂x2
(x) · · · ∂fn∂xn
(x)
17 / 174
![Page 22: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/22.jpg)
Linearisation
Consider the continuous time system
x(t) = f(x(t), u(t)),
choose an equilibrium point (xe, ue) and define
∆x(t) = x(t)− xe,∆u(t) = u(t)− ue
Then, it is easy to verify that the system linearisation is written as
ddt∆x(t) = A∆x(t) +B∆u(t),
where A = (Jxf)(xe, ue) and B = (Juf)(xe, ue).
18 / 174
![Page 23: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/23.jpg)
Simulations
In MATLAB we may use
1. ode23: fast and of decent accuracy
2. ode45: less fast but more reliable
3. Simulink and S-functions
In C++ we may use odeint.In Python try scipy.integrate.ode.
In this project we provide to you a simulator in MATLAB.
19 / 174
![Page 24: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/24.jpg)
III. Stabilisation of Linear Systems
![Page 25: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/25.jpg)
Controllability
Linear time-invariant (LTI) discrete-time systems:
xk+1 = Axk +Buk
We say that the system is controllable if for every x1, x2 ∈ IRnx there is afinite sequence of inputs (u0, u1, . . . , uN ) which can steer the system statefrom x1 to x2 (we can move the system to any point x2 starting from anypoint x1).
20 / 174
![Page 26: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/26.jpg)
Controllability
The system is controllable iff
rank[B AB A2B · · · Anx−1B
]︸ ︷︷ ︸C(A,B)
= nx.
This is the most popular criterion, but there exist a lot more. We also often say that the pair (A,B) is controllable.
21 / 174
![Page 27: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/27.jpg)
Why using rank is a bad idea
Take for example the pair (A,B) with
A =
[1 εε ε
], B =
[01
]Then, the controllability matrix is[
1 ε0 ε
]whose rank is 2 but its singular values are 1 and ε.
In MATLAB use rank(·,tol) or svd instead of rank(·).
22 / 174
![Page 28: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/28.jpg)
Stability
-3 -2 -1 0 1 2 3
-2
-1
0
1
2
3
Bǫ
Bδ
23 / 174
![Page 29: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/29.jpg)
Asymptotic Stability
-3 -2 -1 0 1 2 3
-2
-1
0
1
2
3
24 / 174
![Page 30: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/30.jpg)
Stability analysis of linear systems
Consider the linear discrete-time system
xk+1 = Axk.
The origin (xe = 0) is an asymptotically stable equilibrium point for theabove system if and only if all eigenvalues of A are strictly within the unitcircle, that is
|λi| < 1.
In MATLAB one may find the eigenvalues of a matrix using eig. To find the largest eigenvalue use eigs(A,1).
25 / 174
![Page 31: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/31.jpg)
Stabilisation
Suppose the pair (A,B) is controllable and consider the system
xk+1 = Axk +Buk.
Problem: Find a controller uk = κ(xk) so that for the controlled system
xk+1 = Axk +Bκ(xk)
the origin is an asymptotically stable equilibrium point.
Hint: Choose κ(x) = Kx and find K so that A+BK has all itseigenvalues inside the unit circle.
Such a K always exists because the system is controllable and we may choose K so as to place the eigenvalues of
A + BK at any desired points in the unit circle.
26 / 174
![Page 32: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/32.jpg)
Stabilisation
We may choose K so that A+BK has eigenvalues s1, . . . , snx usingAckerman’s formula. In MATLAB we may use the function place.
But, where should we place the poles?
What about performance?
The linear quadratic regulator (LQR) gives an answer to these questions...
Read carefully the documentation of place...
27 / 174
![Page 33: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/33.jpg)
IV. Linear Quadratic Regulator
![Page 34: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/34.jpg)
Motivation
We need to stabilise the system
xk+1 = Axk +Buk,
by a linear controlleruk = Kxk
so that the closed-loop system’s response minimises a certain cost index.We assume that xk is exactly known at time k.
28 / 174
![Page 35: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/35.jpg)
Before we proceed...
Consider the optimisation problem
minimisex∈IRn
f(x)
s.t. gj(x) = 0,
Suppose the problem is convex and feasible. Let x? be an optimiser anddefine the Lagrangian function
L (x, λ) = f(x) +∑
j λjgj(x)
Then there is a vector λ? so that
∇L (x?, λ?) = 0
We call the problem convex if f is convex and the set {x : g(x) = 0} is convex.
29 / 174
![Page 36: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/36.jpg)
Finite horizon LQR
We need to determine a finite seq. of inputs π = (u0, u1, . . . , uN−1) whichsolves the optimisation problem
minimise J(π;x) := 12x
>NQfxN + 1
2
N−1∑k=0
x>kQxk + u>kRuk
subject to
xk+1 = Axk +Buk, for k = 0, . . . , N − 1
x0 = x
N is called the horizon of the problem.
30 / 174
![Page 37: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/37.jpg)
Finite horizon LQR
Some remarks:
minimise J(π;x) := 12x
>NQfxN + 1
2
N−1∑k=0
x>kQxk + u>kRuk
xk+1 = Axk +Buk, and x0 = x
I We take Q = Q> < 0, R = R> � 0 and
I the problem is convex quadratic with equality constraints,
I with decision variables u0, u1, . . . , uN−1 and x1, . . . , xN and
I it has a unique minimiser.
The notation Q < 0 means that Q is pos. semidefinite. R � 0 means R is pos. def.
31 / 174
![Page 38: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/38.jpg)
Finite horizon LQR: solution
The Lagrangian is
L = J +∑
λ>k+1(Axk +Buk − xk+1),
and by setting the gradient wrt uk (k = 0, . . . , N − 1) equal to 0 we obtain
∇ukL = Ruk +B>λk+1 = 0⇒ uk = −R−1B>λk+1
Similarly wrt xk (k = 1, . . . , N − 1) we obtain
∇xkL = Qxk +A>λk+1 − λk = 0
⇒ λk = Qxk +A>λk+1
Taking the gradient wrt xN we have
∇xNL = QfxN − λN = 0⇒ λN = QfxN
32 / 174
![Page 39: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/39.jpg)
Finite horizon LQR: solution
To sum up
xk+1 = Axk +Buk
λk = Qxk +A>λk+1
λN = QfxN
uk = −R−1B>λk+1
x0 = x
33 / 174
![Page 40: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/40.jpg)
Finite horizon LQR: solution
Therefore,
λN = Qf (AxN−1 +BuN−1)
= Qf (AxN−1 −BR−1B>λN )
⇒ λN = (I +QfBR−1B>)−1QfA · xN−1
and
λN−1 = [A>(I +QfBR−1B>)−1QfA+Q] · xN−1
= PN−1 · xN−1
34 / 174
![Page 41: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/41.jpg)
Finite horizon LQR: solution
We may recursively show that
λN = QfxN = PNxN
and
λk = Pkxk,
where Pk satisfies the recursion
Pk = Q+A>(I + Pk+1BR−1B>)−1Pk+1A.
35 / 174
![Page 42: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/42.jpg)
Finite horizon LQR: solution
The finite-horizon LQR control law has the form
uk = Kkxk,
where
Kk = −R−1B>(I + Pk+1BR−1B>)−1Pk+1A,
and
Pk = Q+A>(I + Pk+1BR−1B>)−1Pk+1A,
PN = Qf .
36 / 174
![Page 43: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/43.jpg)
Infinite horizon LQR
We need to determine a sequence of inputs π = (u0, u1, . . .) which solvesthe optimisation problem
minimise J(π;x) := 12
∞∑k=0
x>kQxk + u>kRuk
subject to
xk+1 = Axk +Buk, for k ∈ Nx0 = x
Assume that (A,B) is controllable so that the problem is feasible. Why?
37 / 174
![Page 44: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/44.jpg)
Infinite horizon LQR
Fact. Assume that (A,B) and (A>,√Q>) are controllable. Then, there is
a unique symmetric pos. def. matrix P satisfying
P = Q+A>(I + PBR−1B>)−1PA
= Q+A>PA−A>PB(R+B>PB)−1B>PA,
and the optimal sequence of inputs is given by
uk = Kxk,
with K = −(R+B>PB)−1B>PA, and K is an asym. stabilising gain.Additionally, the optimal cost will be J?(x0) = 1
2x>0Px0.
38 / 174
![Page 45: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/45.jpg)
Infinite horizon LQR
The gain K computed by the infinite horizon LQR deems A+BK has allits eigenvalues strictly inside the unit circle.
In MATLAB one may compute the LQR solution using lqr or dlqr, butbe wary as they return −K instead of K (they make A−BK stable).
39 / 174
![Page 46: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/46.jpg)
Choice of Q and R
I One may choose Q and R to be diagonal matrices
I The larger Q is, the more responsive and aggressive the controlledsystem becomes,
I The higher R is, the smoother the response will be.
40 / 174
![Page 47: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/47.jpg)
Choice of Q and R
-1 -0.5 0 0.5 1 1.5 2
x1
-1.5
-1
-0.5
0
0.5
1
x2
R=100
R=20
R=1
0 20 40 60 80 100
k
-1.2
-1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8
uk
R=100
R=20
R=1
41 / 174
![Page 48: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/48.jpg)
V. Reference tracking
![Page 49: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/49.jpg)
Problem Statement
So far we have been concerned with the stabilisation of the system towardsan equilibrium point (xe, ue) — or (0, 0) for the system with state ∆x andinput ∆u.
Typically in practice we need to be able to steer the system output
yk = Cxk +Duk,
to a given reference r, that is yk → r as k →∞, for any externallyprovided r.
42 / 174
![Page 50: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/50.jpg)
Reference tracking v1
Choose uk = Kxk + Frk and choose K to be a stabilising gain for (A,B)
We assume that (A,B) is controllable.
43 / 174
![Page 51: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/51.jpg)
Reference tracking v1
We now have a system with input rk, state xk and output yk described by∗
xk+1 = (A+BK)xk +BFrk
yk = (C +DK)xk +DFrk.
Let (xe, re) be an equilibrium point of the above system. Then
(A+BK − I)xe +BFre = 0
(C +DK)xe +DFre = ye.
and observe what A+BK − I is invertible (why?), so
xe = −(A+BK − I)−1BFre = EFre,
therefore((C +DK)E +D)Fre = ye.
44 / 174
![Page 52: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/52.jpg)
Reference tracking v1
We require that the reference-to-output static gain is equal to I, that is
((C +DK)E +D)F = I,
Notice that (C +DK)E +D is not always a square matrix.
In general, we are not always able to find such an F !
One possible solution (expensive):
F = ((C +DK)E +D)+,
Another possible solution:
F ∈ argmin ‖((C +DK)E +D)F − I‖2
X+ is the Moore-Penrose pseudoinverse.
45 / 174
![Page 53: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/53.jpg)
Example
A = [1 1; 0 0.5]; B = [0;1]; C = [1 0.1]; D = 0.1;
nx = length(A); nu = size(B,2); ny = size(C,1);
if rank(ctrb(A,B),1e-6)∼=nx,error('System not controllable ');
end
Q = eye(nx); R = 2*eye(nu);
K = -dlqr(A,B,Q,R,0);
assert( all(abs(eig(A+B*K)) <= 1-1e-7),...
'K is not stabilising ');
E = -(A+B*K-eye(nx))\B;
X = (C+D*K)*E + D;
assert(rank(X, 1e-6) == ny , 'X not full rank');F = X\eye(ny); % OR: F = pinv(X);assert( norm((X*F - eye(ny), Inf) < 1e-12 );
46 / 174
![Page 54: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/54.jpg)
Example
0 50 100 150 200
k
0.75
0.8
0.85
0.9
0.95
1
1.05
1.1
1.15
1.2
1.25
y,r
R=1
R=20
R=100
r
47 / 174
![Page 55: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/55.jpg)
Reference tracking v2
Let us consider a slightly different parametrisation for uk
uk = ue +K(xk − xe),
where xe and ue are computed so that[A− I BC D
]︸ ︷︷ ︸
W
[xe
ue
]=
[0r
]
where W is not necessarily square. Now, find G so that
WG =
[0nx×ny
Iny
]Then [
xe
ue
]= Gr.
This means that (xe, ue) is an equilibrium point of the system dynamics: xe = Axe+Bue and the corresponding
output is r = Cxe + Due.
48 / 174
![Page 56: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/56.jpg)
Reference tracking v2
49 / 174
![Page 57: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/57.jpg)
Example
A = [1 1; 0 0.5];
B = [0;1];
C = [1 0.1];
D = 0.1;
nx = length(A); % nx = 2nu = size(B,2); % nu = 1ny = size(C,1); % ny = 1
Q = eye(nx); R = 20*eye(nu);
K = -dlqr(A,B,Q,R,0);
W = [A-eye(nx), B;
C, D];
G = W\[zeros(nx, ny); eye(ny)];
50 / 174
![Page 58: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/58.jpg)
Example
x = [1;0];
X=x;
Y=[];
for k=1:100 ,
if (k<15),
r = 1.2;
elseif (k>=15 && k<40),
r = 0.8;
else
r = -0.3;
end
xue = G*r;
u = xue(nx+1:nx+nu) + K*(x-xue(1:nx));
x = A*x + B*u;
X = [X;x];
Y = [Y; C*x + D*u];
end
plot(Y','linewidth ' ,2);
51 / 174
![Page 59: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/59.jpg)
Example
0 20 40 60 80 100
k
-0.4
-0.2
0
0.2
0.4
0.6
0.8
1
1.2
1.4
yk
reference
R=20
R=10
52 / 174
![Page 60: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/60.jpg)
Reference tracking with disturbances
Problems. Is the controlled system really offset free? How can weguarantee that ek = yk − rk indeed converges to 0?
What if a constant disturbance dk acts on the system as follows?
xk+1 = Axk +Buk + dk
53 / 174
![Page 61: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/61.jpg)
Reference tracking with disturbances
0 50 100 150 200
0.75
0.8
0.85
0.9
0.95
1
1.05
1.1
1.15
1.2
1.25
ref
Ideal
With disturbance
Here a disturbance as small as d = 0.01 made the system equilibrate away from the set-point.
54 / 174
![Page 62: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/62.jpg)
Integral action
Solution. integral action! We introduce the error integral dynamics:
zk+1 = zk + rk − yk
and use the control law
uk = ue +Kx(xk − xe) +Kzzk.
In MATLAB one may compute the gains Kx and Kz using lqi.
The MATLAB function lqi returns −Kx and −Kz . Read the documentation. The MATLAB function ss will also
be needed.
55 / 174
![Page 63: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/63.jpg)
Integral action stability
As an exercise, show that if[Kx Kz
]is a stabilising gain for the pair([
A 0C I
],
[B0
]),
and the reference signal is constant, rk = r, then yk → r for any constantdisturbance dk = d.
The LQI returns Kx and Kz which satisfy this condition and, additionally, minimise an infinite-horizon cost index.
56 / 174
![Page 64: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/64.jpg)
Integral action
Integral action
57 / 174
![Page 65: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/65.jpg)
Integral action in action
40 60 80 100 120 140 160
0.95
1
1.05
1.1
58 / 174
![Page 66: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/66.jpg)
VI. Linear State Observers
![Page 67: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/67.jpg)
Problem Statement
So far we have assumed that xk is known at time k (measured) and free ofnoise. This is usually unrealistic. Instead, we obtain a measurement yk
yk = Cxk +Duk.
We then need a way to produce estimates xk of the current state usinginformation that can be observed: current and past inputs and outputs.
59 / 174
![Page 68: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/68.jpg)
Observability
Consider the system
xk+1 = Axk +Buk
yk = Cxk +Duk.
We say that it is observable if there is n ∈ N so that for any x0 and anyfinite sequence of inputs {uk}nk=0, x0 can be uniquely determined using{uk}n−1k=0 and {yk}nk=0.
60 / 174
![Page 69: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/69.jpg)
Observability condition
The pair (C,A) is observable if and only if (A>, C>) is controllable, that is
rank
CCACA2
...CAnx−1
= nx
In MATLAB one may use the commands rank and obsv. Use the SVD decomposition of the observability matrix to
tell whether it is near-unobservable (In MATLAB use svd).
61 / 174
![Page 70: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/70.jpg)
State observers
A linear state observer is a dynamical system
xk+1 = Axk + L(yk − yk) +Buk
yk = Cxk +Duk.
Define the state estimation error ek = xk − xk. Then
ek+1 = (A+ LC)ek.
The error dynamics is asymptotically stable (ek → 0 as k →∞) providedthat all eigenvalues of A+ LC are inside the unit circle.
Here we assume that (C,A) is observable.
62 / 174
![Page 71: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/71.jpg)
State observer design
Straightforward solution: by pole placement.
But where should we place the poles?
Rule of thumb: the poles of A+ LC should be about 10 times smallerthan the poles of the controlled system.
One may use MATLAB’s place to construct L.
63 / 174
![Page 72: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/72.jpg)
Separation principle
We may always design the controller and the observer separately and thencombine them. For example, using integral action:
zk+1 = zk + rk − yk,uk = ue +Kx(xk − xe) +Kzzk,
xk+1 = Axk + L(Cxk +Duk︸ ︷︷ ︸yk
−yk) +Buk,
where the first two equations correspond to the controller which computesuk based on the state estimate xk as the state xk is not directly available.The last equation is used to update the estimate xk+1.
64 / 174
![Page 73: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/73.jpg)
Separation principle
ObserverController
Observer
Controller
65 / 174
![Page 74: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/74.jpg)
Estimation under uncertainty
Consider the system
xk+1 = Axk +Buk +Gwk
yk = Cxk +Duk +Hwk + vk,
where wk and vk are zero-mean random variables with
E[wkw>k] = Σw
E[vkv>k] = Σv,
E[wkv>k] = 0,
with E[wkw>j ] = 0 and E[vkv
>j ] = 0 for k 6= j.
66 / 174
![Page 75: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/75.jpg)
Minimum energy estimation
We need to produce an estimate xk out of observations {yj , uj}j≤k so asto minimise
Jmee =
k∑t=−∞
v>tQvt + w>tRwt,
where
I large Q: we trust our measurements
I large R: we trust our system model
I Choose: Q = Σ−1v , R = Σ−1w
This is equivalent to minimising the steady-state covariance of the stateestimation error
P = limk
E[(xk − xk)(xk − xk)>].
67 / 174
![Page 76: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/76.jpg)
Steady-state Kalman Filter
The best estimator is then known to be
xk+1 = Axk + L(Cxk +Duk − yk) +Buk,
where L is given by
L = −(APC> +GQH>)(CPC> +R+HQH>)−1,
where P solves an algebraic Ricatti equation. In MATLAB, L and P arecomputed by kalman (see also lqg).
68 / 174
![Page 77: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/77.jpg)
Example
System dynamics:
xk+1 =
0.5 −0.9 −0.10.9 0.5 0.10.1 0 1
xk +
100
uk + wk
yk =
[1 0 10 1 −0.5
]xk + vk
The system is controllable and observable. We choose the LQRparameters Qlqr = 5I,Rlqr = 4, and the Kalman filter parametersQkal = 10 · I, Rkal = 0.01 · I.
69 / 174
![Page 78: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/78.jpg)
Example — estimation error
50 100 150 200
0
0.1
x1−x1
50 100 150 200
0
0.05
0.1
x2−x2
50 100 150 200
-0.02
0
0.02
x3−x3
70 / 174
![Page 79: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/79.jpg)
Example — response
50 100 150 200
k
-1
-0.5
0
0.5
1
x
71 / 174
![Page 80: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/80.jpg)
Example — poles
-1 -0.5 0 0.5 1
-0.8
-0.6
-0.4
-0.2
0
0.2
0.4
0.6
0.8Controller
Observer
72 / 174
![Page 81: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/81.jpg)
Measurement bias
What will happen if our measurements have a constant bias?
xk+1 = Axk +Buk + wk
yk = Cxk +Duk + vk + dyk,
where vk ∼ N (0,Σv), wk ∼ N (0,Σw) and dyk is constant.
We will see that by applying a standard KF, xk − xk does not converge toa zero-mean distribution.
73 / 174
![Page 82: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/82.jpg)
Measurement bias — Example
% System dataA = [1 1.2; 0.1 0.5];
B = [0;1];
C = [1 0.1];
D = 0.1;
nx = size(A,1); nu = size(B,2); ny = size(C,1);
% LQR designQ = eye (2);
R = 80;
K = -dlqr(A, B, Q, R, 0);
dy = 1.0; % measurement bias
74 / 174
![Page 83: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/83.jpg)
Measurement bias — Example
% Kalman filter designQo = diag ([20, 20]); Ro = 100;
ss_kalman = ss(A, [B eye(2)], C, [D 0 0], -1);
[∼, L, P] = kalman(ss_kalman , Qo , Ro , 0);
x = [ -1;10]; % initial values (x_0, z_0)x_ = [ -1;10]; % initial state estimate
T = 200; % simulation timeX = zeros(2,T); X(:,1) = x; % cache of statesX_ = zeros(2,T); X_(:,1) = x_; % cache of estimatesfor k=1:T,
x = A * x + B * K * x_ + randn *0.02;
y = C * x + D * K * x_ + randn *0.05 + dy ;
x_ = (A - L*C)*x_ + (B - L*D) * K * x_ + L * y;
X_(:,k+1) = x_; X(:,k+1) = x;
end
75 / 174
![Page 84: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/84.jpg)
Measurement bias — State estimation, dy=0
20 40 60 80 100
-6
-4
-2
0
2
x1
Actual
Estimated
20 40 60 80 100
k
-3
-2
-1
0
1
x2
Actual
Estimated
76 / 174
![Page 85: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/85.jpg)
Measurement bias — State estimation, dy=1
20 40 60 80 100
-4
-2
0
x1
Actual
Estimated
20 40 60 80 100
k
-3
-2
-1
0
x2
Actual
Estimated
77 / 174
![Page 86: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/86.jpg)
Measurement bias — State estimation, dy=5
20 40 60 80 100
-15
-10
-5
0
x1
Actual
Estimated
20 40 60 80 100
k
-3
-2
-1
0
1
x2
Actual
Estimated
78 / 174
![Page 87: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/87.jpg)
State estimation & Measurement bias
Some remarks:
I If dy = 0 everything works like clockwork
I A KF fails to reconstruct the system of the state
I A biased output tracks the desired disturbance
79 / 174
![Page 88: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/88.jpg)
Measurement bias — a remedy
One possible remedy is to perfectly calibrate our sensors. But often this isnot enough, let alone, they often wear out with time and use.
The system dynamics (omitting all noise terms) is written as[xk+1
dk+1
]=
[A 00 I
] [xkdk
]+
[B0
]uk
yk =[C I
] [xkdk
]+Duk
Exercise 1: Show that the above system is not controllable. Exercise 2: Construct a matrix A ∈ IR2×2 and a
C ∈ IR1×2 so that (C,A) is observable, but the above system is unobservable.
80 / 174
![Page 89: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/89.jpg)
Measurement bias — a remedy
By defining xk = (xk, dk)> we may rewrite the system as
xk+1 = Axk + Buk
yk = Cxk +Duk,
and if (C, A) is observable (see slides 60 and 61), we may design a stateobserver to estimate xk, so the observer will produce two estimates xk anddk and as k →∞, dk − dk → 0.
81 / 174
![Page 90: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/90.jpg)
Measurement bias — a remedy
% Data of the augmented systemA_ = blkdiag(A, 1);
C_ = [C 1];
B_ = [B;0];
% Design augmented state observersys_ = ss(A_ , [B_ eye(3)], C_ , [D zeros (1,3)], -1);
[∼, L_] = kalman(sys_ Qo , Ro , 0);
% State estimationx_ = (A_ - L*C_)*x_ + (B_ - L*D_) * u + L * y;
xest = x_ (1:2);
dest = x_(3);
82 / 174
![Page 91: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/91.jpg)
Measurement bias — a remedy
20 40 60 80 100 120 140 160 180 200
-5
0
5
x1
estiamate
actual
20 40 60 80 100 120 140 160 180 200
k
-2
-1
0
x2
estiamate
actual
20 40 60 80 100 120 140 160 180 200
k
-1
0
1
2
3
d
bias estiamate
Here we used dy = 3. Indeed the observer produces an estimate dk which converges to dy (plus-minus zero-mean
noise).
83 / 174
![Page 92: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/92.jpg)
Measurement bias — conclusions
Some notes:
I The variable yk − dk is an estimate of the system’s unbiased output
I The controller’s goal should be to drive yk − dk to r, soasymptotically Cxk +Duk → r.
84 / 174
![Page 93: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/93.jpg)
Extensions
There exist more elaborate state estimators such as
I Linear Quadratic Gaussian (LQG): LQR and KF
I Non-steady-state Kalman filter
I The Extended Kalman Filter
I The Unscented Kalman Filter
I Particle filters
and many another.
85 / 174
![Page 94: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/94.jpg)
Designer’s arsenal
We have the following tools in our toolbox:
I Rejection of measurement bias by augmenting the state space systemwith the disturbance
I Integral action to attenuate the effect of constant disturbances actingon the system state
I Use none, both or either depending on the problem, but choose thesimplest formulation that leads to good closed-loop behaviour. Inother words, verify your assumptions with simple experiments.
86 / 174
![Page 95: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/95.jpg)
VII. Quaternions and Rotations
![Page 96: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/96.jpg)
Reference frame
The orientation of a solid body in spacecan only be defined with respect to areference frame.
Here we use a body-fixed frame, thatis, roughly speaking, the orientation isdescribed with what the body itselfdefines as upright and forward.
87 / 174
![Page 97: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/97.jpg)
Euler angles
Euler angles: (α, β, γ)Reference frame: (x, y, z)Rotated frame: (X,Y, Z)
Important: the order of the three elementary rotations matters! The standard convention is: yaw, pitch, roll.
88 / 174
![Page 98: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/98.jpg)
Yaw (heading)
-0.5
0
-0.5
0.5
z
1
0
1.5
x
0.5 1.51
y
1 0.501.5 -0.5
89 / 174
![Page 99: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/99.jpg)
Pitch (elevation)
-0.5
0
-0.5
0.5
z
1
0
1.5
x
0.5 1.51
y
1 0.501.5 -0.5
90 / 174
![Page 100: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/100.jpg)
Roll (bank)
-0.5
0
-0.5
0.5
z
1
0
1.5
x
0.5 1.51
y
1 0.501.5 -0.5
91 / 174
![Page 101: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/101.jpg)
Positive rotation convention
Use the right hand rule. Mnemonic trick: In alphabetical order: x→ y → z → x→ . . .. Positive yaw: face left;
Positive pitch: tilt upward as when an aeroplane taking off; Positive roll: roll to the right side.
92 / 174
![Page 102: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/102.jpg)
Quaternions
A quaternion is a representation of a rotation; it is a 4-dimensional vector
q = (q0, q1, q2, q3).
Often, it is represented as a hyper-complex number
q = q0 + iq1 + jq2 + kq3,
where i2 = j2 = k2 = ijk = −1. We call q0 its scalar part and (q1, q2, q3)its vector part.
The space of quaternions is denoted by H.
93 / 174
![Page 103: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/103.jpg)
Quaternion representations of rotations
A rotation/orientation can be described by a quaternion by determining (i)a direction of rotation u ∈ R3 and (ii) the angle of rotation about u. Thecorresponding quaternion is
q = cos α2 + (iux + juy + kuz) sin α2 .
94 / 174
![Page 104: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/104.jpg)
Quaternions
95 / 174
![Page 105: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/105.jpg)
Quaternions
95 / 174
![Page 106: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/106.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 107: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/107.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 108: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/108.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 109: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/109.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 110: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/110.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 111: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/111.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 112: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/112.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 113: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/113.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 114: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/114.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 115: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/115.jpg)
Rotations using quaternions
1.51
-0.5
-0.5
0
0.5
y
0.5
z
0
x
1
00.5
1.5
1 -0.51.5
96 / 174
![Page 116: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/116.jpg)
Quaternions: some simple calculus
Let p = (p0, p1, p2, p3), q = (q0, q1, q2, q3) and λ ∈ IR. Then:
I Addition: p+ q = (p0 + q0, p1 + q1, p2 + q2, p3 + q3)
I Multiplication:
X ijk = −1X ij = k, jk = i, ki = jX ji = −k, kj = −i, ik = −jX p · q = (p0 + ip1 + jp2 + kp3) · (q0 + iq1 + jq2 + kq3) = . . .X But, p · q is not the same as q · pX Let 1H = (1, 0, 0, 0); then p · 1H = 1H · p = p.
I Division: p/q = p · q−1 (later)
97 / 174
![Page 117: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/117.jpg)
Quaternions — some definitions
The norm of a quaternion q ∈ H is
‖q‖ =√q20 + q21 + q22 + q23
Quaternions with ‖q‖ = 1 are called unit quaternions (H1), are used torepresent rotations in IR3 and have the representation
q = cos α2 + sin α2 (iux + juy + kuz)
The complex conjugate of q is
q∗ = (q0,−q1,−q2,−q3).
Hereafter, we will only be dealing with unit quaternions. In MATLAB the conjugate of a quaternion is computed by
quatconj. The 2-norm of a quaternion is computed by quatnorm.
98 / 174
![Page 118: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/118.jpg)
Quaternions — combined rotation
We define the (Hamilton) product of two quaternions p, q ∈ H
p⊗ q ≡ p · q = Q(p) · q =
p0 −p1 −p2 −p3p1 p0 −p3 p2p2 p3 p0 −p1p3 −p2 p1 p0
q0q1q2q3
The operation ⊗ is not commutative. The product q ⊗ p describes thecombined rotation of p followed by q (in the absolute frame of reference).
Useful properties: (p ⊗ q)∗ = q∗ ⊗ p∗ and (p∗)∗ = p. Therefore, p ⊗ q = (q∗ ⊗ p∗)∗. Source: Fresk &
Nikolakopoulos, 2013. In MATLAB use quatmultiply.
99 / 174
![Page 119: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/119.jpg)
Quaternions — combined rotation
1.5-0.5
0
-0.5 1
0.5z
1
0
1.5
y
0.5
u
x
0.501
-0.51.5
100 / 174
![Page 120: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/120.jpg)
Quaternions — combined rotation
v
1.5-0.5
0
-0.5 1
0.5z
1
0
1.5
y
0.5
x
0.501
-0.51.5
100 / 174
![Page 121: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/121.jpg)
Quaternions — combined rotation
1.5-0.5
comb0
-0.5 1
0.5z
1
0
1.5
y
0.5
x
0.501
-0.51.5
100 / 174
![Page 122: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/122.jpg)
Hamilton product
We defined the Hamilton product p⊗ q = Q(p) · q. We may represent thisproduct in a dual way as
p⊗ q = Q(q)p =
q0 −q1 −q2 −q3q1 q0 q3 −q2q2 −q3 q0 q1q3 q2 −q1 q0
p0p1p2p3
Interesting observations:
I Q(q∗) = Q(q)>.
I For p, q ∈ H1, p⊗ q ∈ H1.
101 / 174
![Page 123: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/123.jpg)
Quaternions — inversion
Let p be a unit quaternion. Then
p⊗ p∗ = p∗ ⊗ p = 1H,
where 1H = (1, 0, 0, 0) is a no-rotation quaternion. In this sense, p∗ is theinverse of p. In general
p−1 =p∗
‖p‖.
The inverse of a quaternion p−1 undoes the rotation p. It is
p⊗ p−1 = p−1 ⊗ p = 1H.
102 / 174
![Page 124: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/124.jpg)
Rotating objects using quaternions
Let an object be defined by some points Pi ∈ IR3 and let q ∈ H1 be a unitquaternion describing a rotation. Then, the application of q on Pi rotatesit to a new point Pi. This rotation is described by[
0
Pi
]= p⊗
[0Pi
]⊗ p−1
= p⊗[
0Pi
]⊗ p∗
= Q(p)Q(p∗)
[0Pi
]= Q(p)Q(p)>
[0Pi
]
103 / 174
![Page 125: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/125.jpg)
Rotations in MATLAB
% Define a boxA = [0 0 0]; B = [1 0 0]; C = [0 1 0]; D = [0 0 1];
E = [0 1 1]; F = [1 0 1]; G = [1 1 0]; H = [1 1 1];
P = [A;B;F;H;G;C;A;D;E;H;F;D;E;C;G;B];
% Define a quaternionalpha = 0.1;
p = [cos(alpha /2), sin(alpha /2)*[2; 0.2; -0.6]'];p = quatnormalize(p);
% Rotate the box PN = size(P,1);
Q = quatmultiply(quatmultiply(p,[zeros(N,1) P]), ...
quatconj(p));
Q = Q(: ,2:4);
% Plotplot3(Q(:,1),Q(:,2),Q(:,3),'LineWidth ' ,2);
In MATLAB quaternions are represented as row vectors. 104 / 174
![Page 126: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/126.jpg)
Rotations in MATLAB
Another alternative is to use the direction cosines matrix, a matrixD ∈ IR3×3 so that for P ∈ IR3, P = DP is the rotated point
% Using DCMQ = P*quat2dcm(p);
Moreover, MATLAB provides the function quatrotate where, however,one needs to provide p∗ and not p
% Using quatrotateQ = quatrotate(quatconj(p), P);
105 / 174
![Page 127: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/127.jpg)
Difference of quaternions — the wrong way
Fact. All quaternions αq, for α 6= 0, represent the same exact rotation!
Let us defined(p, q) = p− q.
Thend(p, αp) = (1− α)p,
but p and αp are equivalent rotations.
If d(p, q) is a no-rotation quaternion then p and q are identical.
106 / 174
![Page 128: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/128.jpg)
Difference of quaternions — the wrong way
Assume p and q are unit quaternions and take p = −q. Then
d(p, q) = 2p.
Although p and q correspond to the same rotation, there is no way to inferthat by d(p, q).
107 / 174
![Page 129: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/129.jpg)
Difference of quaternions — the right way
A useful difference mapping is a δ : H×H→ H given by
δ(p, q) = p⊗ q∗.
Take a unit quaternion p ∈ H:
δ(p, p) = p⊗ p∗ = (1, 0, 0, 0).
108 / 174
![Page 130: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/130.jpg)
Euler angles to quaternions
The rotation described by the Euler angles (φ, θ, ψ) is given by thequaternion
q =
cos φ2 cos θ2 cos ψ2 + sin φ
2 sin θ2 sin ψ
2
sin φ2 cos θ2 cos ψ2 − cos φ2 sin θ
2 sin ψ2
cos φ2 sin θ2 cos ψ2 + sin φ
2 cos θ2 sin ψ2
cos φ2 cos θ2 sin ψ2 − sin φ
2 sin θ2 cos ψ2
In MATLAB, use the function angle2quat. Sequence of rotations: yaw, pitch, roll. Source: Wikipedia.
109 / 174
![Page 131: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/131.jpg)
Euler angles to quaternions
A quaterion q = (q0, q1, q2, q3) corresponds to the following Euler angles
φθψ
=
arctan 2(q0q1+q2q3)1−2(q21+q22)
arcsin(2(q0q2 − q3q1))arctan 2(q0q3+q1q2)
1−2(q22+q23)
In MATLAB, do not use atan — use atan2 instead.
Remember: do not use atan!
110 / 174
![Page 132: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/132.jpg)
Why quaternions?
I With Euler angles we always need to specify the sequence of rotations
I Gimbal lock — a singularity
I Rotations with simple algebraic operations
I Quaternions: numerically robust rotations.
111 / 174
![Page 133: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/133.jpg)
VIII. Attitude Dynamics & Control
![Page 134: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/134.jpg)
Reference frames
Source: Xiong and Zheng, 2013.
112 / 174
![Page 135: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/135.jpg)
Quadcopter dynamics
MotorDynamics
Quaternion-basedFlight Dynamics
113 / 174
![Page 136: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/136.jpg)
Motor & propeller dynamics
114 / 174
![Page 137: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/137.jpg)
Motor & propeller dynamics
ESC + Motor
114 / 174
![Page 138: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/138.jpg)
Motor & propeller dynamics
Motor dynamics
114 / 174
![Page 139: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/139.jpg)
Motor & propeller dynamics
Reaction wheel
114 / 174
![Page 140: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/140.jpg)
Motor & propeller dynamics
Thrust
114 / 174
![Page 141: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/141.jpg)
Parameters of motor & propeller dynamics
Parameters k1 and k2 are given by
kx,y,z1 =Vmax − Vmin
60Kv
kx,y,z2 = 1τm
where
I Vmax is the max. voltage (around 11.1V ),
I Vmin = 10%Vmax,
I Kv is the motor speed constant (in rpm/V ),
I τm is the time constant of the motors (τm = 35ms)
115 / 174
![Page 142: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/142.jpg)
Parameters of motor & propeller dynamics
Parameters kx3 and ky3 are given by (for j = x, y)
kj3 = ddn CTρn
2D4∣∣nh
NmL√2Ijj
where
I CT is the thrust coeff. the propellers,
I ρ is the density of the air,
I Nm is the number of motor,
I D is the diameter of each propeller,
I L is the arm length of the quadcopter,
I Iprop is the moment of inertia of each propeller,
I Ixx, Iyy are the m.o.i about the x and y axes and
I nh is the hovering frequency of rotation (rps)
I CTρn2D4 is the thrust applied by each propeller.
116 / 174
![Page 143: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/143.jpg)
Parameters of motor & propeller dynamics
Parameter kz3 is given by
kz3 = ddn
CPρn2D5
2π
∣∣∣∣nh
Nm
Izz
where
I CP is the power coefficient of the propellers,
I Izz is the m.o.i about the z axis and
I nh is the hovering frequency of rotation (rps)
117 / 174
![Page 144: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/144.jpg)
Parameters of motor & propeller dynamics
Parameters kx,y4 and kz4 are given by
kx,y4 = 0
kz4 = 2πNmIprop + Im
Izz,
where
I Iprop is the moment of inertia of each propeller
I Im is each motor’s moment of inertia
118 / 174
![Page 145: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/145.jpg)
Flight dynamics
The attitude dynamics is described by
q = 12 · q ⊗
[0ω
],
ω = I−1cm(τ − ω × (Icmω)),
where Icm is the inertia matrix
Icm =
Ixx IyyIzz
Recall that we are using an X-shaped body-fixed frame (see slide 87).
119 / 174
![Page 146: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/146.jpg)
Signals to motors
The signals v1, . . . , v4 to the four motors are computed in terms of thetorque signals ux, uy, uz and the a common signal c as follows
v1 = c+ ux + uy − uzv2 = c+ ux − uy + uz
v3 = c− ux + uy + uz
v4 = c− ux − uy − uz
where v1, . . . , v4 are relative voltage signals and will create the torques τx,τy and τz.
120 / 174
![Page 147: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/147.jpg)
Parameters
Parameter Symbol Value Units
Number of motors Nm 4 −Total mass of the quadcopter m 1.85 kgArm length L 27 cmAir density (sea level, 15◦) ρ 1.225 kg/m3
Gravitational acceleration g 9.81 m/s2
Table : General properties of the EAGLE quadcopter and various constants.
121 / 174
![Page 148: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/148.jpg)
Parameters
Parameter Symbol Value Units
Thrust coefficient∗ CT 0.1 −Power coefficient∗ CP 0.04 −Propeller mass mp 20 gPropeller diameter D 12 in
Table : Parameters of the propellers (12x4.5 Thin Electric).
122 / 174
![Page 149: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/149.jpg)
Parameters
Parameter Symbol Value Units
Motor speed constant Kv 700 rpm/VMotor time constant τm 35 msRotor mass mr 42 gRotor radius Rr 1.9 cmTotal motor mass mm 102 g
Table : Motor parameters (3508-700KV Turnigy Multistar 14 Pole BrushlessMulti-Rotor Motor With Extra Long Leads).
123 / 174
![Page 150: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/150.jpg)
Moments of inertia
The moment of inertial about the verticalaxis that runs through the CoM of thesuspended body is
I =mgd2T 2
n
16π2L,
where Tn is its natural period of oscillation.
Image source: http://www.schoolphysics.co.uk.
124 / 174
![Page 151: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/151.jpg)
Closed-loop system
MotorDynamics
Quaternion-basedFlight Dynamics
ControllerError
computation
125 / 174
![Page 152: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/152.jpg)
Details
I Consider only the dynamics of (q1, q2, q3), exclusing q0I Use the fact that rotation quaternions are unit quaternions
I Write down the (q1, q2, q3), ω-dynamics and linearise the system
I Verify that it is controllable
I Use the scalar part of qe to “regularise” its vector part
I You will find bibliographic references at the end.
Details: Y. Yang, 2012. Regarding the regularisation, see also: Fresk and Nikolakopoulos, 2013.
126 / 174
![Page 153: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/153.jpg)
Details
Given that rotation quaternions q ∈ H are unitary
‖q‖ = 1,
we have that
q0 =√
1− q21 − q22 − q23.
This means that the vector part of q suffices to determine q0 (using theconvention that q0 > 0). We may use this fact to eliminate q0 from themodel.
Details: Y. Yang, 2012. Regarding the regularisation, see also: Fresk and Nikolakopoulos, 2013.
127 / 174
![Page 154: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/154.jpg)
Details
A rotation of −5◦ (about any axis) has the same effect as a rotation of355◦. In order to avoid going around the long way we need to canonicaliseqr as follows
qr =
{qr, if qr0 > 0,
−qr, otherwise
Unless we need to perform a full rotation about any axis (which, unless wewant to perform acrobatic manoeuvres, is the case only for the z axis).
128 / 174
![Page 155: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/155.jpg)
Details
To compute the (hovering) equilibrium point (qe, ωe, ne, ue) we need totake into account the following
I The quadrotor should stay upright, that is qe = (1, 0, 0, 0)
I It should not rotate or wobble, so ωe = 0
I It should lift its own weight, so the total thrust should be equal to itsweight, that is
NmCTρn2hD
4 = mg
I We then use nh to compute k3 at equilibrium,
I We linearise about (nx, ny, nz) = (0, 0, 0).
129 / 174
![Page 156: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/156.jpg)
RC signals
Note. The references from the RC aresent out as Euler angles. First, we need tocompute the corresponding quaternion qr.
130 / 174
![Page 157: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/157.jpg)
IX. Implementation of AttitudeControl System
![Page 158: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/158.jpg)
The EAGLE MATLAB simulator
Attitude dynamics IMU
Quadcopter
LQR KF
Attitude Controller
This is the schematic overview of what we need to build.
131 / 174
![Page 159: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/159.jpg)
The EAGLE MATLAB simulator
Main functions:
I quat params: model parameters (adjust them!)
I quat dynamics: continuous-time attitude dynamics
I quat linear dyn: linearised continuous-time dynamics
I eagle simulator: attitude controller/observer designer (lots ofoptions; read the documentation).
Helper functions:
I deg2quat: degrees to quaternion
I print mv: generates C code for mat-vec operations. This functionmay assist in generating C code.
There is detailed documentation. Do study it.
132 / 174
![Page 160: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/160.jpg)
Useful MATLAB functions (i)
I For C code generation we need:
X fopen, fclose: open and close filesX fprintf: write to file
I Make nice figures using:
X plot, plot3: make figure; use the option linewidth to make linesadequately visible
X hold on: make plots with multiple dataX xlabel, ylabel: always add axis labelsX grid on: show grid
I Control
X dlqr, kalman: LQR and Kalman designX eig: eigenvalues of a matrixX svd: singular value decompositionX cov: estimate covariance matrixX mvnrnd: random vectors from N (µ,Σ)
133 / 174
![Page 161: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/161.jpg)
Useful MATLAB functions (ii)
I Quaternions:
X quatnorm: returns ‖p‖X quatnormalize(p): returns p/‖p‖X deg2quat: angle (in degrees) to unit quaternionX quatmultiply(p,q): performs p⊗ qX quatdivide: multiply by inverse quaternionX quatconj: conjugate quaternionX quatinv: inverse quaternionX quatrotate: rotates a vector by (the conjugate of) a quaternionX atan2: four quadrant arctangent
134 / 174
![Page 162: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/162.jpg)
Step 1
I Peruse the course material
I Write down the system dynamics in state space form
I Determine the equilibrium points of the (CT) system
I Linearise the system & simulate
I Discretise & simulate again — compare
135 / 174
![Page 163: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/163.jpg)
Step 2
I Decide the system structure (inputs, states, outputs)
I Is the system controllable? Are you sure?
I In MATLAB use ctrb and rank.
The SVD decomposition is also useful for finding the rank of a matrix and determining whether (A,B) is near-
uncontrollable. In MATLAB use svd or rank(·,tol).
136 / 174
![Page 164: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/164.jpg)
Step 3
I Design a controller that steers the system state to a fixed equilibriumpoint (as in Section IV; no reference tracking)
I Choose some initial values for Q and R and find an LQR gain K
I Compute the eigenvalues of A+BK. Is it stable?
I Fine tune Q and R until you get satisfactory closed-loop behaviour
137 / 174
![Page 165: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/165.jpg)
Step 4
I Implement an LQR controller with integral action
I Tweak the LQR weights Q and R — choose Q = blkdiag(Qx, Qz)
I Apply a constant disturbance — is it attenuated adequately?
I Find the poles of the overall system (with state (xk, zk))
138 / 174
![Page 166: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/166.jpg)
Step 5
I Design a controller using LQR for reference tracking
I Design a steady-state Kalman filter
I Plot the poles of the observer and the controller
I Simulate the system and assess the closed-loop performance
I Assume we have inexact estimates of the model parameters andmeasurement noise
Make reasonable assumptions regarding the level of noise and the error in the determination of the system parameters.
Use experimental data. For example, how accurate are the IMU measurements? How accurate is the model?
139 / 174
![Page 167: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/167.jpg)
Step 6
I Implement a C code generator that will procude an ANSI C
implementation of your controller/observer
I Test the C code (on a computer)
I Do not hard-code the controller observer yourselves! You willcertainly need to modify/re-tune your controller/observer and test iton the quadcopter several times. Make a good code generator.
I Test your implementation on the quadcopter:
X Print the signals to the motors (with the motors disconnected)X Test without the propellers attached to the motorsX Test on the gimbal viseX First test flight
140 / 174
![Page 168: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/168.jpg)
Step 7
How to get rid of the drift...
I Calibrate the IMU (every time before taking off)I Balance the quadcopter
X Make sure the arms are straight and perpendicular to one anotherX Balance the centre of massX Straighten up all motors and level the propellers
I Re-calibrate the ESCs
I Reduce vibrations
I As a last resort, use the trimmers on the RC to correct
141 / 174
![Page 169: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/169.jpg)
Regarding the C code
I Pure ANSI C,
I /* comments are like that */
I You cannot mix declarations with initialisations, e.g., you cannotwrite float a = 0.0;. Use initialisers instead.
I Use structures (struct).
I Avoid allocating memory dynamically, e.g., using malloc.
I Document your functions with /** ... */!
I Perform unit tests.
I Use git or some other code versioning system.
142 / 174
![Page 170: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/170.jpg)
X. Navigation
![Page 171: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/171.jpg)
Navigation
Goal: Design a controller which will make the quadcopter flyautonomously, hover at specified positions, maintain a given heading,follow prescribed paths or traverse given points in space.
The variables we want to control are (x, y, z, ψ).
Approach: We first derive the navigation dynamics based on firstprinciples and then we use data to identify a dynamical model which weuse to auto-pilot the quadcopter.
143 / 174
![Page 172: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/172.jpg)
Navigation — the closed-loop system
Attitude dynamics IMU
Quadcopter
LQR KF
Camera
VideoProcessor
LQR KF
Attitude Controller
Navigator
Sonar
Attitude information
GeometricTransformations
Filter
The navigation system need to collaborate with the image processing module and the attitude control module.
144 / 174
![Page 173: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/173.jpg)
High-tilt corrections of measurements
I At low-tilt mode, we may assume that the camera and the sonar face(approximately) downwards and that the (x, y, z) measurements arereliable
I The tilt of the quadcopter may significantly alter these measurements
I We shall first see how the tilt affects the measured altitude
I And then we will use a quaternion-based correction technique.
Have a look at slide 144: the block “Geometric transformations” uses the current quaternion q to correct the
(x, y, z) measurements.
145 / 174
![Page 174: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/174.jpg)
High-tilt correction of sonar measurements
2
B A
O
-2
-1.5
-1
-2
y
-0.5
0
0z
0.5
1
-1
1.5
x
2
01 -22
Tilted about y-axis by θ = BOA.4BAO is a right-angled triangle.We measure z = |OB|, but need
z = |OA| = z
cos θ.
This example is only to gain some intuition. In practice, use the methodology on slides 148 to 151.
146 / 174
![Page 175: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/175.jpg)
High-tilt correction of sonar measurements
2
B A
O
-2
-1.5
-1C
-2
y
-0.5
0
0z
0.5
1
-1
1.5
x
2
01 -22
Tilted about x and y-axes byθ = BOA and φ = COB.4CAO is a right-angled triangleWe measure z = |OC|, but need
z = |OA| = z
cos θ cosφ.
This example is only to gain some intuition. In practice, use the methodology on slides 148 to 151.
147 / 174
![Page 176: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/176.jpg)
Quaternion-based correction
1
E
-1
cos σ = u′v
v
B
u
O
-1-0.5
-0.5
y
0
0
A
z
0.5
1
x
00.5 -11
Define a unit vector v = (0, 0,−1) facingdownwards and rotate it using q toobtain a vector u, that is[
0u
]= q ⊗
[0v
]⊗ q−1.
Note that ‖u‖ = 1. The angle betweenu and v is cosσ = u′v.
Exercise: show that ux = −2(q0q2 + q1q3), uy = 2(q0q1 − q2q3) and qz = −q20 + q21 + q22 − q23 .
148 / 174
![Page 177: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/177.jpg)
Quaternion-based correction
1
E
-1
cos σ = u′v
v
B
u
O
-1-0.5
-0.5
y
0
0
A
z
0.5
1
x
00.5 -11
Let z = |OB| be the sonar measurement.Then, the altitude z = |OA| is
z = cosσ · z = u′v · z.
If the quadcopter is upright, u = v andu′v = ‖u‖2 = 1, so z = z.
149 / 174
![Page 178: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/178.jpg)
Quaternion-based correction
1
E
-1
cos σ = u′v
v
B
u
O
-1-0.5
-0.5
y
0
0
A
z
0.5
1
x
00.5 -11
By virtue of the Pythagorean Theoremon 4BAO (A is a right angle), thelength |AB| is
|AB| =√z2 − z2
Both A and B lie on a line E whosedirection is defined by the first twocomponents of u = (ux, uy, uz).
150 / 174
![Page 179: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/179.jpg)
Quaternion-based correction
1
E
-1
cos σ = u′v
v
B
u
O
-1-0.5
-0.5
y
0
0
A
z
0.5
1
x
00.5 -11
On the (x, y)-plane, define the vectoru = (ux, uy). Then, the x and y errors,namely e = (ex, ey), are given by
e = |AB| · u
‖u‖.
151 / 174
![Page 180: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/180.jpg)
Example (i)
A = [-.5 0 0; .5 0 0]; % arm #1B = [0 .5 0; 0 -.5 0]; % arm #2v = [0; 0; -1]; % vector facing downz_ = 1.101718; % measured altitudetheta = 15; phi = 20; psi = 30; % rotationq = quatconj(deg2quat(psi , theta , phi));
u = quatrotate(q, v')';A_ = quatrotate(q, A); % rotated arm #1B_ = quatrotate(q, B); % rotated arm #2cos_sigma = u'*v;z = z_*cos_sigma; % actual altitudeAB = sqrt(z_^2 - z^2); % length |AB|e = AB*u(1:2)/ norm(u(1:2)); % (x,y)−corrections
Check out the documentation of quatrotate (it applies q∗).
152 / 174
![Page 181: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/181.jpg)
Example (ii)
figure; hold on;
% Quadcopter, original upright position:plot3(A(:,1), A(:,2), A(:,3),'--','Color ','k');plot3(B(:,1), B(:,2), B(:,3),'--','Color ','k');plot3 ([0 v(1)] ,[0 v(2)] ,[0 v(3)]);
plot3 ([0 u(1)] ,[0 u(2)] ,[0 u(3)]);
% Quadcopter, rotated by p:plot3(A_(:,1), A_(:,2), A_(:,3),'r','linewidth ' ,2);plot3(B_(:,1), B_(:,2), B_(:,3),'r','linewidth ' ,2);% Position correction:plot3(e(1), e(2), -1, 'x');% Figure configuration (camera, grid, labels):campos ([ -5.3 -6.5 2.8]); axis([-1 1 -1 1 -1 1]);
grid on; xlabel('x'); ylabel('y'); zlabel('z');
153 / 174
![Page 182: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/182.jpg)
Example (iii)
-11
-0.5
0z
1
0.5
0.5
1
0.5
y
0
x
0-0.5 -0.5
-1 -1
(Dashed black lines): original position, (Red lines): rotated quadcopter, (Orange vertical line): vector v, (Purple
tilted line): Vector u, (Point marked with green ×): correction e.
154 / 174
![Page 183: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/183.jpg)
Median filter
Problem: Raw measurements, such as sonar measurements, may givemeasurements which are outright outliers (e.g., altitude measurements of0 or 5m while hovering around 1m). Any kind of moving average,complementary or Kalman-type filters will be affected by these outliers.
Solution: Store the past m measurements (m should preferably be an oddnumber), that is {yk−m+1, . . . , yk} and take the median value; this is thefiltered value yk.
To compute the median of m numbers {a1, . . . , am} with m ≥ 3 an odd integer sort them into a sequence
{a(1), . . . , a(m)} and take a(dm/2e).
155 / 174
![Page 184: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/184.jpg)
Median filter
5 10 15 20
time [s]
0.4
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
altitude [m
]
Original data
Median filter (m=3)
156 / 174
![Page 185: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/185.jpg)
Median filter
5 10 15 20
time [s]
0.4
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
altitude [m
]
Original data
Median filter (m=5)
157 / 174
![Page 186: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/186.jpg)
Median filter
5 10 15 20
time [s]
0.4
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
altitude [m
]
Original data
Median filter (m=7)
158 / 174
![Page 187: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/187.jpg)
Median filter
5 10 15 20
time [s]
0.4
0.5
0.6
0.7
0.8
0.9
1
1.1
1.2
1.3
1.4
altitude [m
]
Original data
Median filter (m=9)
159 / 174
![Page 188: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/188.jpg)
Median filter — Implementation
In MATLAB, use medfilt1(data, m). In C one may use the standardlibrary function qsort to sort an array of numbers in ascending order.
160 / 174
![Page 189: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/189.jpg)
Measurement fusion for localisation
Measurements:
I Sonar (z)
I Camera (x, y)
I Accelerometer (ax, ay, az)
I Total acceleration estimated from ni
Models:
I Kinematics: x = a− (0, 0, g)>
I Total acceleration: a = F/m, where F = F(q, ni)
Methods:
I Kalman filter with bias rejection
I LQR with integral action
161 / 174
![Page 190: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/190.jpg)
Modelling
The total force which is applied by the 4 propellers is
F = CTρD4∑
i=1,...,4
n2i ,
and the (norm of the) total acceleration which is due to the propellers isF/m, where ni is the frequency of rotation of propeller i.
162 / 174
![Page 191: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/191.jpg)
Modelling
To find the direction of the total acceleration we need to use thequaternion which described the orientation of the quadcopter, that is
[0
aprop
]= q ⊗
000Fm
⊗ q−1.In other words, we rotate the vertical upward vector (0, 0, Fm) by thequaternion q. Then the total acceleration isaxay
az
= aprop +
00−g
,where we simply subtracted the gravitational acceleration from az.
163 / 174
![Page 192: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/192.jpg)
Modelling
164 / 174
![Page 193: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/193.jpg)
Modelling
Then, the (x, y, z)-dynamics of the quadcopter’s motion is
x = ax,
y = ay,
z = az.
Where (ax, ay, az) is computed as on slide 163. The above equations leadto the dynamical system
v = a,
r = v,
where v = (vv, vy, vz) is the vector of linear velocities and r = (x, y, z) isthe position of the quadcopter in space.
165 / 174
![Page 194: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/194.jpg)
Auto-pilot
Think of auto-piloting as a replacement of the remote control. Thecontrolled system has manipulated variables
I the pitch command upI the roll command urI the throttle command ut and
I the yaw command which, however, will be disregarded here.
The system has the following measurable outputs
I the position (x, y) of the quadcopter (from the vision module)
I the altitude z (from the sonar)
I the acceleration a(1) from the attitude module
I the acceleration a(2) from the accelerometer of the IMU
166 / 174
![Page 195: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/195.jpg)
Auto-pilot
QuadcopterFlight Dynamics
Observerwith measurement
bias rejection
Controller(s)
167 / 174
![Page 196: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/196.jpg)
Example: Kinematics in continuous time
Define xnav = (r, v) ∈ IR6. The observer on slide 167 is based on the verysimple dynamical system (in continuous time)
ddtxnav =
[03×3 I303×3 03×3
]︸ ︷︷ ︸
Anav
xnav +
[03×3I3
]︸ ︷︷ ︸Bnav
a,
and ynav = (r,a(1),a(2)) ∈ IR12, that is
ynav =
[I3 03×3
06×3 06×3
]︸ ︷︷ ︸
Cnav
xnav +
03×3I3I3
︸ ︷︷ ︸Dnav
a
We have omitted the disturbance terms.
168 / 174
![Page 197: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/197.jpg)
Example: Kinematics in discrete time
The kinematics in discrete time is described by a system
xnav,k+1 = Adnav · xnav,k +Bd
nav · akynav,k = Cd
nav · xnav,k +Ddnav · ak
with matrices
Adnav =
1 h1 h
1 h1
11
, Bdnav =
[h2/2 · I3hI3
],
Cdnav = Cnav, D
dnav = Dnav
169 / 174
![Page 198: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/198.jpg)
Example: Modelling noise (i)
We assume that external disturbances act on the system
xnav,k+1 = Adnav · xnav,k +Bd
nav · ak + wk
ynav,k = Cdnav · xnav,k +Dd
nav · ak + vk +Gddk,
where
I wk ∈ IR6 is a zero-mean process noise
I vk ∈ IR9 is a zero-mean measurement noise
I dk ∈ IR? is a persistent disturbance (dk+1 = dk)
I Gd ∈ IR9×? is a matrix that maps the effect on dk on ynav,k
170 / 174
![Page 199: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/199.jpg)
Example: Modelling noise (ii)
Kinematic model:
xnav,k+1 = Adnav · xnav,k +Bd
nav · ak + wk
ynav,k = Cdnav · xnav,k +Dd
nav · ak + vk +Gddk,
Tasks:
I Perform experiments
I Use the logs to determine the covariance matrices of wk and vkI Choose a dimension for dk and a matrix Gd
X Is there a bias in r and/or a(1) and/or a(2)?X Is the resulting augmented model observable?
I Implement KF with bias rejection
171 / 174
![Page 200: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/200.jpg)
Example: Controller design
A very simple navigation controller is one of the form
up = u0p +Kp(x− xref),
ur = u0r +Kr(y − yref),
ut = u0t +Kt(z − zref).
This can be modified by adding integrators, use the velocity estimates anda lot more.
This part of the desing is left to you.
The equilibrium values u0p, u0
r and u0t need to be determined experimentally.
172 / 174
![Page 201: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/201.jpg)
Flight modes
I Manual mode: The quadcopter receives yaw, pitch, roll and thrustcommands from the RC (done)
I Altitude hold mode: The quadcopter hovers at a constant altitudewhile we may use the rudder on the RC to make it move around
I Loiter mode: The quadcopter stays at a fixed position (x, y, z) andwith fixed yaw ψ
I Reference tracking mode: The quadcopter follows a prescribedtrajectory of (x, y, z, ψ)
173 / 174
![Page 202: Attitude control and autonomous navigation of quadcopters · Attitude control and autonomous navigation of quadcopters ... the quadcopter hover, that is ˚= 0 ... 2. ode45: less fast](https://reader035.vdocuments.net/reader035/viewer/2022070611/5b0797047f8b9ac33f8e6fc5/html5/thumbnails/202.jpg)
References
1. J.P. Hespanha, Lecture notes on LQR/LQG controller design, 2005. Availableonline at http://goo.gl/pZzguU.
2. E. Fresk and G. Nikolakopoulos, Full Quaternion Based Attitude Control for aQuadrotor, 2013 European Control Conference, Zurich, Switzerland, 2013.
3. B.L. Stevens, L.L. Frank, Aircraft Control and Simulation, Wiley-Interscience, 2ndEdition.
4. A. Chovancova et al., Mathematical Modelling and Parameter Identification ofQuadrotor (a survey), Procedia Eng. 96, 2014, pp. 172–181.
5. J.-J. Xiong and E.-H. Zheng, Position and attitude tracking control for aquadrotor UAV, ISA Transactions 53(3), 2014, pp. 725–731.
6. Y. Yang, Analytic LQR Design for Spacecraft Control System Based onQuaternion Model, J. Aerosp. Eng. 25(3), 2012.
7. Y. Yang, Quaternion based model for momentum biased nadir pointing spacecraft,Aerospace Sci. & Tech. 14, 2010.
8. Y. Yang, Spacecraft attitude determination and control: Quaternion basedmethod, Annual Reviews in Control 36(2), 2012.
174 / 174