# OpEn Rust Examples: Obstacle Avoidance Constraint

In this example, we are going to test a little bit more complicated constraint that was used for **nonlinear-shaped obstacle avoidance** in robotics systems in the following papers [1][2].

- [1] [Small et al., 2018, "Aerial navigation in obstructed environments with embedded nonlinear model predictive control"](https://arxiv.org/abs/1812.04755)

- [2] [Sathya et al., 2019, "Embedded nonlinear model predictive control for obstacle avoidance using PANOC"](https://arxiv.org/abs/1904.10546)

Now, I am going to introduce the key idea in terms of how to mathematically formulate the obstacle avoidance constraint, then we are going to implement the constraint with a very simple robot model.

## Obstacle Avoidance Constraint: A Breif Description

### Basic form
Let's say there are multiple obstacles around a robot. We can consider the $j$-th obstacle as the intersection of a set of $m_j(t)$ nonlinear inequalities:

$$ O_j(t) = \{ p \in \mathbb{R}^{n_d} : h_j^i(p,t) > 0, \forall i \in \mathbb{N}_{[1,m_j(t)]} \} $$

 [1, Eqn (11)]

where
- $n_d$: the number of space dimensions (normally 2 or 3)
- $h^i_j$: the $i$-th nonlinear function that is formed of the $j$-th obstacle shape
- $\mathbb{N}_{[1,m_j(t)]} = \{1,2,...,m_j(t)\}$: a natural number set from 1 to $m_j(t)$, which is the number of nonlinear functions for the obstacle.

Let's say $x(t)$ is the position of a mobile robot at time $t$. In order for the robot to avoid the obstacle, we should make sure the following constraint:

$$ x(t) \notin O_j(t) $$

 [1, Eqn (9e)]

### Another form

Since the above constraint is complicated to be considered in an optimisation problem setting, now we are going to reformulate it.

#### Reformulation

The constraint above is satisfied if and only if

$$ h^{i_0}_j( x(t) ,t) \le 0, \text{  for some } i_0 \in \mathbb{N}_{[1,m_j(t)]} $$

 [2, Eqn (8)]

or equvalently

$$ [h^{i_0}_j(x(t),t)]^2_{+} := \max \{ 0, h^{i_0}_j(x(t),t) \}^2 = 0. $$

This constraint can be encoded as

$$ \psi_{O_j(t)} (x(t)) = 0$$

 [1, Eqn (12)]

where

$$ \psi_{O_j(t)} (p) := \frac{1}{2} \prod_{i=1}^{m_j(t)} [h^{i}_j(p,t)]^2_{+}.$$

 [1, Eqn (13)]

$\psi_{O_j(t)} (p)$ becomes $0$ if $p$ is outside $O_j(t)$; and increases in the interior of it as $p$ moves away from $O_j(t)$'s boundary.

#### Gradient

Function $\psi_{O_j(t)} (p)$ is **differentiable** inside $O_j(t)$ with gradient

  \begin{equation}
    \nabla \psi_{O_j(t)} (p) =
    \begin{cases}
      \sum_{i=1}^{m_j(t)} \{  \nabla h_j^i(p,t)  \cdot h^i_j(p,t) \cdot \prod_{k \neq i} ( h^{k}_j(p,t) ) ^2 \} , & \text{if}\ p \in O_j(t) \\
      0, & \text{otherwise}
    \end{cases}
  \end{equation}

 [2, Eqn (10)]

#### In a nutshell

As long as we can define an obstacle shape by using $h^i(p,t)$ functions, then we can formulate the corresponding obstacle avoidance constraint by using [1, Eqn (12)] and [2, Eqn (10)].

### Examples of Obstacle Avoidance Constraints

#### (1) Ellipsoids

An ellipsoid can be modelled by

$$ O = \{ p \in \mathbb{R}^{n_d} : h(p) > 0 \} $$

and it has a single $h$ function

$$ h(p) = 1 - (p - c)^{\top} E (p - c) $$

where

- $c \in \mathbb{R}^{n_d}$: the centre of the ellipsoid
- $E$: a diagonal matrix in $\mathbb{R}^{n_d \times n_d}$ that controls the ellipsoid's size

Then, its reformulated constraint (i.e. [1, Eqn (12)]) becomes

$$ \psi_{O}(p) = \frac{1}{2}[ 1 - (p - c)^{\top} E (p - c)]^2_{+} = 0$$

## Rust Implementation

Now, we are going to implement optimisation problems with the constraints above, by using OpEn rust. Here, we use ALM/PM method (for details, see example [03](http://localhost:8888/notebooks/Jupyter/OpEn_Rust_example_ALMPM.ipynb) [04](https://github.com/inmo-jang/optimisation_tutorial/blob/master/tools_examples/OpEn/examples_rust/OpEn_Rust_example_04.ipynb) [05](https://github.com/inmo-jang/optimisation_tutorial/blob/master/tools_examples/OpEn/examples_rust/OpEn_Rust_example_05.ipynb)).

#### OpEn Implementation

First, we need to import "optimization_engine"
- In your local PC, it should be also declared in "Cargo.toml".
- Instead, in this jupyter notebook, we need to have "extern crate" as follows.

```Rust
extern crate optimization_engine;
use optimization_engine::{
    alm::*,
    constraints::*, panoc::*, *
};
```

### (1) A simple minimisation problem with an ellipsoid obstacle constraint

The optimisation problem that we are going to solve is:

Minimise $$f(\mathbf{u},\mathbf{p}) = p_1 \cdot (u_1 - p_2)^2 + p_3$$

subject to

$$ \psi_{O}(\mathbf{u}) = \frac{1}{2}[ 1 - (\mathbf{u} - \mathbf{c})^{\top}(\mathbf{u} - \mathbf{c})]^2_{+} = 0$$

where
- $\mathbf{p} = [p_1, p_2, p_3]^{\top}$: a parameter set for the objective function
- $\mathbf{u} = [u_1, f(\mathbf{u},\mathbf{p})]^{\top}$: $u_1$ is the actual decision variable; $f(\mathbf{u},\mathbf{p})$ is the objective function
- $\mathbf{c} = [c_1, c_2]^{\top}$: the centre for the ellipsoid constraint

For simplicity, the above can be rewritten as

Minimise $$f(\mathbf{u}) = u_2$$

 (P1)

subject to

$$ \psi_{O}(\mathbf{x}) =  [1 - (\mathbf{u} - \mathbf{c})^{\top}(\mathbf{u} - \mathbf{c})]_{+} = 0$$

 (P1C)

$$ u_2 = p_1 \cdot (u_1 - p_2)^2 + p_3$$

 (P2C)

Then, we need to define the objective function that we are going to optimise, and its gradient function.

```Rust
pub fn f(u: &[f64], cost: &mut f64) -> Result<(), SolverError> {
    *cost = u[1];
    Ok(())
}
```

Its gradient is

$$ \nabla f(u) = [0,1] $$

```Rust
pub fn df(u: &[f64], grad: &mut [f64]) -> Result<(), SolverError> {
    grad[0] = 0.0;
    grad[1] = 1.0;
    Ok(())
}
```

The two constraints (P1C and P2C) are rewritten as

$$F_1(u) =
\begin{bmatrix}
 \max \{ 0, 1- (u_1 - c_1)^2 - (u_2 - c_2)^2 \}  \\
p_1 \cdot (u_1 - p_2)^2 + p_3 - u_2
\end{bmatrix} = \{0\}$$

```Rust
pub fn _f1(u: &[f64], p: &[f64], centre: &[f64], f1u: &mut [f64]) {
    f1u[0] = (1.0 - (u[0]-centre[0]).powi(2) - (u[1]-centre[1]).powi(2) ).max(0.0);
    f1u[1] = p[0]*(u[0] - p[1]).powi(2) + p[2] - u[1];
}
```

We need to define its Jacobian product $JF_1^{\top} \cdot d$, as follows:

$$JF_1 = \begin{bmatrix}
\frac{\partial F_{1,1}}{\partial u_1} & \frac{\partial F_{1,1}}{\partial u_2} \\
\frac{\partial F_{1,2}}{\partial u_1} & \frac{\partial F_{1,2}}{\partial u_2}
\end{bmatrix}
=
 \begin{bmatrix}
-(u_1 - c_1) & -(u_2 - c_2)  \\
2 p_1 (u_1 - p_2) & -1
\end{bmatrix}
$$

Hence,
$$JF_1^{\top} \cdot d = \begin{bmatrix}
-(u_1 - c_1)d_1 + 2 p_1 (u_1 - p_2) d_2  \\
-(u_2 - c_2)d_1 - d_2
\end{bmatrix}$$

```Rust
pub fn _f1_jacobian_product(u: &[f64], d: &[f64], p: &[f64], centre: &[f64], res: &mut [f64]) {

    let test = (1.0 - (u[0]-centre[0]).powi(2) - (u[1]-centre[1])).powi(2);
    let f11u1 = 0.0;
    let f11u2 = 0.0;
    if  test > 0.0 { // Inside the obstacle
        let f11u1 = -(u[0] - centre[0]);
        let f11u2 = -(u[1] - centre[1]);
    }

    res[0] = f11u1*d[0] + 2.0*p[0]*(u[0]-p[1])*d[1];
    res[1] = f11u2*d[0] - d[1];
}
```

Now, let's solve this problem:

```Rust
fn main(p: &[f64], centre: &[f64]) {
    let tolerance = 1e-5;
    let nx = 2; // problem_size: dimension of the decision variables
    let n1 = 2; // range dimensions of mappings F1
    let n2 = 0; // range dimensions of mappings F2
    let lbfgs_mem = 5; // memory of the LBFGS buffer

    // PANOCCache: All the information needed at every step of the algorithm
    let panoc_cache = PANOCCache::new(nx, tolerance, lbfgs_mem);

    // AlmCache: A cache structure that contains all the data
    // that make up the state of the ALM/PM algorithm
    // (i.e., all those data that the algorithm updates)
    let mut alm_cache = AlmCache::new(panoc_cache, n1, n2);

    let set_c = Zero::new(); // Set C
    let bounds = Ball2::new(None, 100.0); // Set U
    let set_y = Ball2::new(None, 1e12);  // Set Y

    // =============
    // Re-define the functions linked to user parameters
    let f1 = |u: &[f64], f1u: &mut [f64]| -> Result<(), SolverError> {
        _f1(u, &p, &centre, f1u);
        Ok(())
    };

    let f1_jacobian_product = |u: &[f64], d: &[f64], res: &mut [f64]| -> Result<(), SolverError> {
        _f1_jacobian_product(u, d, &p, &centre, res);
        Ok(())
    };
    // ==============

    // AlmFactory: Prepare function psi and its gradient
    // given the problem data such as f, del_f and
    // optionally F_1, JF_1, C, F_2
    let factory = AlmFactory::new(
        f, // Cost function
        df, // Cost Gradient
        Some(f1), // MappingF1
        Some(f1_jacobian_product), // Jacobian Mapping F1 Trans
        NO_MAPPING, // MappingF2
        NO_JACOBIAN_MAPPING, // Jacobian Mapping F2 Trans
        Some(set_c), // Constraint set
        n2,
    );

    // Define an optimisation problem
    // to be solved with AlmOptimizer
    let alm_problem = AlmProblem::new(
        bounds,
        Some(set_c),
        Some(set_y),
        |u: &[f64], xi: &[f64], cost: &mut f64| -> Result<(), SolverError> {
            factory.psi(u, xi, cost)
        },
        |u: &[f64], xi: &[f64], grad: &mut [f64]| -> Result<(), SolverError> {
            factory.d_psi(u, xi, grad)
        },
        Some(f1),
        NO_MAPPING,
        n1,
        n2,
    );

    let mut alm_optimizer = AlmOptimizer::new(&mut alm_cache, alm_problem)
        .with_delta_tolerance(1e-5)
        .with_max_outer_iterations(200)
        .with_epsilon_tolerance(1e-6)
        .with_initial_inner_tolerance(1e-2)
        .with_inner_tolerance_update_factor(0.5)
        .with_initial_penalty(100.0)
        .with_penalty_update_factor(1.05)
        .with_sufficient_decrease_coefficient(0.2)
        .with_initial_lagrange_multipliers(&vec![5.0; n1]);

    let mut u = vec![0.0; nx]; // Initial guess
    let solver_result = alm_optimizer.solve(&mut u);
    let r = solver_result.unwrap();
    println!("\n\nSolver result : {:#.7?}\n", r);
    println!("Solution u = {:#.6?}", u);
}
```

#### Result

##### Case (1) : $\mathbf{p} = [0.5, 0.0, 0.0]$; and $\mathbf{c} = [0.0, 0.0]$

```Rust
main(&[0.5, 0.0, 0.0], &[0.0, 0.0]);
```

    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 294357,
        last_problem_norm_fpr: 0.0000002,
        lagrange_multipliers: Some(
            [
                418903.3411792,
                -0.1598174,
            ],
        ),
        solve_time: 1.2080253s,
        penalty: 1422664.9032171,
        delta_y_norm: 11169.1870569,
        f2_norm: 0.0000000,
    }

    Solution u = [
        -0.906969,
        0.411296,
    ]

##### Discussion regarding `with_max_outer_iterations`
- The above result shows that $\sqrt{(-0.906969)^2 + (0.411296)^2} = 0.9957$, which should have been $1$. This violation of the constraint seems to be because of `num_outer_iterations`. See the result saying that `exit_status: NotConvergedIterations`. If we have `.with_max_outer_iterations(20000)` instead, the result becomes much closer to 1.

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 15841,
    num_inner_iterations: 294598,
    last_problem_norm_fpr: 0.0000003,
    lagrange_multipliers: Some(
        [
            13042371154156472153855234973664144029470618410860165048517000607377595587216413717926002755063360427685147309074955380215484788703290607983858748435202048.0000000,
            -423753861818101173183033768863923337961929353698038541150171630273247468286124610675679897724729598403957822352154925420157726222643676129874240493060096.0000000,
        ],
    ),
    solve_time: 685.3194070ms,
    penalty: 1314173191634037641867321827952707564425574569942640742965913261261991610173351306165449104273636083623511981029636043416492478574566316102250934036316035219456.0000000,
    delta_y_norm: 13049253337190544769480745317718605727474957653716969434776105632101374675741179779251287164398950375716031261056815864055903190176215976986135912459010048.0000000,
    f2_norm: 0.0000000,
}

Solution u = [
    -0.910176,
    0.414210,
]
```

##### Case (2) : $\mathbf{p} = [0.5, 0.0, 0.0]$; and $\mathbf{c} = [-0.25, 0.0]$

```Rust
main(&[0.5, 0.0, 0.0], &[-0.25, 0.0]);
```

    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 135491,
        last_problem_norm_fpr: 0.0000005,
        lagrange_multipliers: Some(
            [
                1408361.2510441,
                -0.0600980,
            ],
        ),
        solve_time: 565.9462160ms,
        penalty: 1290399.0051856,
        delta_y_norm: 66981.4338340,
        f2_norm: 0.0000000,
    }

    Solution u = [
        -1.050587,
        0.551867,
    ]

#### Discussion regarding initial guess $u$

- Actually, the result should have positive $u_1$, which is not the case here. That is because of the initial guess being used. If we have `let mut u = vec![0.01; nx];`, then the result becomes

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 143,
    num_inner_iterations: 124212,
    last_problem_norm_fpr: 0.0000001,
    lagrange_multipliers: Some(
        [
            4945.6339502,
            3.6234874,
        ],
    ),
    solve_time: 265.9254640ms,
    penalty: 83969.8295572,
    delta_y_norm: 0.7404363,
    f2_norm: 0.0000000,
}

Solution u = [
    0.716492,
    0.256681,
]
```

- The result also shows that an appropriate initial guess can reduce the number of required iterations for convergence.

##### Case (3) : $\mathbf{p} = [0.5, -0.5, 0.0]$; and $\mathbf{c} = [0.0, 0.0]$

```Rust
main(&[0.5, -0.5, 0.0], &[0.0, 0.0]);
```

    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 146713,
        last_problem_norm_fpr: 0.0000000,
        lagrange_multipliers: Some(
            [
                24603222.0202653,
                0.9999934,
            ],
        ),
        solve_time: 518.7304490ms,
        penalty: 1646912.4585867,
        delta_y_norm: 1173307.9920330,
        f2_norm: 0.0000000,
    }

    Solution u = [
        -0.501946,
        0.000002,
    ]

#### Discussion

- I used another initial guess `let mut u = vec![-10.0,100.0]`

- Also used more max_iteration `.with_max_outer_iterations(200000)`

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 4712,
    num_inner_iterations: 361,
    last_problem_norm_fpr: 0.0000001,
    lagrange_multipliers: Some(
        [
            5976883622830658845248063464346752524709298600838388224476889604094220605747149755684405781725184.0000000,
            -69101021379080966758879847606054738488281126479954440574070188930121413196685134303076548608000.0000000,
        ],
    ),
    solve_time: 5.1134950ms,
    penalty: 603127763126907906755649330948729811734730445314381263460459221876363493539180583510258581622531031040.0000000,
    delta_y_norm: 5977283061058509043015099253727812474755596146491414834183257557507118217200494501180480675643392.0000000,
    f2_norm: 0.0000000,
}

Solution u = [
    -0.992607,
    0.121331,
]
```

##### Case (4) : $\mathbf{p} = [0.1, 0.0, 0.0]$; and $\mathbf{c} = [0.5, 0.0]$

```Rust
main(&[0.1, 0.0, 0.0], &[0.5, 0.0]);
```

    Solver result : AlmOptimizerStatus {
        exit_status: NotConvergedIterations,
        num_outer_iterations: 200,
        num_inner_iterations: 6262,
        last_problem_norm_fpr: 0.0000003,
        lagrange_multipliers: Some(
            [
                24702201.4098780,
                1.1272551,
            ],
        ),
        solve_time: 26.2890600ms,
        penalty: 1646912.4585867,
        delta_y_norm: 1176366.0421865,
        f2_norm: 0.0000000,
    }

    Solution u = [
        0.000000,
        -0.000000,
    ]

- Re-solve with another initial guess `let mut u = vec![-10.0, 0.0];` along with `with_max_outer_iterations(200000)`

```
Solver result : AlmOptimizerStatus {
    exit_status: Converged,
    num_outer_iterations: 58212,
    num_inner_iterations: 505,
    last_problem_norm_fpr: 0.0000000,
    lagrange_multipliers: Some(
        [
            13404549262479835859589751960528458747089645770673533789043717319399913427339339649894823677542737890532912175920611259535236215980552009743086975979618304.0000000,
            -36087273675454447961635736381068040923436891998998803552880018912595707759007342697397663450575419910571250849302105127966573820973304429961460481785856.0000000,
        ],
    ),
    solve_time: 56.1390930ms,
    penalty: 1379881851215739514205223699612867219579361955336128074649839728497056344247153372644434875675550324928731008007705435712857942748898935359088556740355945398272.0000000,
    delta_y_norm: 13404597838859994525841441037492313287571256763071118491702576132599371299634447257190855189024608715345730234127782723897537779342065412648792381468966912.0000000,
    f2_norm: 0.0000000,
}

Solution u = [
    -0.499683,
    0.024968,
]
```

##### Case (5) : $\mathbf{p} = [-0.5, -0.5864, 0.0]$; and $\mathbf{c} = [-
3.0, 2.0]$

```Rust
main(&[0.5, -0.5864, 0.0], &[-3.0, 2.0]);
```

    Solver result : AlmOptimizerStatus {
        exit_status: Converged,
        num_outer_iterations: 15,
        num_inner_iterations: 17,
        last_problem_norm_fpr: 0.0000001,
        lagrange_multipliers: Some(
            [
                5.0000000,
                0.9999987,
            ],
        ),
        solve_time: 37.4400000µs,
        penalty: 171.0339358,
        delta_y_norm: 0.0000026,
        f2_norm: 0.0000000,
    }

    Solution u = [
        -0.586389,
        0.000000,
    ]

In this case, the circle constraint does not cover the minimum point of the cost function. Thus, the optimal value should be zero, where $u_1 = p_2$.

## Summary

- If your outcome's `exit_status` is not converged, then you should modify `with_max_outer_iterations` or **initial guess**.

## Future work

- In this example, we had to manually define `df` and `f1_jacobian_product`. Later, let's use a general function that can obtain such gradient values.


# Musterlösung zu Übungsblatt 1

 * [Aufgabe 1](#Aufgabe-1)
 * [Aufgabe 2](#Aufgabe-2)
 * [Aufgabe 3](#Aufgabe-3)

```python
%matplotlib inline
import numpy as np
import matplotlib.pyplot as plt
import scipy.stats
plt.style.use('ggplot')
```

---
## Aufgabe 1
Gegeben sei eine parametrische Funktion $y = f(x)$, $y = 1 + a_1x + a_2x^2$ mit Parametern $a_1 = 2.0 ± 0.2$, $a_2 = 1.0 ± 0.1$ und Korrelationskoeffizient $ρ = −0.8$.

---

```python
a1, a1_err = 2.0, 0.2
a2, a2_err = 1.0, 0.1
rho = -0.8
```

---
### 1.1
Geben Sie die Kovarianzmatrix von $a_1$ und $a_2$ an.

---

Die Kovarianzmatrix von $a_1$ und $a_2$ lässt sich wie folgt berechnen:

$$\mathrm{Cov}(a) = \pmatrix{\sigma^2_{a_1} & \rho\sigma_{a_1}\sigma_{a_2} \\
                                               \rho\sigma_{a_1}\sigma_{a_2} & \sigma^2_{a_2}}$$

```python
c12 = rho * a1_err * a2_err
covariance = np.matrix([[a1_err ** 2, c12], [c12, a2_err ** 2]])
covariance
```

    matrix([[ 0.04 , -0.016],
            [-0.016,  0.01 ]])

---
### 1.2
Bestimmen Sie analytisch die Unsicherheit von $y$ als Funktion von $x$:

---

Dazu bestimmen wir zunaechst die Ableitungen von $y$ nach $a_1$ und $a_2$

$$\frac{\partial{}y}{\partial{}a_1} = x \,,\quad \frac{\partial{}y}{\partial{}a_2} = x^2 \,.$$

Daraus können wir die Kovarianz von $y$ nach

$$\sigma^2_y = \mathrm{Cov}(y) = \sum_{ij}\frac{\partial{}y}{\partial{}a_i}\frac{\partial{}y}{\partial{}a_j}\mathrm{Cov}(a)_{ij}$$

bestimmen. Einsetzen der eingangs bestimmten Ableitungen liefert

$$\sigma^2_y = c_{11}x^2 + 2c_{12}x^3 + c_{22}x^4$$

wobei die $c_{ij}$ die Einträge von $\mathrm{Cov}(a)$ sind. Der Ausdruck lässt sich weiter vereinfachen zu

$$\sigma^2_y = x^2\left(\sigma^2_{a_1} + \sigma^2_{a_2}x^2 + 2\rho\sigma_{a_1}\sigma_{a_2}x\right) \,.$$

Daraus ergibt sich für die Unsicherheit

$$ \sigma_y = \lvert{}x\rvert\sqrt{\sigma^2_{a_1} + \sigma^2_{a_2}x^2 + 2\rho\sigma_{a_1}\sigma_{a_2}x} \,.$$

---
#### 1.2.1
unter Vernachlässigung der Korrelation

---

(also für $\rho = 0$) vereinfacht sich der obige Ausdruck zu

$$\sigma_y = \lvert{}x\rvert\sqrt{\sigma^2_{a_1} + \sigma^2_{a_2}x^2} \,.$$

Mit den eingangs berechneten Werten aus `covariance` ergibt sich also

$$\sigma_y = \lvert{}x\rvert\sqrt{0.04 + 0.01x^2} = 0.2\lvert{}x\rvert{}\sqrt{1 + 0.25x^2} \,.$$

```python
def err_ana_wo(x):
    return 0.2 * np.abs(x) * np.sqrt(1 + 0.25 * x ** 2)

xs = np.linspace(-10, 10, 10000)
ys = 1 + a1 * xs + a2 * xs ** 2
errs = err_ana_wo(xs)

plt.plot(xs, ys)
plt.fill_between(xs, ys - errs, ys + errs, alpha=0.5)
plt.show()
```

---
#### 1.2.2
mit Berücksichtigung der Korrelation

---

$$\sigma_y = \lvert{}x\rvert\sqrt{0.04 + 0.01x^2 - 0.016x} = 0.2\lvert{}x\rvert\sqrt{1 + 0.25x^2 - 0.4x} \,.$$

```python
def err_ana(x):
    return 0.2 * np.abs(x) * np.sqrt(1 + 0.25 * x ** 2 - 0.4 * x)

errs = err_ana(xs)

plt.plot(xs, ys)
plt.fill_between(xs, ys - errs, ys + errs, alpha=0.5)
plt.show()
```

---
### 1.3
Bestimmen Sie per Monte Carlo die Unsicherheit von $y$ als Funktion von $x$:

#### 1.3.1
Generieren Sie Wertepaare $(a_1, a_2)$ gemäß ihrer Kovarianzmatrix und visualisieren Sie diese, z.B. mit einem Scatter-Plot.

_Hinweis_: Wenn $x_1$ und $x_2$ zwei gaussverteilte Zufallszahlen mit Mittelwert null und Varianz eins sind, erhält man ein Paar korrelierter gaussverteilter Zufallszahlen $(y_1, y_2)$ mit Mittelwert null und Varianz eins durch $(y_1 = x_1; y_2 = x_1ρ + x_2\sqrt{1 − \rho^2})$.

---

```python
x1s, x2s = np.random.normal(size=(2, 10000))
plt.hist2d(x1s, x2s, bins=40)
plt.title('2-dim Normalverteilung')
plt.xlabel('$x_1$')
plt.ylabel('$x_2$')
plt.show()

y1s = x1s
y2s = x1s * rho + x2s * np.sqrt(1 - rho ** 2)
plt.hist2d(y1s, y2s, bins=40)
plt.title('2-dim Normalverteilung mit Korrelation')
plt.xlabel('$y_1$')
plt.ylabel('$y_2$')
plt.show()

a1s = a1 + y1s * a1_err
a2s = a2 + y2s * a2_err
plt.hist2d(a1s, a2s, bins=40)
plt.title('Verteilung der $a_1$ und $a_2$')
plt.xlabel('$a_1$')
plt.ylabel('$a_2$')
plt.show()
```

---
#### 1.3.2
Bestimmen Sie die Verteilung von $y$ für $x = \{−1, 0, +1\}$ und vergleichen Sie Mittelwert und Varianz (Standardabweichung) mit den Resultaten der analytischen Rechnung.

---

```python
def y(x, a1, a2):
    return 1 + a1 * x + a2 * x ** 2

def var_analytical(x):
    xx = x ** 2
    return 0.04 * xx * (1 + 0.25 * xx - 0.16 * x)

for x in (-1, 0, 1):
    ys = y(x, a1s, a2s)
    mean = np.mean(ys)
    var = np.var(ys)
    print('〈y({})〉= {:.3f}'.format(x, mean))
    print(' σ² = {:.3f}'.format(var))
    print(' analytical = {:.3f}'.format(var_analytical(x)))
    plt.hist(ys, bins=100)
    plt.xlabel('y({})'.format(x))
    plt.ylabel('Absolute Häufigkeit')
    plt.show()
```

Der Fall $x = 0$ ist hier besonders. Da alle Koeffizienten vor Potenzen von $x$ stehen, ergibt sich für den Fall immer $y=0$ unabhängig von den $a_i$. Wir können also keine Aussage über die Varianz treffen.

---
## Aufgabe 2
Betrachten Sie folgende Reparametrisierung von $y = f(x)$:

$$y = 1 + \frac{x(1+x)}{b_1} + \frac{x(1-x)}{b_2}$$

### 2.1
Bestimmen Sie analytisch die transformierten Parameter $b_1$ und $b_2$ und deren Kovarianzmatrix

---

Wir lösen die Reparametrisierung nach Koeffizienten von Potenzen von $x$ auf. Dabei können wir den Term $1$ vernachlässigen, weil er in beiden Definitionen gleichermaßen auftritt.

\begin{align}
 a_1 x + a_2 x^2 &= \frac{x(1 + x)}{b_1} + \frac{x(1 - x)}{b_2} \\
                 &= \frac{x}{b_1} + \frac{x^2}{b_1} + \frac{x}{b_2} - \frac{x^2}{b_2} \\
                 &= x\left(\frac{1}{b_1} + \frac{1}{b_2}\right) + x^2\left(\frac{1}{b_1} - \frac{1}{b_2}\right)
\end{align}

Damit ist

$$a_1 = \left(\frac{1}{b_1} + \frac{1}{b_2}\right) \quad\text{und}\quad a_2 = \left(\frac{1}{b_1} - \frac{1}{b_2}\right)$$

also

$$b_1 = \frac{2}{a_1 + a_2} \quad\text{und}\quad b_2 = \frac{2}{a_1 - a_2} \,.$$

Für die Jacobimatrix der Transformation ergibt sich

\begin{equation}
    M = \pmatrix{
        \frac{-2}{(a_1 + a_2)^2} & \frac{-2}{(a_1 + a_2)^2} \\
        \frac{-2}{(a_1 - a_2)^2} & \frac{+2}{(a_1 - a_2)^2}
    } \quad\text{wobei}\quad m_{ij} = \frac{\partial b_i}{\partial a_j}
\end{equation}

\begin{equation}
    M^T = \pmatrix{
        \frac{-2}{(a_1 + a_2)^2} & \frac{-2}{(a_1 - a_2)^2} \\
        \frac{-2}{(a_1 + a_2)^2} & \frac{+2}{(a_1 - a_2)^2}
    }
\end{equation}

Die transformierte Kovarianzmatrix ist dann $\mathrm{Cov}(b) = M\mathrm{Cov}(a)M^T$.

```python
b1 = 2 / (a1 + a2)
b2 = 2 / (a1 - a2)

denom1 = (a1 + a2) ** 2
denom2 = (a1 - a2) ** 2

M = np.matrix([[-2 / denom1, -2 / denom1], [-2 / denom2, 2 / denom2]])
cov_b = M * covariance * M.T
cov_b
```

    matrix([[ 0.00088889,  0.01333333],
            [ 0.01333333,  0.328     ]])

---
### 2.2
Bestimmen Sie die Kovarianzmatrix der transformierten Parameter per Monte Carlo

---

```python
b1s = 2 / (a1s + a2s)
b2s = 2 / (a1s - a2s)

print('b1 = {}'.format(np.mean(b1s)))
print('var = {}'.format(np.var(b1s)))
print('b2 = {}'.format(np.mean(b2s)))
print('var = {}'.format(np.var(b2s)))

plt.hist(b1s, bins=100)
plt.show()
plt.hist(b2s, bins=100)
plt.show()
```

Dabei tritt das Problem auf, dass für einige Kombinationen von Werten für $a_1$ und $a_2$ der Nenner sehr nah an `0` kommt. Dadurch ergeben sich sehr große (unrealistische) Werte für $b_2$. Wir können dem entgegenwirken, indem wir einen Bereich festlegen in dem wir die Werte für $b_2$ erwarten.

```python
cut = np.logical_and(b2s < 5, b2s > 0)

b1s_ = b1s[cut]
b2s_ = b2s[cut]

print('b2 gefiltert = {}'.format(np.mean(b2s_)))
print('var = {}'.format(np.var(b2s_)))

plt.hist(b1s_, bins=100)
plt.show()

plt.hist2d(b1s_, b2s_, bins=100)
plt.show()
```

Wenn wir uns das Leben etwas erleichtern wollen, koennen wir auch einfach die Funktion `cov` aus Numpy verwenden, die uns fuer zwei Arrays direkt die Kovarianzmatrix ausrechnet

```python
ncov_b = np.cov(b1s_, b2s_)
ncov_b
```

    array([[ 0.0008353 ,  0.01402655],
           [ 0.01402655,  0.45535159]])

---
### 2.3
Bestimmen Sie analytisch die Unsicherheit von $y$ als Funktion von $x$:

#### 2.3.1
unter Verwendung der analytisch bestimmten Kovarianzmatrix von $(b_1, b_2)$

---

Zunächst berechnen wir die partiellen Ableitungen von $y$ nach den Koeffizienten $b_1$ und $b_2$.

\begin{equation}
    \frac{\partial y}{\partial b_1} = \frac{-x(1+x)}{b_1^2} \quad\text{und}{\quad} \frac{\partial y}{\partial b_2} = \frac{-x(1-x)}{b_2^2}
\end{equation}

Damit ergibt sich für die Varianz

\begin{align}
    \sigma_y^2 &= \sum_{ij}\frac{\partial y}{\partial b_i}\frac{\partial y}{\partial b_j}\mathrm{Cov}(b)_{ij} \\
               &= \frac{x^2(1+x)^2}{b_1^4}\sigma_{b_1}^2 + 2\frac{x^2 - x^3}{b_1^2b_2^2}\rho\sigma_{b_1}\sigma_{b_2} + \frac{x^2(1-x)^2}{b_2^4}\sigma_{b_2}^2 \\
               &= x^2\left[(\alpha+\beta)x^2 + (2\alpha - 2\beta - \gamma)x + (\alpha + \beta + \gamma)\right] \\
               &= x^2\left(c_2x^2 + c_1x + c_0\right)
\end{align}

mit den Koeffizienten

\begin{equation}
    \alpha = \frac{\sigma_{b_1}^2}{b_1^4} \quad,\quad
    \beta = \frac{\sigma_{b_2}^2}{b_2^4} \quad\text{und}\quad
    \gamma = \frac{\rho\sigma_{b_1}\sigma_{b_2}}{b_1^2b_2^2} \,.
\end{equation}

In Zahlen ausgedrückt sind die Koeffizienten

```python
s2_b1 = cov_b[0, 0]
s2_b2 = cov_b[1, 1]
rho12 = cov_b[0, 1]

alpha = s2_b1 / b1 ** 4
beta = s2_b2 / b2 ** 4
gamma = rho12 / b1 ** 2 / b2 ** 2

c2 = alpha + beta
c1 = 2 * c2 - gamma
c0 = c2 + gamma

c2, c1, c0
```

    (0.025000000000000005, 0.04250000000000001, 0.032500000000000008)

Es ist also

\begin{equation}
    \sigma_y^2 = x^2\left(0.025x^2 + 0.0425x + 0.0325\right)
\end{equation}

und damit

\begin{equation}
    \sigma_y = \lvert{}x\rvert\sqrt{0.025x^2 + 0.0425x + 0.0325}
\end{equation}

```python
def err_ana(x):
    return np.abs(x) * np.sqrt(c2 * x ** 2 + c1 * x + c0)

xs = np.linspace(-10, 10, 10000)
ys = 1 + xs * (xs + 1) / b1 + xs * (xs - 1) / b2
errs = err_ana(xs)

plt.plot(xs, ys)
plt.fill_between(xs, ys - errs, ys + errs, alpha=0.5)
plt.show()
```

---
#### 2.3.2
unter Verwendung der numerisch bestimmten Kovarianzmatrix von $(b_1, b_2)$

---

Die gleiche Rechnung mit den per Monte Carlo bestimmten Werten

```python
s2_b1 = ncov_b[0, 0]
s2_b2 = ncov_b[1, 1]
rho12 = ncov_b[0, 1]

alpha = s2_b1 / b1 ** 4
beta = s2_b2 / b2 ** 4
gamma = rho12 / b1 ** 2 / b2 ** 2

c2n = alpha + beta
c1n = 2 * c2 - gamma
c0n = c2 + gamma

c2n, c1n, c0n
```

    (0.032688177932642684, 0.042110063518531804, 0.032889936481468214)

liefert also ein etwas überschätzte Ungenauigkeit von

\begin{equation}
    \sigma_y = \lvert{}x\rvert\sqrt{0.0320x^2 + 0.0563x + 0.0398}
\end{equation}

```python
def err_num(x):
    return np.abs(x) * np.sqrt(c2n * x ** 2 + c1n * x + c0n)

errs = err_num(xs)

plt.plot(xs, ys)
plt.fill_between(xs, ys - errs, ys + errs, alpha=0.5)
plt.show()
```

---
## Aufgabe 3
Lösen Sie die obigen Teilaufgaben für $y = f(x)$ mit

$$y = \ln\left(1 + a_1x + a_2x^2\right) \quad \text{bzw.} \quad y = \ln\left(1 + \frac{x(1+x)}{b_1} + \frac{x(x-1)}{b_2}\right)$$

Im folgenden nennen wir das neue $y$ der Einfachheit halber $z$.

\begin{equation}
    z = \ln(1 + a_1x + a_2x^2) = \ln(y)
\end{equation}

Für die Unsicherheit von $z$ ergibt sich

\begin{align}
    \sigma_z &= \sqrt{\left(\frac{\partial z}{\partial y}\right)^2\sigma_y^2} \\
             &= \sqrt{\left(\frac{1}{y}\right)^2\sigma_y^2} \\
             &= \left\lvert\frac{\sigma_y}{y}\right\rvert \\
             &= \frac{0.2\lvert x\rvert\sqrt{1 + 0.25x^2 - 0.4x}}{\lvert 1 + a_1 x + a_2 x^2 \rvert} \,.
\end{align}

Völlig analog ist die Rechnung für die Reparametrisierung. Hier ergibt sich

\begin{align}
    \sigma_z &= \left\lvert\frac{\sigma_y}{y}\right\rvert \\
             &= \frac{\lvert{}x\rvert\sqrt{0.025x^2 + 0.0425x + 0.0325}}{\left\lvert 1 + \frac{x(1+x)}{b_1} + \frac{x(x-1)}{b_2}\right\rvert} \,.
\end{align}

```python
def err1(x):
    return 0.2 * np.abs(x) * np.sqrt(1 + 0.25 * x ** 2 - 0.4 * x) / np.abs(1)

def err2(x):
    return np.abs(x) * np.sqrt(c2 * x ** 2 + c1 * x + c0) / np.abs(1 + xs * (xs + 1) / b1 + xs * (xs - 1) / b2)

xs = np.linspace(-10, 10, 10000)
ys1 = 1 + a1 * xs + a2 * xs ** 2
ys2 = np.log(1 + xs * (xs + 1) / b1 + xs * (xs - 1) / b2)
errs1 = err1(xs)
errs2 = err2(xs)

plt.plot(xs, ys1)
plt.fill_between(xs, ys1 - errs1, ys1 + errs1, alpha=0.5)
plt.show()

plt.plot(xs, ys2)
plt.fill_between(xs, ys2 - errs2, ys2 + errs2, alpha=0.5)
plt.show()
```