Helicopter Lab Day 4

Optimal Control of Pitch/Travel and Elevation with Feedback:

In this task we include the dynamics of elevation, $e$. The helicopter is moved from $x_0$ to $x_f$ past a restriction causing the elevation angle to change during the flight. We must therefore add the elevation to the state vector; the setpoint $e_c$ is a new manipulated variable in the system.

Preparations

1)

Task

Write the system on continuous state space form with the two extra states $e$ and $\dot{e}$. Use $x={\left[\begin{array}{cccccc}\lambda & r& p& \dot{p}& e& \dot{e}\end{array}\right]}^T$ and $u={\left[\begin{array}{cc}{p}_c& e_c\end{array}\right]}^T$.

We remember from Helicopter Lab Day 2

Transclude of Helicopter-Lab-Day-2#1-state-space-formulation

Extending this according to the task give us

$$\left[\begin{array}{c}\dot{\lambda }\\ \dot{r}\\ \dot{p}\\ \ddot{p}\\ \dot{e}\\ \ddot{e}\end{array}\right]=\underset{A_c}{\underbrace{\left[\begin{array}{cccccc}0& 1& 0& 0& 0& 0\\ 0& 0& -K_2& 0& 0& 0\\ 0& 0& 0& 1& 0& 0\\ 0& 0& -K_1{K}_{pp}& -K_1{K}_{pd}& 0& 0\\ 0& 0& 0& 0& 0& 1\\ 0& 0& 0& 0& -K_3{K}_{ep}& -K_3{K}_{ed}\end{array}\right]}}\left[\begin{array}{c}\lambda \\ r\\ p\\ \dot{p}\\ e\\ \dot{e}\end{array}\right]+\underset{B_c}{\underbrace{\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ K_1{K}_{pp}& 0\\ 0& 0\\ 0& K_3{K}_{ep}\end{array}\right]}}$$

Where the relevant formulas can be found here page=13

2)

Task

Discretize the model using the forward Euler method (Euler Discretization) and write the resulting model on discrete state space form.

We know that the discretized system is given by

Link to original

Where $T$ is the discretization time. This gives us

$$x[l+1]=\underset{A}{\underbrace{\left[\begin{array}{cccccc}1& T& 0& 0& 0& 0\\ 0& 1& -T{K}_2& 0& 0& 0\\ 0& 0& 1& T& 0& 0\\ 0& 0& -T{K}_1{K}_{pp}& 1-T{K}_1{K}_{pd}& 0& 0\\ 0& 0& 0& 0& 1& T\\ 0& 0& 0& 0& -T{K}_3{K}_{ep}& 1-T{K}_3{K}_{ed}\end{array}\right]}}x[k]+\underset{B}{\underbrace{\left[\begin{array}{cc}0& 0\\ 0& 0\\ 0& 0\\ T{K}_1{K}_{pp}& 0\\ 0& 0\\ 0& T{K}_3{K}_{ep}\end{array}\right]}}$$

Where $x$ and $u$ are unchanged from 1)

3)

Task

Add an inequality constraint on the elevation $e_k\ge \alpha \exp (-\beta ({\lambda }_k-{\lambda }_t{)}^2)\forall k\in \{1,\dots ,N\}$

nonlinear constraint so we must use theMatlab function fmincon, as this is an SQP type algorithm

Note that this is a

We now want to minimize $\varphi ={\sum }_{i=0}^{N-1}({\lambda }_{i+1}-{\lambda }_f{)}^2+q_1{p}_{ci}^2+q_2{e}_{ci}^2$

We need to solve this similarly as we did in 3) Calculating the Optimal Path

We want to express the cost function, $\varphi$ as

$$f(z)=\sum _{t=0}^{N-1}\frac{1}{2}{x}_{t+1}^{T}Q{x}_{t+1}+\frac{1}{2}{u}_t^{T}R{u}_t$$

Finding $Q$: We therefore get

$$\begin{array}{rl}({\lambda }_{i+1}-{\lambda }_f{)}^2& =\frac{1}{2}{x}_{t+1}^{T}Q{x}_{t+1}\\ ({\lambda }_{i+1}-0{)}^2& =\frac{1}{2}\left[\begin{array}{cccccc}{\lambda }_{i+1}& r_{i+1}& p_{i+1}& {\dot{p}}_{i+1}& e_{i+1}& {\dot{e}}_{i+1}\end{array}\right]Q\left[\begin{array}{c}{\lambda }_{i+1}\\ r_{i+1}\\ p_{i+1}\\ {\dot{p}}_{i+1}\\ e_{i+1}\\ {\dot{e}}_{i+1}\end{array}\right]\end{array}$$

Giving us

$$Q=\left[\begin{array}{cccccc}2& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0\end{array}\right]$$

Same reasoning for finding $R$:

$$\begin{array}{rl}\frac{1}{2}{u}_i^{T}R{u}_i& =q_1{p}_{ci}^2+q_2{e}_{ci}^2\end{array}$$

Which gives us

$$R=2\left[\begin{array}{cc}{q}_1& 0\\ 0& q_2\end{array}\right]$$

See the Matlab file init08.m for implementation.

On The Lab

6)

Yes

7)

8)

TEST PLAN

Testing different horizons:

1. $N=40$
2. $N=60$ BEST Testing different objective function weights q1 and q2.
3. q1=q2= 1
4. q1=q2=0.1 BEST
5. q1=q2=10

^Everything above is with

$$\begin{array}{rl}Q& =diag([1,1,1,1,1,1])\\ R& =diag([1,1])\end{array}$$

Testing with different LQR:

1. good: Q = [3 0.3 2 0.5 3 0.5], R = [0.1 0.1] BEST
2. unit: Q = [1 1 1 1 1 1], R = [1 1]

Row vs data

1. Time
2. Travel
3. Travel rate
4. Pitch
5. Pitch rate
6. Elevation
7. Elevation rate
8. Pitch reference
9. Elevation reference
10. V_d
11. V_s 13-17. x_star (row 13-17)
12. Constraint