Helicopter Lab (Optimization and Control)

Norwegian University of Science and Technology

Helicopter Lab

TTK4135 Optimization and Control

Spring 2012

Group members

1003910045100561007510088

Abstract

The purpose of this project is to utilize optimization and control theory, reducing cost func-tions with constraints, and control the helicopter both with and without feedback. The projectis a mandatory assignment in the course TTK4135 – Optimization and Control.

Tasks throughout this report are mainly solved using MATLAB and Simulink. The solutionsare tested at the helicopter lab and compared with its optimal trajectory.

The following topics will be covered

– Discretization of the mathematical model.– Optimal control of pitch/travel, with and without feedback.– Optimal control of pitch/travel and elevation, with and without feedback.– Appendices containing developed Simulink diagrams, plots and MATLAB files.

We assume that the reader has basic knowledge regarding optimization and control theory.

I

Contents

1 The Mathematical Model and Nomenclature 1

2 Optimal Control of Pitch/Travel without Feedback 22.1 Continuous time state space model . . . . . . . . . . . . . . . . . . . . . . . . . . . 22.2 Discrete state space model . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22.3 Optimal Trajectory . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 32.4 Simulation using optimal input sequence . . . . . . . . . . . . . . . . . . . . . . . 4

3 Optimal Control of Pitch/Travel with Feedback 53.1 Linear Quadratic (LQ) Controller . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.2 Implementation of feedback . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 53.3 Model Predictive Control (MPC) . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5

4 Optimal Control of Pitch/Travel and Elevation with and without Feedback 74.1 Continuous state space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74.2 Discrete state space . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 74.3 Inequality constraints on elevation . . . . . . . . . . . . . . . . . . . . . . . . . . . 84.4 Optimal input Sequence with and without feedback . . . . . . . . . . . . . . . . . 84.5 Decoupling of states . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 94.6 Optional exercise . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 9

References 10

A Plots 111.1 Task 2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 111.2 Task 3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 151.3 Task 4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16

B MATLAB Codes 192.1 Task 2.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 192.2 Task 2.4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 222.3 Task 3.2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 252.4 Task 4.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 28

2.4.1 Objective function . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 312.4.2 Nonlinear constraints . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31

2.5 Accompanying scripts . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 322.5.1 Initialization file for helicopter 3 . . . . . . . . . . . . . . . . . . . . . . . . 322.5.2 Enable LATEX interpretation of plot legends . . . . . . . . . . . . . . . . . . 332.5.3 Automate creation of PDF plots . . . . . . . . . . . . . . . . . . . . . . . . 33

C Simulink Diagrams 343.1 Simulink diagram for Task 2.4 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343.2 Simulink diagram for Task 3.2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 343.3 Simulink diagram for Task 4.3 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

III

List of Figures

3.1 Control hierarchy when using MPC (from assignment text). . . . . . . . . . . . . 6A.1 Optimal control value u∗ and resulting states x∗ using q = 0.1. . . . . . . . . . . 11A.2 Optimal control value u∗ and resulting states x∗ using q = 1. . . . . . . . . . . . 12A.3 Optimal control value u∗ and resulting states x∗ using q = 10. . . . . . . . . . . . 13A.4 Calculations compared to measurements when applying the optimal control

value u∗ = p∗c directly without feedback. The set-points p∗ci are weighted by q = 1. 14A.5 Calculations compared to measurements when using the optimal control law

uk = u∗k − K>(xk − x∗k ), thus correction through feedback. . . . . . . . . . . . . . 15A.6 Optimal control without feedback. Nonlinear constraints applied to elevation. . 16A.7 Optimal control with feedback. Nonlinear constraints applied to elevation. . . . 17A.8 Optimal control with feedback and an extended control horizon, N = 80. Non-

linear constraints applied to elevation. . . . . . . . . . . . . . . . . . . . . . . . . . 18C.1 Simulink diagram for Task 2.4. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34C.2 Simulink diagram for Task 3.2. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 34C.3 Simulink diagram for Task 4.3. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 35

IV

NTNU – Department of Engineering Cybernetics Helicopter lab

1 The Mathematical Model and Nomenclature

The mathematical model used in this assignment is summarized by the following equations

e + K3Ked e + K3Kepe = K3Kepec (1.1a)p + K1Kpd p + K1Kpp p = K1Kpp pc (1.1b)

λ = r (1.1c)r = −K2 p (1.1d)

Parameter values and descriptions are given in Tables 1.1 and 1.2.

Table 1.1: Parameters and values

Symbol Parameter Value Unit

la Distance from elevation axis to helicopter body 0.63 mlh Distance from pitch axis to motor 0.18 mK f Force constant motor 0.25 N/VJe Moment of inertia for elevation 0.83 kgm2

Jt Moment of inertia for travel 0.83 kgm2

Jp Moment of inertia for pitch 0.034 kgm2

mh Mass of helicopter 1.05 kgmw Balance weight 1.87 kgmg Effective mass of the helicopter 0.05 kgKp Force to lift the helicopter from the ground 0.49 N

Table 1.2: Variables

Symbol Variable

p Pitchpc Setpoint for pitchλ Travelr Speed of travelrc Setpoint for speed of travele Elevationec Setpoint for elevationVf Voltage, motor in frontVb Voltage, motor in backVd Voltage difference, Vf −VbVs Voltage sum, Vf + VbKpp, Kpd, Kep, Kei, Ked Controller gainsTg Moment needed to keep the helicopter flying

Page 1 of 35


2 Optimal Control of Pitch/Travel without Feedback

This part of the assignment covers optimal control of pitch and travel without using feedback.An optimal input sequence is computed as the solution of a QP problem, and applied to thehelicopter. The sequence makes the helicopter travel an angle of 180 degrees.

2.1 Continuous time state space model

A general linear continuous time state space model can be written as

x = Acx + Bcu

In this part the following state vector and controller output is used.

x =[λ r p p

]> and u = pc (2.1)

By rearranging the equations of the summarized model (1.1), we obtain

λ = r (2.2a)r = −K2 p (2.2b)p = −K1Kpd p− K1Kpp p + K1Kpp pc (2.2c)

which can be stated in the following state space representation

x =

0 1 0 00 0 −K2 00 0 0 10 0 −K1Kpp −K1Kpd

︸︷︷︸

Ac

x +

000

K1Kpp

︸︷︷︸

Bc

u (2.3)

This model incorporates four states, consisting of travel, speed of travel, pitch and pitch rate.The input to the model is the desired pitch angle. As we see, no equations for the elevationor elevation rate are included in the model. This can be done because the basic control layer(Figure 7 in the assignment) takes care of the elevation, using a constant reference. The restof the system can assume that the elevation is constant and independent of the rest of thedynamics. Decoupling the states in this way simplifies the model and control problem, butthe model accuracy will decrease.

2.2 Discrete state space model

The continuous state space model (2.3) is to be discretized using the forward Euler method.Euler’s method approximates derivatives by lines of constant slope between consecutive pointsin time, mathematically written as

x ≈ xk+1 − xk

∆t(2.4)

where ∆t is the time step between each sample k. The continuous model (2.3) can be dis-cretized as follows

xk+1 − xk

∆t= Acxk + Bcuk

xk+1 = (I + ∆tAc)︸︷︷︸A

xk + ∆tBc︸︷︷︸B

uk (2.5)

Page 2 of 35


Expanding the discrete-time matrices A and B for our system, yields

xk+1 =

1 ∆t 0 00 1 −∆tK2 00 0 1 ∆t0 0 −∆tK1Kpp 1− ∆K1Kpd

xk +

000

∆tK1Kpp

uk (2.6)

The state vector x and controller output u remains the same as depict in (2.1).

2.3 Optimal Trajectory

The following cost function is to be minimized

φ =N

∑i=1

(λi − λ f )2 + qp2

ci where q ≥ 0 (2.7)

with the given linear constraints

|pk| <30π

180and |pck| <

30π

180for all k ∈ {1, . . . , N} (2.8)

which limits the pitch and pitch setpoint. The aim is to move the helicopter from the initialpoint x0 =

[λ0 0 0 0

]> to the desired endpoint x f =[λ f 0 0 0

]>, given λ0 = π andλ f = 0, i.e. a travel of 180 degrees. From (2.7) it is clear that this is a QP problem, which canbe rewritten to the standard representation

φ =12

N−1

∑k=0{x>k Qxk + ukRuk} (2.9)

Despite the unusual naming q, the weight matrices are identified as

Q = 2 · diag{

1 0 0 0}

and R = 2q (2.10)

Further transformation of the problem, to include the summation internally in the matrices,yields

minz

z>Gz (2.11a)

s.t. AQPz = BQP (2.11b)

where the new state vector, which includes all the time steps of both x and u, is given by

z =[x>1 . . . x>N , u>0 . . . u>N−1

]> ∈ R300×1 (2.12)

Page 3 of 35


The rest of the matrices are constructed as follows

G =

Q 0 · · · · · · · · · 0

0. . . . . .

......

. . . Q. . .

......

. . . R. . .

......

. . . . . . 00 · · · · · · · · · 0 R

∈ R300×300 (2.13)

AQP =

I 0 · · · · · · 0 −B 0 · · · · · · 0

−A I. . .

... 0. . . . . .

...

0. . . . . . . . .

......

. . . . . . . . ....

.... . . . . . . . . 0

.... . . . . . 0

0 · · · 0 −A I 0 · · · · · · 0 −B

∈ R240×300 (2.14)

BQP =

Ax0

0...0

∈ R240×1 (2.15)

The dimensions shown are for both a control horizon and a prediction horizon of N = M = 60time steps.

Now, the system is ready to be solved by MATLAB’s quadprog function. Throughout thistask it is assumed a constant, decoupled, elevation angle.

The term (λi − λ)2 is a calculated deviation after each indices, with regards to travel. Un-wanted effects can arise when steering the helicopter to λ = λ f with this objective function,since it does not take the travel rate into account. A result can be that the helicopter just passesthrough λ f (without stopping) when the optimal sequence is finished.

The presented plots in Figures A.1, A.2 and A.3, shows the computed optimal control valuesu∗ and states x∗ for q = 0.1, q = 1 and q = 10, respectively. A high value of q will make thepitch setpoint term dominate the cost function, thus high values will be punished. The controlbecomes smoother as well as reducing wear and tear on the machinery. A result of good pitchcontrol is a smoother travel trajectory, but the duration of the optimal travel increases for highvalues of q and decreases for low values.

2.4 Simulation using optimal input sequence

Figure A.4 shows how the helicopter behaves when the calculated optimal input sequence u∗

is implemented. The parameter q is set to 1, so the error term (λ− λ f )2 and the control-value

qp2ci are equally punished in the objective function (2.7).

As seen in the plot, both travel and travel rate miss the desired endpoint of zero because ofthe lack of feedback. If we had a perfect model, no deviation would be observed. In real lifethere will always be model errors, some large enough to require feedback. The model error ofcourse increases when discretizing.

Page 4 of 35


3 Optimal Control of Pitch/Travel with Feedback

In this task, feedback is introduced to the controller from the previous task.

3.1 Linear Quadratic (LQ) Controller

Feedback is introduced to the optimal controller (2.1) in form of a linear quadratic controller.This means that the controller minimizes a quadratic criteria J, given by

J =∞

∑i=0

∆x>i+1QLQ∆xi+1 + ∆u>i RLQ∆ui (3.1)

where QLQ ≥ 0 and RLQ > 0 are diagonal matrices weighting the deviation ∆x and ∆ufrom the optimal pre-calculated values x∗ and u∗. Feedback is introduced with the followingmanipulated variable

uk = u∗k − K>(xk − x∗k ) (3.2)

The optimal feedback matrix K was calculated by the MATLAB function dlqr, assuming themanipulated variable is given by (3.2). Computation yields

K =[−1.3170 −3.5785 −0.3113 0.0737

](3.3)

when using the weight matrices

QLQ = diag{

10 0.2 0.1 0}

and RLQ = 1 (3.4)

The weights are mainly based on trial and error, with emphasis on the travel λ. The derivativevariables r and p have low weights, because they contain high frequency noise, which isundesirable in this control system. The value of R makes u∗ smoother, thus minimizing wearand tear on the helicopter.

3.2 Implementation of feedback

Figure A.5 shows the response of the helicopter compared to the computed values. In theappendix, Figure C.2 shows the Simulink diagram for the simulation. Feedback makes thesystem able to measure the helicopters position and pitch angle for each time step. Whenfeedback is implemented into the model, the travel will follow the optimal trajectory betterthan without feedback. The pitch however, will follow the optimal trajectory better withoutfeedback. Since travel is related to pitch, and the travel is weighted higher, the helicopter willfollow the travel trajectory. It is not possible to make both pitch and travel follow the optimaltrajectories perfectly at the same time.

3.3 Model Predictive Control (MPC)

Using Model Predictive Control, the control hierarchy would be similar to Figure 3.1. Inthis system, the “Product costs” will be the weight on each of the setpoints in the objectivefunction. A high weight indicates a high cost (e.g. expensive fuel, maintenances costs, etc.),which should be avoided.

With MPC there is a moving prediction horizon, this should be chosen so that it includes thebulk part of the reference, this will determine the length of the prediction horizon. Model

Page 5 of 35


Model-based optimization withModel PredictiveContol (MPC)

Linear-Quadratic Regulator (LQR)

Pitch controller (PD)Elevation controller (PID)

Plant (helicopter)

u* x*

u

x

Product costsPlant constraints

Vd, Vs

Figure 3.1: Control hierarchy when using MPC (from assignment text).

accuracy depends on the re-optimization, which is executed after each time step in whicherrors are corrected. Re-optimizations means that for each step there is a new optimizationproblem being solved, for instance LP problem, QP problem, etc.

The main objective with MPC control is to calculate future variables in order to optimize futurebehavior. This is accomplished by tuning the weight matrices Q and R as well as adjusting thelength of the prediction horizon. Tuning is done in order to obtain an acceptable closed-loopperformance. Shorter horizon gives lower complexity optimization problems at each timestep, which result in less computational load. A longer horizon gives better performance, butincreases computational load at each time step.

The following is a summary of the properties of LQR and MPC (Imsland 2007).

Linear Quadratic Regulator

– Advantages• It is inherently multivariable (it takes care of couplings in the process).• It is optimal on the infinite horizon.• It can be shown that it has good robustness properties (due to feedback).

– Disadvantages• It does not look to the future.• It does not handle constraints on states and inputs.

Model Predictive Control

– Advantages• It looks to the future.• It handles constraints on states and inputs.

– Disadvantages• It is not optimal.• It requires more computational power, and have strict real-time demands.

Page 6 of 35


4 Optimal Control of Pitch/Travel and Elevation with and withoutFeedback

The objective of this task is optimal control of pitch/travel and elevation with and withoutfeedback; that is, we have an optimal trajectory in two dimensions.

4.1 Continuous state space

Introducing two additional states, elevation e and elevation rate e, the new state vector be-comes

x =[λ r p p e e

]> , u =[pc ec

]> (4.1)

Writing the system on continuous state space form, yields

x =

0 1 0 0 0 00 0 −K2 0 0 00 0 0 1 0 00 0 −K1Kpp −K1Kpd 0 00 0 0 0 0 10 0 0 0 −K3Kep −K3Ked

x

+

0 00 00 0

K1Kpp 00 00 K3Kep

u

(4.2)

4.2 Discrete state space

The acquired state space model is discretized using the forward Euler method. This wassolved in the same manner as in section (2.2). Thus resulting in

xk+1 =

1 ∆t 0 0 0 00 1 −∆tK2 0 0 00 0 1 ∆t 0 00 0 −∆tK1Kpp 1− ∆tK1Kpd 0 00 0 0 0 1 ∆t0 0 0 0 −∆tK3Kep 1− ∆tK3Ked

xk

+

0 00 00 0

∆tK1Kpp 00 00 ∆tK3Kep

uk

(4.3)

Page 7 of 35


4.3 Inequality constraints on elevation

The objective function that is to be minimized, is stated as

φ =N

∑i=1

(λi − λ f )2 + q1 pci

2 + q2eci2 (4.4)

The inequality constraint imposed on the elevation is given as

ek ≥ αe−β(λk−λt)2for all k ∈ {1, . . . , N} (4.5)

where

α = 0.2, β = 20 and λt =2π

3(4.6)

The MATLAB function fmincon was used in order to solve the optimization problem. Thisdiffers from the function used previously, since the problem at hand involves a nonlinear con-straint instead of a linear constraint. After each time step the constraint (4.5) was implementedand passed to the function, in the form of

c(xk) = αe−β(λk−λt)2 − ek ≤ 0 (4.7)

The optimization problem was solved with different values for q1 and q2. Initial values wasq1 = q2 = 1. If the values are increased, large values of pci and eci are punished, resulting infmincon failing to find an optimal solution. This is intuitively right, since the system has lessdegrees of freedom when applying stricter constrains.

The fmincon was not able to perform the optimization for N = 40. To get a better result alonger optimization and prediction horizon was tried. When N = 80, fmincon was able tocalculate the optimal solution for the problem.

4.4 Optimal input Sequence with and without feedback

As in 3.1, we use a LQ controller to correct the optimal input sequence. It was necessary tore-tune the controller, and the best results were obtained when using

QLQ = diag{

5 0 0.1 0 2 0}

and RLQ = diag{

1 1}

(4.8)

As the values indicates, the travel λ is assumed to be the most “important” state to control,and the pitch p thereafter. The resulting gain matrix became

K =

[−0.9502 −2.9437 −0.3441 0.0692 0 0

0 0 0 0 0.6425 0.8635

](4.9)

Figure A.6 shows how the helicopter responds to the optimal sequence u∗ when not usingfeedback. This causes the real pitch to deviate from the optimal pitch and real elevation todeviate from the optimal elevation.

When feedback is introduced, as shown in Figure A.7, the LQ controller correct most of theearlier observed deviation in pitch and travel. Introducing feedback clearly improves theperformance.

Page 8 of 35


The travel plots in both Figure A.6 (no feedback) and Figure A.7 (with feedback ) shows thatthe optimal travels are not brought to zero in an optimal way using M = N = 40. Thealgorithm is not able to calculate the optimal travel because the time horizon is to short andthe constrains are to strict. Despite this, the travels are (eventually) brought to zero after theoptimal sequence is finished, by the LQ controller. This is caused by the padding of zeros.

Extending the control horizon and prediction horizon to M = N = 80 (as described in theprevious task) gives the helicopter some extra time to stabilize. The results of this flight withfeedback are shown in Figure A.8. It is clear that this work much better, but the optimizationalgorithm needs longer time to find the solution.

4.5 Decoupling of states

In the model the first 4 states are completely decoupled from the last 2. This is not the casein reality. In reality a change of the pitch would affect the elevation of the helicopter. Alsothe travel rate would be affected if a change in the elevation is made while the pitch angle isdifferent from zero.

An example of this is the observed behavior from the previous task:The elevation plots in Figure A.6 and Figure A.7 shows that the real elevation deviates fromthe optimal elevation. This happens because the last two states are decoupled from the firstfour in the model of which the optimal trajectory is based. This makes the elevation changeof the helicopter occur earlier than the change of the optimal elevation, because the helicopterspeeds up when it tries to increase the elevation.

A solution to this would be to improve the model, ensuring coupling between all states. Thisalso implies possibly making the model non-linear (which fmincon can handle).

4.6 Optional exercise

We tried to add constraints to the last state vector, xN = 0, to force all the states to zero at theend of the horizon. It was clear that the problem then became infeasible. Attempting to makethe problem feasible, we relaxed some of the states, but with no luck.

Page 9 of 35


References

Foss, B. A. (2004), ‘Linear quadratic control’, Rev. 2007 .

Imsland, L. (2007), ‘Introduction to model predictive control’.

Knuth, D. E., Larrabee, T. & Roberts, P. M. (1997), ‘Mathematical Writing’, Knowl. Eng. Rev.12, 331–334.URL: http://dl.acm.org/citation.cfm?id=976246.976258

Nocedal, J. & Wright, S. (2006), Numerical optimization, Springer series in operations research,2 edn, Springer.

Page 10 of 35

http://dl.acm.org/citation.cfm?id=976246.976258


A Plots

1.1 Task 2

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal control value u

∗ = p∗

c

−50

0

50

100

150

200

Deg

rees

[d

eg]

Optimal travel λ∗

−30

−20

−10

0

10

Deg

rees

/se

c [d

eg/

s]

Optimal travel rate r

∗

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal pitch p

∗

0 5 10 15 20 25−300

−200

−100

0

100

200

Time [s]

Deg

rees

/se

c [d

eg/

s]

Optimal pitch rate p

∗

Figure A.1: Optimal control value u∗ and resulting states x∗ using q = 0.1.

Page 11 of 35


−40

−20

0

20

40

Deg

rees

[d

eg]


∗ = p∗

c

−50

0

50

100

150

200

Deg

rees

[d

eg]


−30

−20

−10

0

10

Deg

rees

/se

c [d

eg/

s]


∗

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal pitch p

∗

0 5 10 15 20 25−300

−200

−100

0

100

200

Time [s]

Deg

rees

/se

c [d

eg/

s]


∗

Figure A.2: Optimal control value u∗ and resulting states x∗ using q = 1.

Page 12 of 35


−10

0

10

20

30

Deg

rees

[d

eg]


∗ = p∗

c

−50

0

50

100

150

200

Deg

rees

[d

eg]


−20

−15

−10

−5

0

5

Deg

rees

/se

c [d

eg/

s]


∗

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal pitch p

∗

0 5 10 15 20 25−300

−200

−100

0

100

200

Time [s]

Deg

rees

/se

c [d

eg/

s]


∗

Figure A.3: Optimal control value u∗ and resulting states x∗ using q = 10.

Page 13 of 35


−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal control value u∗ = p∗

c

−100

0

100

200

Deg

rees

[d

eg]


Real travel λ

−60

−40

−20

0

20

Deg

rees

/se

c [d

eg/

s]

Optimal travel rate r∗

Real travel rate r

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal pitch p∗

Real pitch p

0 5 10 15 20 25−50

0

50

100

150

Time [s]

Deg

rees

/se

c [d

eg/

s]

Optimal pitch rate p∗

Real pitch rate p

Figure A.4: Calculations compared to measurements when applying the optimal control valueu∗ = p∗c directly without feedback. The set-points p∗ci are weighted by q = 1.

Page 14 of 35


1.2 Task 3

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal control value u∗ = p∗

c

0

50

100

150

200

Deg

rees

[d

eg]


Real travel λ

−30

−20

−10

0

10

Deg

rees

/se

c [d

eg/

s]


Real travel rate r

−40

−20

0

20

40

Deg

rees

[d

eg]

Optimal pitch p∗

Real pitch p

0 5 10 15 20 25−50

0

50

100

150

Time [s]

Deg

rees

/se

c [d

eg/

s]


Real pitch rate p

Figure A.5: Calculations compared to measurements when using the optimal control lawuk = u∗k − K>(xk − x∗k ), thus correction through feedback.

Page 15 of 35


1.3 Task 4

−20

0

20

40

Deg

rees

Optimal pitch control u∗1 = p∗

c

−10

0

10

20

Deg

rees

Optimal elevation control u∗2 = e∗

c

−200

0

200

Deg

rees


Real travel λ

−40

−20

0

20

Deg

rees

/se

c


Real travel rate r

−50

0

50

Deg

rees

Optimal pitch p∗

Real pitch p

−200

0

200

Deg

rees

/se

c


Real pitch rate p

−5

0

5

10

15

Deg

rees

Optimal elevation e

∗

Real elevation e

0 2 4 6 8 10 12 14 16 18 20−20

0

20

Time [s]

Deg

rees

/se

c

Real elevation rate e

Optimal elevation rate e∗

Figure A.6: Optimal control without feedback. Nonlinear constraints applied to elevation.

Page 16 of 35


−20

0

20

40

Deg

rees


c

−10

0

10

20

Deg

rees


c

−200

0

200

Deg

rees


Real travel λ

−40

−20

0

20

Deg

rees

/se

c


Real travel rate r

−50

0

50

Deg

rees

Optimal pitch p∗

Real pitch p

−200

0

200

Deg

rees

/se

c


Real pitch rate p

−5

0

5

10

15

Deg

rees

Optimal elevation e

∗

Real elevation e

0 2 4 6 8 10 12 14 16 18 20−20

0

20

Time [s]

Deg

rees

/se

c



Figure A.7: Optimal control with feedback. Nonlinear constraints applied to elevation.

Page 17 of 35


−50

0

50

Deg

rees


c

−10

0

10

20

Deg

rees


c

−200

0

200

Deg

rees


Real travel λ

−40

−20

0

20

Deg

rees

/se

c


Real travel rate r

−50

0

50

Deg

rees

Optimal pitch p∗

Real pitch p

−200

0

200

Deg

rees

/se

c


Real pitch rate p

−5

0

5

10

15

Deg

rees

Optimal elevation e

∗

Real elevation e

0 5 10 15 20 25 30−20

0

20

Time [s]

Deg

rees

/se

c



Figure A.8: Optimal control with feedback and an extended control horizon, N = 80. Nonlin-ear constraints applied to elevation.

Page 18 of 35


B MATLAB Codes

2.1 Task 2.3

1 % oppg10_2_3.m23 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%4 %%%%%%%%%%%%%%%%%%%%%%% 10.2.3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%67 clear all89 for runde = 1:1:3 % q−loop

1011 disp(['RundeXXX ', sprintf('%u', runde)]);12 close all1314 init;15 delta_t = 0.25; % tastetid16 sek_forst = 5;1718 % Setter opp modellen for systemet. x = [lambda r p p_dot]'19 A1 = [1 delta_t 0 0;20 0 1 −delta_t*K_2 0;21 0 0 1 delta_t;22 0 0 −delta_t*K_1*K_pp 1−delta_t*K_1*K_pd];23 B1 = [0;24 0;25 0;26 delta_t*K_1*K_pp];2728 % Antall pådrag og tilstander29 mx = size(A1,1); % Antall tilstander (antall rader i A)30 mu = size(B1,2); % Antall paadrag (antall kolonner i B)3132 % Initialverdier33 lambda_0 = pi;34 x0 = [lambda_0 0 0 0]'; % Initialverdier3536 % Tidshorisont og initialisering37 N = 60; % Tidshorisont for tilstander38 M = N; % Tidshorisont for pådrag39 z = zeros(N*mx+M*mu,1); % Initialiserer z for hele horisonten40 z0 = z; % Startverdi for optimaliseringen41 z0(1,1) = lambda_0;4243 % Cost function44 I_n = eye(N);45 Qk = 2*diag([1 0 0 0]);46 Q = kron(I_n, Qk);4748 % q er vekten på p^2_{ci}, i=1,...,N = ALLE pådrag.49 if runde == 150 q = 0.1;51 elseif runde == 252 q = 1;53 elseif runde == 354 q = 10;55 end565758 Rk = 2*q;59 R = kron(I_n, Rk);60 G = blkdiag(Q, R);616263 % Equality constraint64 I_n = eye(N);65 Aeq_c1 = eye(N*mx); % Component 1 of A66 Aeq_c2 = kron(diag(ones(N−1,1),−1), −A1); % Component 2 of A

Page 19 of 35


67 Aeq_c3 = kron(I_n, −B1); % Component 3 of A68 Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3];69 beq = [A1*x0; zeros((N−1)*mx,1)];707172 %%%%%%%%%%%%%%73 %%%% Krav %%%%74 %%%%%%%%%%%%%%75 %76 % p_k < 30 pi / 180 = tilstanden x3_k77 % OG78 % p_c < 30 pi / 180 = pådraget u_k79 %80 % Husk at p_k = pitch = x3 og81 % p_c = pådrag = u82 %83 %%%%%%%%%%%%%%8485 p_limit = 30*pi/180; % degrees868788 % Inequality constraints89 ul = −p_limit*ones(N*mu,1); % −30 deg < pitch−pådrag < 30 deg90 uu = p_limit*ones(N*mu,1);91 ul(N*mu) = 0; % Siste pådrag skal være 092 uu(N*mu) = 0;93 xl = −Inf(N*mx,1); % Begrensning for tilstandene (ingen begrensning)94 xu = Inf(N*mx,1);95 xl(3:mx:N*mx) = −p_limit; % −30 deg < pitch x3 < 30 deg96 xu(3:mx:N*mx) = p_limit;9798 lambda_f = 0;99 xf = zeros(mx,1);

100 xf(1,1) = lambda_f;101102 xl(N*mx−mx+1:N*mx) = xf; % Siste x_N = xf = [0 0 0 0]103 xu(N*mx−mx+1:N*mx) = xf;104105 zl = [xl;ul];106 zu = [xu;uu];107108109 % Solving the equality− and inequality−constrained QP with quadprog110 opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off');111 tic112 [z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt);113 t1 = toc;114115116 % Beregnede paadrag og tilstander117 u = [z(N*mx+1:N*mx+M*mu);z(N*mx+M*mu)]; % Beregnet paadrag118 x1 = [x0(1);z(1:mx:N*mx)]; % Beregnet tilstand x1119 x2 = [x0(2);z(2:mx:N*mx)]; % Beregnet tilstand x2120 x3 = [x0(3);z(3:mx:N*mx)]; % Beregnet tilstand x3121 x4 = [x0(4);z(4:mx:N*mx)]; % Beregnet tilstand x4122123 Antall = sek_forst/delta_t;124 Nuller = zeros(Antall,1);125 Enere = ones(Antall,1);126127 u = [Nuller; u; Nuller];128 x1 = [lambda_0*Enere; x1; Nuller];129 x2 = [Nuller; x2; Nuller];130 x3 = [Nuller; x3; Nuller];131 x4 = [Nuller; x4; Nuller];132133 % figur134 t = (0:delta_t:delta_t*(length(u)−1))'; % Virkelig tid135136137 R2D = 180/pi;138 u = u*R2D;139 x1 = x1*R2D;

Page 20 of 35


140 x2 = x2*R2D;141 x3 = x3*R2D;142 x4 = x4*R2D;143144145 %%%%%%%%%%%%%%%%%146 % Lag figurer for q = 0.1, 1, 10147 %%%%%%%%%%%%%%%%%148 figure(1)149 subplot(5,1,1)150 stairs(t,u)151 legend('Optimal control value $u^* = p_c^*$')152 ylabel('Degrees [deg]')153 fixplot;154155 subplot(5,1,2)156 plot( t,x1,'−mo');157 legend('Optimal travel $\lambda^*$','Real travel $\lambda$')158 ylabel('Degrees [deg]')159 fixplot;160161 subplot(5,1,3)162 plot(t,x2','−mo');163 legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthEast')164 ylabel('Degrees/sec [deg/s]')165 fixplot;166167 subplot(5,1,4)168 plot(t,x3,'−mo');169 legend('Optimal pitch $p^*$','Real pitch $p$')170 ylabel('Degrees [deg]')171 fixplot;172173 subplot(5,1,5)174 plot(t,x4','−mo');175 legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$')176 xlabel('Time [s]')177 ylabel('Degrees/sec [deg/s]')178 fixplot;179180 set(gca,'XTickLabelMode','Auto');181182183 set(gcf,'PaperType', 'A4')184 set(gcf,'PaperOrientation', 'portrait')185 papersize=get(gcf,'PaperSize');186 set(gcf,'PaperPositionMode','manual')187 set(gcf,'PaperPosition',[0.25 0.25 papersize(1)−0.5 papersize(2)−0.5])188189 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%190 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%191192 filnavn = ['10_2_3_q' , sprintf('%u',q*10)];193194 createPdf(filnavn)195 grid on196 set(gcf,'Position',[100,00 700,700])197198 end % for

Page 21 of 35


2.2 Task 2.4

1 % oppg10_2_4.m23 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%4 %%%%%%%%%%%%%%%%%%%%%%% 10.2.4 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%67 clear all89 init;

10 delta_t = 0.25; % tastetid11 sek_forst = 5;1213 % Setter opp modellen for systemet. x = [lambda r p p_dot]'14 A1 = [1 delta_t 0 0;15 0 1 −delta_t*K_2 0;16 0 0 1 delta_t;17 0 0 −delta_t*K_1*K_pp 1−delta_t*K_1*K_pd];18 B1 = [0;19 0;20 0;21 delta_t*K_1*K_pp];2223 % Antall pådrag og tilstander24 mx = size(A1,1); % Antall tilstander (antall rader i A)25 mu = size(B1,2); % Antall paadrag (antall kolonner i B)2627 % Initialverdier28 lambda_0 = pi;29 x0 = [lambda_0 0 0 0]'; % Initialverdier3031 % Tidshorisont og initialisering32 N = 60; % Tidshorisont for tilstander33 M = N; % Tidshorisont for pådrag34 z = zeros(N*mx+M*mu,1); % Initialiserer z for hele horisonten35 z0 = z; % Startverdi for optimaliseringen36 z0(1,1) = lambda_0;3738 % Cost function39 I_n = eye(N);40 Qk = 2*diag([1 0 0 0]);41 Q = kron(I_n, Qk);4243 % q er vekten på p^2_{ci}, i=1,...,N = ALLE pådrag.44 q = 1;454647 Rk = 2*q;48 R = kron(I_n, Rk);49 G = blkdiag(Q, R);505152 % Equality constraint53 I_n = eye(N);54 Aeq_c1 = eye(N*mx); % Component 1 of A55 Aeq_c2 = kron(diag(ones(N−1,1),−1), −A1); % Component 2 of A56 Aeq_c3 = kron(I_n, −B1); % Component 3 of A57 Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3];58 beq = [A1*x0; zeros((N−1)*mx,1)];596061 %%%%%%%%%%%%%%62 %%%% Krav %%%%63 %%%%%%%%%%%%%%64 %65 % p_k < 30 pi / 180 = tilstanden x3_k66 % OG67 % p_c < 30 pi / 180 = pådraget u_k68 %69 % Husk at p_k = pitch = x3 og

Page 22 of 35


70 % p_c = pådrag = u71 %72 %%%%%%%%%%%%%%737475 p_limit = 30*pi/180; % degrees767778 % Inequality constraints79 ul = −p_limit*ones(N*mu,1); % −30 deg < pitch−pådrag < 30 deg80 uu = p_limit*ones(N*mu,1);81 ul(N*mu) = 0; % Siste pådrag skal være 082 uu(N*mu) = 0;83 xl = −Inf(N*mx,1); % Begrensning for tilstandene (ingen begrensning)84 xu = Inf(N*mx,1);85 xl(3:mx:N*mx) = −p_limit; % −30 deg < pitch x3 < 30 deg86 xu(3:mx:N*mx) = p_limit;8788 lambda_f = 0;89 xf = zeros(mx,1);90 xf(1,1) = lambda_f;9192 xl(N*mx−mx+1:N*mx) = xf; % Siste x_N = xf = [0 0 0 0]93 xu(N*mx−mx+1:N*mx) = xf;9495 zl = [xl;ul];96 zu = [xu;uu];979899 % Solving the equality− and inequality−constrained QP with quadprog

100 opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off');101 tic102 [z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt);103 t1 = toc;104105 % Beregnede paadrag og tilstander106 u = [z(N*mx+1:N*mx+M*mu);z(N*mx+M*mu)]; % Beregnet paadrag107 x1 = [x0(1);z(1:mx:N*mx)]; % Beregnet tilstand x1108 x2 = [x0(2);z(2:mx:N*mx)]; % Beregnet tilstand x2109 x3 = [x0(3);z(3:mx:N*mx)]; % Beregnet tilstand x3110 x4 = [x0(4);z(4:mx:N*mx)]; % Beregnet tilstand x4111112 Antall = sek_forst/delta_t;113 Nuller = zeros(Antall,1);114 Enere = ones(Antall,1);115116 u = [Nuller; u; Nuller];117 x1 = [lambda_0*Enere; x1; Nuller];118 x2 = [Nuller; x2; Nuller];119 x3 = [Nuller; x3; Nuller];120 x4 = [Nuller; x4; Nuller];121122 % figur123 t = (0:delta_t:delta_t*(length(u)−1))'; % Virkelig tid124 tstop= 25;125 u_sim = [t,u];126127 %%%%%%%%%%%%%%%%%128 %sim heli_2_4129 %%%%%%%%%%%%%%%%%130131132 R2D = 180/pi;133 u = u*R2D;134 x1 = x1*R2D;135 x2 = x2*R2D;136 x3 = x3*R2D;137 x4 = x4*R2D;138139140 %%%%%%%%%%%%%%%%%141 load heli_2_4142 %%%%%%%%%%%%%%%%%

Page 23 of 35


143 figure(1)144 subplot(5,1,1)145 stairs(t,u)146 legend('Optimal control value $u^* = p_c^*$')147 ylabel('Degrees [deg]')148 fixplot;149150 subplot(5,1,2)151 plot( t,x1,'−mo', rt_lambda(:,1),rt_lambda(:,2));152 legend('Optimal travel $\lambda^*$','Real travel $\lambda$')153 ylabel('Degrees [deg]')154 fixplot;155156 subplot(5,1,3)157 plot(t,x2','−mo', rt_r(:,1),rt_r(:,2));158 legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthEast')159 ylabel('Degrees/sec [deg/s]')160 fixplot;161162 subplot(5,1,4)163 plot(t,x3,'−mo', rt_p(:,1),rt_p(:,2));164 legend('Optimal pitch $p^*$','Real pitch $p$')165 ylabel('Degrees [deg]')166 fixplot;167168 subplot(5,1,5)169 plot(t,x4','−mo', rt_p_dot(:,1),rt_p_dot(:,2));170 legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$')171 xlabel('Time [s]')172 ylabel('Degrees/sec [deg/s]')173 fixplot;174175 set(gca,'XTickLabelMode','Auto');176177178 set(gcf,'PaperType', 'A4')179 set(gcf,'PaperOrientation', 'portrait')180 papersize=get(gcf,'PaperSize');181 set(gcf,'PaperPositionMode','manual')182 set(gcf,'PaperPosition',[0.25 0.25 papersize(1)−0.5 papersize(2)−0.5])183184 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%185 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%186 createPdf('10_2_4_x')187 grid on188 set(gcf,'Position',[100,00 700,700])

Page 24 of 35


2.3 Task 3.2

1 % oppg10_3_2.m23 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%4 %%%%%%%%%%%%%%%%%%%%%% 10.3.1 + 10.3.2 %%%%%%%%%%%%%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%67 init;8 delta_t = 0.25; % tastetid9 sek_forst = 5;

1011 % Setter opp modellen for systemet. x = [lambda r p p_dot]'12 A1 = [1 delta_t 0 0;13 0 1 −delta_t*K_2 0;14 0 0 1 delta_t;15 0 0 −delta_t*K_1*K_pp 1−delta_t*K_1*K_pd];16 B1 = [0;17 0;18 0;19 delta_t*K_1*K_pp];2021 % Antall pÃ¥drag og tilstander22 mx = size(A1,1); % Antall tilstander (antall rader i A)23 mu = size(B1,2); % Antall paadrag (antall kolonner i B)2425 % Initialverdier26 lambda_0 = pi;27 x0 = [lambda_0 0 0 0]'; % Initialverdier2829 % Tidshorisont og initialisering30 N = 60; % Tidshorisont for tilstander31 M = N; % Tidshorisont for pÃ¥drag32 z = zeros(N*mx+M*mu,1); % Initialiserer z for hele horisonten33 z0 = z; % Startverdi for optimaliseringen3435 % Cost function36 I_n = eye(N);37 Qk = 2*diag([1, 0, 0, 0]);38 Q = kron(I_n, Qk);39 q = 1;40 Rk = 2*q;41 R = kron(I_n, Rk);42 G = blkdiag(Q, R);434445 % Equality constraint46 I_n = eye(N);47 Aeq_c1 = eye(N*mx); % Component 1 of A48 Aeq_c2 = kron(diag(ones(N−1,1),−1), −A1); % Component 2 of A49 Aeq_c3 = kron(I_n, −B1); % Component 3 of A50 Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3];51 beq = [A1*x0; zeros((N−1)*mx,1)];525354 p_limit = 30*pi/180; % degrees5556 % Inequality constraints57 ul = −p_limit*ones(N*mu,1); % −30 deg < pitch−pådrag < 30 deg58 uu = p_limit*ones(N*mu,1);59 ul(N*mu) = 0; % Siste pådrag skal være 060 uu(N*mu) = 0;61 xl = −Inf(N*mx,1); % Begrensning for tilstandene (ingen begrensning)62 xu = Inf(N*mx,1);63 xl(3:mx:N*mx) = −p_limit; % −30 deg < pitch x3 < 30 deg64 xu(3:mx:N*mx) = p_limit;6566 lambda_f = 0;67 xf = zeros(mx,1);68 xf(1,1) = lambda_f;69

Page 25 of 35


70 xl(N*mx−mx+1:N*mx) = xf; % Siste x_N = xf = [0 0 0 0]71 xu(N*mx−mx+1:N*mx) = xf;7273 zl = [xl;ul];74 zu = [xu;uu];757677 % Solving the equality− and inequality−constrained QP with quadprog78 opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off');79 tic80 [z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt);81 t1 = toc;8283 % Optimal feedback matrix K, task 10.3.184 Qlq = diag([10 0.2 0.1 0]);85 Rlq = 1 ;86 [K,~,~] = dlqr(A1,B1,Qlq,Rlq);878889 % Beregnede paadrag og tilstander90 u = [z(N*mx+1:N*mx+M*mu);z(N*mx+M*mu)]; % Beregnet paadrag91 x1 = [x0(1);z(1:mx:N*mx)]; % Beregnet tilstand x192 x2 = [x0(2);z(2:mx:N*mx)]; % Beregnet tilstand x293 x3 = [x0(3);z(3:mx:N*mx)]; % Beregnet tilstand x394 x4 = [x0(4);z(4:mx:N*mx)]; % Beregnet tilstand x49596 Antall = sek_forst/delta_t;97 Nuller = zeros(Antall,1);98 Enere = ones(Antall,1);99

100101 u = [Nuller; u; Nuller];102 x1 = [lambda_0*Enere; x1; Nuller];103 x2 = [Nuller; x2; Nuller];104 x3 = [Nuller; x3; Nuller];105 x4 = [Nuller; x4; Nuller];106107 % figur108 t = (0:delta_t:delta_t*(length(u)−1))'; % Virkelig tid109110 tstop = N*delta_t + 2*sek_forst;111 u_sim = [t,u];112 x_sim = [t,x1,x2,x3,x4];113114 %%%%%%%%%%%%%%%%%115 %sim heli_3_2116 %%%%%%%%%%%%%%%%%117118119 R2D = 180/pi;120 u = u*R2D;121 x1 = x1*R2D;122 x2 = x2*R2D;123 x3 = x3*R2D;124 x4 = x4*R2D;125126 %%127128 %%%%%%%%%%%%%%%%%129 load heli_3_2130 %%%%%%%%%%%%%%%%%131 figure(1)132 subplot(5,1,1)133 stairs(t,u)134 legend('Optimal control value $u^* = p_c^*$')135 ylabel('Degrees [deg]')136 fixplot;137138 subplot(5,1,2)139 plot( t,x1,'−mo', rt_lambda(:,1),rt_lambda(:,2));140 legend('Optimal travel $\lambda^*$','Real travel $\lambda$')141 ylabel('Degrees [deg]')142 fixplot;

Page 26 of 35


143 ylim([0 200]);144145 subplot(5,1,3)146 plot(t,x2','−mo', rt_r(:,1),rt_r(:,2));147 legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthEast')148 ylabel('Degrees/sec [deg/s]')149 fixplot;150151 subplot(5,1,4)152 plot(t,x3,'−mo', rt_p(:,1),rt_p(:,2));153 legend('Optimal pitch $p^*$','Real pitch $p$')154 ylabel('Degrees [deg]')155 fixplot;156157 subplot(5,1,5)158 plot(t,x4','−mo', rt_p_dot(:,1),rt_p_dot(:,2));159 legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$')160 xlabel('Time [s]')161 ylabel('Degrees/sec [deg/s]')162 fixplot;163164 set(gca,'XTickLabelMode','Auto');165166167168 set(gcf,'PaperType', 'A4')169 set(gcf,'PaperOrientation', 'portrait')170 papersize=get(gcf,'PaperSize');171 set(gcf,'PaperPositionMode','manual')172 set(gcf,'PaperPosition',[0.25 0.25 papersize(1)−0.5 papersize(2)−0.5])173174 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%175 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%176 createPdf('10_3_2_feedback')177 grid on178 set(gcf,'Position',[100,00 700,700])

Page 27 of 35


2.4 Task 4.3

1 % oppg10_4_3.m23 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%4 %%%%%%%%%%%%%%%%%%%%%%% 10.4.3 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%5 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%678 init;9 delta_t = 0.25; % tastetid

10 sek_forst = 5;1112 global N M G mu mx beta alpha lambda_f q1 q21314 alpha = 0.2;15 beta = 20;1617 lambda_f = 2*pi/3;181920 % Setter opp modellen for systemet. x = [lambda r p p_dot]'21 A1 = zeros(6,6);22 A1(1,1) = 1;23 A1(1,2) = delta_t;24 A1(2,2) = 1;25 A1(2,3) = −delta_t*K_2;26 A1(3,3) = 1;27 A1(3,4) = delta_t;28 A1(4,3) = −delta_t*K_1*K_pp;29 A1(4,4) = 1−delta_t*K_1*K_pd;%%%%%%%%%%%%%%%%%%%%%%%%%FEIL med minus 1 forst?????????30 A1(5,5) = 1;31 A1(5,6) = delta_t;32 A1(6,5) = −delta_t*K_3*K_ep;33 A1(6,6) = 1−delta_t*K_3*K_ed;3435 B1 = zeros(6,2);36 B1(4,1) = delta_t*K_1*K_pp;37 B1(6,2) = delta_t*K_3*K_ep;3839 % Antall pådrag og tilstander40 mx = size(A1,1); % Antall tilstander (antall rader i A)41 mu = size(B1,2); % Antall paadrag (antall kolonner i B)4243 % Initialverdier44 lambda_0 = pi;45 x0 = zeros(mx,1);46 x0(1,1) = lambda_0; % Initialverdier4748 % Tidshorisont og initialisering49 N = 80; % Tidshorisont for tilstander50 M = N; % Tidshorisont for pådrag51 z = zeros(N*mx+M*mu,1); % Initialiserer z for hele horisonten52 z0 = z; % Startverdi for optimaliseringen5354 % N=80 tar 3.17 min5556 % Cost function57 I_n = eye(N);58 Qk = diag([1 0 0 0 0 0]);59 Q = kron(I_n, Qk);60 q1 = 1;61 q2 = 1;62 Rk = diag([q1, q2]);63 R = kron(I_n, Rk);64 G = blkdiag(Q, R);656667 % Equality constraint68 I_n = eye(N);69 Aeq_c1 = eye(N*mx); % Component 1 of A

Page 28 of 35


70 Aeq_c2 = kron(diag(ones(N−1,1),−1), −A1); % Component 2 of A71 Aeq_c3 = kron(I_n, −B1); % Component 3 of A72 Aeq = [Aeq_c1 + Aeq_c2, Aeq_c3];73 beq = [A1*x0; zeros((N−1)*mx,1)];747576 p_limit = 30*pi/180; % degrees77 xf = zeros(mx,1);78 xf(1,1) = 0; %lambda_f; % NBNBNBNBNB7980 % Inequality constraints81 ul = −Inf(N*mu,1);82 uu = Inf(N*mu,1);83 ul(1:mu:N*mu) = −p_limit; % −30 deg < pitch−pådrag < 30 deg84 uu(1:mu:N*mu) = p_limit;85 ul(N*mu) = 0; % Siste pådrag skal være 086 uu(N*mu) = 0;87 ul(N*mu−1) = 0; % Siste pådrag skal være 088 uu(N*mu−1) = 0;8990 xl = −Inf(N*mx,1); % Begrensning for tilstandene (ingen begrensning)91 xu = Inf(N*mx,1);92 xl(3:mx:N*mx) = −p_limit; % −30 deg < pitch x3 < 30 deg93 xu(3:mx:N*mx) = p_limit;9495 zl = [xl;ul];96 zu = [xu;uu];979899 % Solving with fmincon

100 opt = optimset('Display','notify', 'Diagnostics','off', 'LargeScale','off');101 tic102 [z,fval] = fmincon(@objective,z0,[],[],Aeq,beq,zl,zu,@constraint);103 %[z,fval,exitflag,output,lambda] = quadprog(G,[],[],[],Aeq,beq,zl,zu,z0,opt);104 t1 = toc;105106 Qlq = diag([5 0 0.1 0 2 0]);107 Rlq = diag([1 1]);108 [K,~,~] = dlqr(A1,B1,Qlq,Rlq);109110111 % Beregnede paadrag og tilstander112 u1 = [z(N*mx+1:mu:N*mx+M*mu);z(N*mx+M*mu−1)]; % Beregnet paadrag u1113 u2 = [z(N*mx+2:mu:N*mx+M*mu);z(N*mx+M*mu)]; % Beregnet paadrag u2114 x1 = [x0(1);z(1:mx:N*mx)]; % Beregnet tilstand x1115 x2 = [x0(2);z(2:mx:N*mx)]; % Beregnet tilstand x2116 x3 = [x0(3);z(3:mx:N*mx)]; % Beregnet tilstand x3117 x4 = [x0(4);z(4:mx:N*mx)]; % Beregnet tilstand x4118 x5 = [x0(5);z(5:mx:N*mx)]; % Beregnet tilstand x5119 x6 = [x0(6);z(6:mx:N*mx)]; % Beregnet tilstand x6120121 Antall = sek_forst/delta_t;122 Nuller = zeros(Antall,1);123 Enere = ones(Antall,1);124125126 u1 = [Nuller; u1; Nuller];127 u2 = [Nuller; u2; Nuller];128 x1 = [lambda_0*Enere; x1; 0*Enere];129 x2 = [Nuller; x2; Nuller];130 x3 = [Nuller; x3; Nuller];131 x4 = [Nuller; x4; Nuller];132 x5 = [Nuller; x5; Nuller];133 x6 = [Nuller; x6; Nuller];134135136 % figur137 t = (0:delta_t:delta_t*(length(u1)−1))'; % Virkelig tid138139 tstop = N*delta_t + 2*sek_forst;140 u_sim = [t,u1,u2];141 x_sim = [t,x1,x2,x3,x4,x5,x6];142

Page 29 of 35


143144 R2D = 180/pi;145 u1 = u1*R2D;146 u2 = u2*R2D;147 x1 = x1*R2D;148 x2 = x2*R2D;149 x3 = x3*R2D;150 x4 = x4*R2D;151 x5 = x5*R2D;152 x6 = x6*R2D;153154155 %%156157 feedback = 1;158159 if feedback==1160 %%%%%%%%%%%%%%%%%161 load heli_4_3_N80162 %%%%%%%%%%%%%%%%%163 else164 %%%%%%%%%%%%%%%%%165 load heli_4_3_no_feedback_new166 %%%%%%%%%%%%%%%%%167 end168169170 figure(1)171 subplot(8,1,1)172 stairs(t,u1)173 legend('Optimal pitch control $u_1^* = p_c^*$','location','SouthWest')174 ylabel('Degrees')175 fixplot;176177 subplot(8,1,2)178 stairs(t,u2)179 legend('Optimal elevation control $u_2^* = e_c^*$','location','SouthWest')180 ylabel('Degrees')181 fixplot;182183 subplot(8,1,3)184 plot( t,x1,'−mo', rt_lambda(:,1),rt_lambda(:,2));185 legend('Optimal travel $\lambda^*$','Real travel $\lambda$','location','SouthWest')186 ylabel('Degrees')187 ylim([−200,200]);188 fixplot;189190 subplot(8,1,4)191 plot(t,x2','−mo', rt_r(:,1),rt_r(:,2));192 legend('Optimal travel rate $r^*$', 'Real travel rate $r$','location','SouthWest')193 ylabel('Degrees/sec')194 ylim([−50,20]);195 fixplot;196197 subplot(8,1,5)198 plot(t,x3,'−mo', rt_p(:,1),rt_p(:,2));199 legend('Optimal pitch $p^*$','Real pitch $p$','location','SouthWest')200 ylabel('Degrees')201 ylim([−60,50]);202 fixplot;203204 subplot(8,1,6)205 plot(t,x4','−mo', rt_p_dot(:,1),rt_p_dot(:,2));206 legend('Optimal pitch rate $\dot{p}^*$','Real pitch rate $\dot{p}$','location','SouthWest')207 ylabel('Degrees/sec')208 ylim([−200,200]);209 fixplot;210211 subplot(8,1,7)212 plot(t,x5','−mo', rt_e(:,1),rt_e(:,2));213 legend('Optimal elevation $e^*$','Real elevation $e$','location','NorthWest')214 ylabel('Degrees')215 ylim([−5,15]);

Page 30 of 35


216 fixplot;217218 subplot(8,1,8)219 plot(rt_e_dot(:,1),rt_e_dot(:,2), t,x6','−mo');220 legend('Real elevation rate $\dot{e}$', 'Optimal elevation rate ...

$\dot{e}^*$','location','SouthWest')221 xlabel('Time [s]')222 ylabel('Degrees/sec')223 ylim([−20,20]);224 fixplot;225226 set(gca,'XTickLabelMode','Auto');227228 set(gcf,'PaperType', 'A4')229 set(gcf,'PaperOrientation', 'portrait')230 papersize=get(gcf,'PaperSize');231 set(gcf,'PaperPositionMode','manual')232 set(gcf,'PaperPosition',[0.25 0.25 papersize(1)−0.5 papersize(2)−0.5])233234 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%235 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%236237 if feedback==1238 createPdf('10_4_3_feedback_N80')239 else240 createPdf('10_4_3_no_feedback')241 end242 grid on243 set(gcf,'Position',[100,00 700,700])

2.4.1 Objective function

1 % objective.m23 function out = objective(z)45 global G67 out = z' * G * z;89 return

2.4.2 Nonlinear constraints

1 % constraint.m23 function [c,ceq] = constraint(z)4 % c(z) <= 0 elevation constraint5 % ceq(z) = 0 not used6 global N mx mu beta alpha lambda_f7 c_tmp = −Inf(N,1);89 lambda_k = z(1:mx:N*mx); % Beregnet tilstand x1

10 ek = z(5:mx:N*mx); % Beregnet tilstand x5 = elevasjon1112 for n = 1:N13 c_tmp(n) = alpha*exp(−beta*(lambda_k(n)−lambda_f)^2) − ek(n);14 end1516 ceq = [];17 c = c_tmp;18 end

Page 31 of 35


2.5 Accompanying scripts

2.5.1 Initialization file for helicopter 3

1 % init.m23 %Denne fila inneholder initialisering for prosjektoppgave 2 i SIE3030. Den skal4 %brukes bare for helikopteret på forsøkshallen.56 clear all;7 clc;8 %%%%%%%%%%% Kalibrering av encoder og hw−oppsett for aktuelt helikopter9 KalibVandring = −.0879;

10 KalibPitch = .0879;11 KalibElevasjon = .0879*1.5;12 EncoderInputVandring = 0;13 EncoderInputPitch = 1;14 EncoderInputElevasjon = 2;15 joystick_gain_x = 1;16 joystick_gain_y = 1;1718 %%%%%%%%%%% Faste fysiske verdier gitt initielt19 m_w = 1.799; % motvekt20 m_h = 1.34; % helikopter21 l_a = 0.655; % lengde fra pivotpunkt til helikopterkropp22 l_h = 0.177; % lengde fra midten på helikopterkroppen til motor23 J_e = 2 * m_h * l_a *l_a; % treghetsmoment om elevasjonsaksen24 J_p = 2 * ( m_h/2 * l_h * l_h); % treghetsmoment om pitchaksen25 J_t = 2 * m_h * l_a *l_a; %treghetsmoment om vandringsaksen26 m_g=0.028; % Effektiv masse (med motvekt)27 K_p = m_g*9.81; % Kraften som trengs for å holde helikopteret i likevekt.28 V_f_eq = 1.2; %Bytt verdi slik at det stemmer med det aktuelle helikopteret29 V_b_eq = .85;%1.2; %Bytt verdi slik at det stemmer med det aktuelle helikopteret30 V_s_eq = V_f_eq+V_b_eq; % Minimum spenning for å holde helikopteret i likevekt31 K_f = K_p/V_s_eq; % Kraftkonstant (N/V)32 %K_ep = 15.1243;33 %K_ed = 9.3779;34 %K_ei = 1.4216;3536 % K_ep = 15;37 % K_ed = 11.5;38 % K_ei = 2.3;39 K_ep = 6.3;5.5;40 K_ed = 10;41 K_ei = 2.1;4243 K_1 = l_h*K_f/J_p;44 K_2 = K_p*l_a/J_t;45 K_3 = K_f*l_a/J_e;46 K_4 = K_p*l_a/J_e;4748 w_c = 5;49 K_pd = w_c/K_1;50 K_pp = (sqrt(2)*w_c^2)/K_1;

Page 32 of 35


2.5.2 Enable LATEX interpretation of plot legends

1 % fixplot.m23 grid on4 h=legend;5 set(h, 'interpreter', 'latex');67 set(gca,'xticklabel',[]);89 p = get(gca,'position');

10 p(4) = p(4)*1.25;11 set(gca, 'position', p);

2.5.3 Automate creation of PDF plots

1 % createPdf.m23 function [] = createPdf(filename, enable)45 try6 grid on7 h=legend;8 set(h, 'interpreter', 'latex');9

10 if nargin == 111 enable = 1;12 end13 if enable > 014 if isdir('pdf') == 015 mkdir('pdf');16 end17 print('−painters','−dpdf','−r900', ['pdf//', filename] );18 end19 catch err20 warning(err.message);21 end

Page 33 of 35


C Simulink Diagrams

3.1 Simulink diagram for Task 2.4

u

r

p_dot

plambda_0

-C-

lambda

elevation

-C-

e

V_d/V_s --> V_f/V_b

V_d

V_s

V_f

V_b

Terminator1

Terminator

Pitch-kontroller

p_c, rad

p, rad

p_dot, rad/sek

Out2

Heli 3D

V_f, motor foran

V_b, motor bak

Vandring, (grader)

Vandringsrate, (grader/sek)

Pitch, (grader)

Pitchrate, (grader/sek)

Elevasjon, (grader)

Elevasjonsrate (grader/sek)

HIL Initialize??? (???-???)

Bad Link

Gain

K*u

FromWorkspace

u_sim

Elevasjonskontroller

e, rad

e_dot, rad/sek

e_c, rad

V_s

Constant

-30

Add

Figure C.1: Simulink diagram for Task 2.4.


u r

p_dot

plambda_0

-C-

lambda

elevation

-C-

e

V_d/V_s --> V_f/V_b

V_d

V_s

V_f

V_b

Pitch-kontroller

p_c, rad

p, rad

p_dot, rad/sek

Out2

Mult

MatrixMultiply

K

K

Heli 3D

V_f, motor foran

V_b, motor bak

Vandring, (grader)


Pitch, (grader)


Elevasjon, (grader)



Bad Link

Gain

K*u

x_sim

u_sim


e, rad

e_dot, rad/sek

e_c, rad

V_s

Constant

-30


Page 34 of 35



u

r

p_dot

p

lambda_0

-C-

lambda

e_dote

V_d/V_s --> V_f/V_b

V_d

V_s

V_f

V_b

Pitch-kontroller

p_c, rad

p, rad

p_dot, rad/sek

Out2

Mult

MatrixMultiplyK

K

Heli 3D

V_f, motor foran

V_b, motor bak

Vandring, (grader)


Pitch, (grader)


Elevasjon, (grader)



Bad Link

Gain

K*u

x_sim

u_sim


e, rad

e_dot, rad/sek

e_c, rad

V_s

Constant

-30


Page 35 of 35

Documents

Helicopter Lab (Optimization and Control)